Skip to content

Commit ee0b6cc

Browse files
BruceVonKMichelle Sintov
BruceVonK
authored and
Michelle Sintov
committed
VIC-11673 3d viewer cube not visible (#80)
* First pass of adding ability to connect to light cube * added comment, cube connection state looks better * Put cube connection on thread now shows text when connecting
1 parent bcd1ec0 commit ee0b6cc

3 files changed

Lines changed: 42 additions & 6 deletions

File tree

‎anki_vector/opengl/opengl_vector.py‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -587,8 +587,10 @@ class WorldRenderFrame(): # pylint: disable=too-few-public-methods
587587
defined in it's world class.
588588
"""
589589

590-
def __init__(self, robot):
590+
def __init__(self, robot, connecting_to_cube):
591591

592+
self.connected_cube = robot.world.connected_light_cube is not None
593+
self.connecting_to_cube = connecting_to_cube
592594
self.robot_frame = RobotRenderFrame(robot)
593595

594596
self.cube_frames: List[CubeRenderFrame] = []
@@ -608,3 +610,11 @@ def __init__(self, robot):
608610
is_fixed = isinstance(obj, FixedCustomObject)
609611
if is_custom or is_fixed:
610612
self.custom_object_frames.append(CustomObjectRenderFrame(obj, is_fixed))
613+
614+
def cube_connected(self):
615+
'''Is there a light cube connected to Vector'''
616+
return self.connected_cube
617+
618+
def cube_connecting(self):
619+
'''Is there a current attempt to connect to a light cube'''
620+
return self.connecting_to_cube

‎anki_vector/opengl/opengl_viewer.py‎

Lines changed: 15 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -94,11 +94,12 @@ class _RobotControlIntents(): # pylint: disable=too-few-public-methods
9494
"""
9595

9696
def __init__(self, left_wheel_speed=0.0, right_wheel_speed=0.0,
97-
lift_speed=0.0, head_speed=0.0):
97+
lift_speed=0.0, head_speed=0.0, connect_to_light_block=False):
9898
self.left_wheel_speed = left_wheel_speed
9999
self.right_wheel_speed = right_wheel_speed
100100
self.lift_speed = lift_speed
101101
self.head_speed = head_speed
102+
self.connect_to_light_block = connect_to_light_block
102103

103104

104105
def _draw_text(font, input_str, x, y, line_height=16, r=1.0, g=1.0, b=1.0):
@@ -369,8 +370,10 @@ def get_intent_direction(key1, key2):
369370
lift_speed = 4.0 * lift_dir * speed_scalar
370371
head_speed = head_dir * speed_scalar
371372

373+
connect_block = self._is_key_pressed.get(b'c', False)
374+
372375
control_intents = _RobotControlIntents(left_wheel_speed, right_wheel_speed,
373-
lift_speed, head_speed)
376+
lift_speed, head_speed, connect_block)
374377
self._input_intent_queue.put(control_intents, True)
375378

376379
def _idle(self):
@@ -488,6 +491,8 @@ def __init__(self,
488491
'R, F: Lift up, down',
489492
'T, G: Head up, down',
490493
'',
494+
'C: Connect to LightCube',
495+
'',
491496
'LMB: Rotate camera',
492497
'RMB: Move camera',
493498
'LMB + RMB: Move camera up/down',
@@ -587,15 +592,21 @@ def _render_world_frame(self, world_frame: opengl_vector.WorldRenderFrame):
587592
robot_view.display(robot_frame.pose, robot_frame.head_angle, robot_frame.lift_position)
588593

589594
if self.show_controls:
590-
self._draw_controls()
595+
self._draw_controls(world_frame.cube_connected(),world_frame.cube_connecting())
591596

592-
def _draw_controls(self):
597+
def _draw_controls(self, cube_connected, cube_connecting):
593598
try:
594599
GLUT_BITMAP_9_BY_15
595600
except NameError:
596601
pass
597602
else:
598603
_draw_text(GLUT_BITMAP_9_BY_15, self._instructions, x=10, y=10)
604+
if cube_connecting:
605+
_draw_text(GLUT_BITMAP_9_BY_15, "<connecting...>", x=600, y=10, r=0.75, g=0.5, b=0.0)
606+
elif cube_connected:
607+
_draw_text(GLUT_BITMAP_9_BY_15, "<cube connected>", x=600, y=10, r=0.0, g=0.85, b=0.0)
608+
else:
609+
_draw_text(GLUT_BITMAP_9_BY_15, "<no cube connected>", x=600, y=10, r=0.75, g=0.75, b=0.75)
599610

600611
def _render_3d_view(self, window: opengl.OpenGLWindow):
601612
"""Renders 3d objects to an openGL window

‎anki_vector/viewer.py‎

Lines changed: 16 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -243,6 +243,7 @@ def __init__(self, robot):
243243
self._process: mp.process.BaseProcess = None
244244
self._update_thread: threading.Thread = None
245245
self._last_robot_control_intents = None
246+
self.connecting_to_cube = False
246247

247248
def show(self, show_viewer_controls: bool = True):
248249
"""Spawns a background process that shows the navigation map in a 3D view.
@@ -353,6 +354,16 @@ def close(self):
353354
self._process.terminate()
354355
self._process = None
355356

357+
def connect_to_cube(self):
358+
'''Connect to light cube'''
359+
if self.connecting_to_cube:
360+
return
361+
362+
self.connecting_to_cube = True
363+
self.robot.world.connect_cube()
364+
self.connecting_to_cube = False
365+
return
366+
356367
def _update(self):
357368
"""Reads most recently stored user-triggered intents, and sends
358369
motor messages to the robot if the intents should effect the robot's
@@ -387,6 +398,10 @@ def _update(self):
387398

388399
if not old_intents or old_intents.head_speed != input_intents.head_speed:
389400
self.robot.motors.set_head_motor(input_intents.head_speed, _return_future=True)
401+
402+
if input_intents.connect_to_light_block and (old_intents is None or not old_intents.connect_to_light_block):
403+
threading.Thread(target=self.connect_to_cube).start()
404+
390405
except mp.queues.Empty:
391406
pass
392407
close_event = self._close_event
@@ -403,7 +418,7 @@ def _on_robot_state_update(self, *_):
403418
(main) process via a multiprocessing queue.
404419
"""
405420
from .opengl import opengl_vector
406-
world_frame = opengl_vector.WorldRenderFrame(self.robot)
421+
world_frame = opengl_vector.WorldRenderFrame(self.robot, self.connecting_to_cube)
407422
queue = self._world_frame_queue
408423
if queue:
409424
try:

0 commit comments

Comments
 (0)