def ScaleMarker(marker_template, control_scale=None, visual_scale=None): """Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker. @type marker_template: subclass of MarkerTemplate() @param marker_template: The template object containing InteractiveMarkers. @type control_scale: float @param control_scale: The scale factor for the InteractiveMarker. @type visual_scale: geometry_msgs/Vector3 @param visual_scale: The scale factor for the visualization Marker in the template. """ server = marker_template.server menu_handler = marker_template.menu_handler marker_name = marker_template.key if server: current_marker = server.get(marker_name) if current_marker: # rescale marker marker = Marker() marker = GetVisualMarker(current_marker) if visual_scale is not None: marker.scale = visual_scale # push marker into visual control visual = InteractiveMarkerControl() visual.name = "visual" visual.always_visible = GetVisualControl( current_marker).always_visible visual.interaction_mode = GetVisualControl( current_marker).interaction_mode visual.orientation = GetVisualControl(current_marker).orientation visual.markers.append(marker) new_marker = InteractiveMarker() new_marker.header.frame_id = current_marker.header.frame_id new_marker.name = current_marker.name new_marker.description = current_marker.description new_marker.pose = current_marker.pose new_marker.scale = current_marker.scale if control_scale is not None: new_marker.scale = control_scale new_marker.controls.append(visual) for control in current_marker.controls: if 'Translate' in control.name or 'Rotate' in control.name: # todo rename Plane Translate so we don't need to do this extra check if control.name not in [ 'TranslateXY', 'TranslateYZ', 'TranslateXZ' ]: new_marker.controls.append( CreateTransRotControl(control.name)) # insert the updated marker into the server server.insert(new_marker) menu_handler.apply(server, marker_name)
def ScaleMarker(marker_template, control_scale=None, visual_scale=None): """Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker. @type marker_template: subclass of MarkerTemplate() @param marker_template: The template object containing InteractiveMarkers. @type control_scale: float @param control_scale: The scale factor for the InteractiveMarker. @type visual_scale: geometry_msgs/Vector3 @param visual_scale: The scale factor for the visualization Marker in the template. """ server = marker_template.server menu_handler = marker_template.menu_handler marker_name = marker_template.key if server: current_marker = server.get(marker_name) if current_marker: # rescale marker marker = Marker() marker = GetVisualMarker(current_marker) if visual_scale is not None: marker.scale = visual_scale # push marker into visual control visual = InteractiveMarkerControl() visual.name = "visual" visual.always_visible = GetVisualControl(current_marker).always_visible visual.interaction_mode = GetVisualControl(current_marker).interaction_mode visual.orientation = GetVisualControl(current_marker).orientation visual.markers.append(marker) new_marker = InteractiveMarker() new_marker.header.frame_id = current_marker.header.frame_id new_marker.name = current_marker.name new_marker.description = current_marker.description new_marker.pose = current_marker.pose new_marker.scale = current_marker.scale if control_scale is not None: new_marker.scale = control_scale new_marker.controls.append(visual) for control in current_marker.controls: if 'Translate' in control.name or 'Rotate' in control.name: # todo rename Plane Translate so we don't need to do this extra check if control.name not in ['TranslateXY', 'TranslateYZ','TranslateXZ']: new_marker.controls.append(CreateTransRotControl(control.name)) # insert the updated marker into the server server.insert(new_marker) menu_handler.apply(server, marker_name)
def _get_surface_marker(pose, dimensions): ''' Function that generates a surface marker''' int_marker = InteractiveMarker() int_marker.name = 'surface' int_marker.header.frame_id = 'base_link' int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=2000, lifetime=rospy.Duration(2), scale=dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.8, 0.0, 0.4, 0.4), pose=pose) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1))) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker
def CreateInteractiveMarker(frame_id, name, scale): interactive_marker = InteractiveMarker() interactive_marker.header.frame_id = frame_id interactive_marker.name = name interactive_marker.description = name interactive_marker.scale = scale return interactive_marker
def makeHumanTagMarker(self, position, name): """ Make coordinates and control for tag :param: position of tag :param: name for tag :returns: """ int_marker = InteractiveMarker() int_marker.pose.position = position int_marker.scale = 1 int_marker.name = name int_marker.description = name self.makeBoxControlHumanTag(int_marker) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE int_marker.controls.append(copy.deepcopy(control)) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) server.insert(int_marker, self.processFeedback)
def create_interactive_marker(self, T): im = InteractiveMarker() im.header.frame_id = "world" im.name = "target" im.description = "Controller Target" im.scale = 0.2 im.pose.position = Point(*T[0:3, 3]) im.pose.orientation = Quaternion(*tf.quaternion_from_matrix(T)) self.process_marker_feedback(im) # set target to initial pose # Create a control to move a (sphere) marker around with the mouse control = InteractiveMarkerControl() control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.markers.extend( frame(numpy.identity(4), scale=0.1, frame_id='').markers) im.controls.append(control) # Create arrow controls to move the marker for dir in 'xyz': control = InteractiveMarkerControl() control.name = "move_" + dir control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation.x = 1 if dir == 'x' else 0 control.orientation.y = 1 if dir == 'y' else 0 control.orientation.z = 1 if dir == 'z' else 0 control.orientation.w = 1 im.controls.append(control) # Add the marker to the server and indicate that processMarkerFeedback should be called self.insert(im, self.process_marker_feedback) # Publish all changes self.applyChanges()
def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str( pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker' int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb)
def make_marker(self): int_marker = InteractiveMarker() int_marker.header.frame_id = "map" int_marker.pose = self.pose int_marker.scale = 1 int_marker.name = self.marker_name control = InteractiveMarkerControl() control.orientation.w = math.sqrt(2) / 2 control.orientation.x = 0 control.orientation.y = math.sqrt(2) / 2 control.orientation.z = 0 control.interaction_mode = self.interaction_mode int_marker.controls.append(copy.deepcopy(control)) # make a box which also moves in the plane markers = self.make_individual_markers(int_marker) for marker in markers: control.markers.append(marker) control.always_visible = True int_marker.controls.append(control) # we want to use our special callback function self.server.insert(int_marker, self.feedback)
def _get_object_marker(self, index, mesh=None): '''Generate a marker for world objects''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.2, 0.8, 0.0, 0.6), pose=World.objects[index].object.pose) if (mesh != None): object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = (World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06) button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) return int_marker
def _make_revolute_marker(self, revolute_joint: RevoluteJoint_): int_marker = InteractiveMarker() int_marker.header.frame_id = revolute_joint.child_body().body_frame().name() int_marker.name = revolute_joint.name() int_marker.scale = 0.3 int_marker.pose.position.x = 0. int_marker.pose.position.y = 0. int_marker.pose.position.z = 0. int_marker.pose.orientation.w = 1. int_marker.pose.orientation.x = 0. int_marker.pose.orientation.y = 0. int_marker.pose.orientation.z = 0. # Drake revolute axis is in frame F on parent axis_hat = revolute_joint.revolute_axis() self._joint_axis_in_child[revolute_joint.name()] = axis_hat # What rotation would get the parent X axis to align with the joint axis? rotation_matrix = ComputeBasisFromAxis(0, axis_hat) pydrake_quat = RotationMatrix(rotation_matrix).ToQuaternion() joint_control = InteractiveMarkerControl() joint_control.orientation.w = pydrake_quat.w() joint_control.orientation.x = pydrake_quat.x() joint_control.orientation.y = pydrake_quat.y() joint_control.orientation.z = pydrake_quat.z() joint_control.always_visible = True joint_control.name = f'rotate_axis_{revolute_joint.name()}' joint_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(joint_control) return int_marker
def MakeMuneObject(self, MenuName, MenuPose): MenuInteractiveMarker = InteractiveMarker() MenuInteractiveMarker.name = MenuName MenuInteractiveMarker.header.frame_id = self.frame_id MenuInteractiveMarker.pose.position.z += self.MenuHight MenuInteractiveMarker.scale = self.MenuScale MenuControl = InteractiveMarkerControl() MenuControl.interaction_mode = InteractiveMarkerControl.MENU MenuControl.always_visible = False MenuMarker = Marker() MenuMarker.type = Marker.ARROW MenuMarker.scale.x = MenuInteractiveMarker.scale * 2 MenuMarker.scale.y = MenuInteractiveMarker.scale * 0.45 MenuMarker.scale.z = MenuInteractiveMarker.scale * 0.45 MenuMarker.color.r = 0.5 MenuMarker.color.g = 0.5 MenuMarker.color.b = 0.5 MenuMarker.color.a = 1.0 MenuMarker.pose = MenuPose MenuControl.markers.append(MenuMarker) MenuInteractiveMarker.controls.append(MenuControl) #print '###################MenuInteractiveMarker info:\n', MenuInteractiveMarker self.server.insert(MenuInteractiveMarker) rospy.loginfo('insert Menu Marker Object')
def start(self): # create blue cube marker obj_marker = Marker() obj_marker.type = Marker.CUBE obj_marker.pose.orientation.w = 1 obj_marker.scale.x = 0.06 obj_marker.scale.y = 0.06 obj_marker.scale.z = 0.06 obj_marker.color.r = 0.09 obj_marker.color.g = 0.97 obj_marker.color.b = 2.39 obj_marker.color.a = 1.0 obj_control = InteractiveMarkerControl() obj_control.interaction_mode = InteractiveMarkerControl.MENU obj_control.always_visible = True obj_control.markers.append(obj_marker) obj_im = InteractiveMarker() obj_im.name = 'object' obj_im.header.frame_id = 'base_link' obj_im.pose.position.x = 0.7 obj_im.pose.position.y = 0 obj_im.pose.position.z = 0.5 obj_im.pose.orientation.w = 1 obj_im.scale = 0.3 obj_im.menu_entries.append(menu_entry(1, 'Pick from front')) obj_im.menu_entries.append(menu_entry(2, 'Open gripper')) obj_im.controls.append(obj_control) obj_im.controls.extend(six_dof_controls()) self._im_server.insert(obj_im, feedback_cb=self.handle_feedback) self._im_server.applyChanges() self.update_gripper()
def make_def(p, color=(0.5, 0.5, 0.5), desc=None): im = InteractiveMarker() im.header.frame_id = p.pose.header.frame_id im.pose = p.pose.pose im.name = p.name if desc is None: im.description = p.name im.scale = 1.2 * max(p.bbox.dimensions) marker = Marker() marker.type = Marker.CUBE marker.scale.x = p.bbox.dimensions[0] marker.scale.y = p.bbox.dimensions[1] marker.scale.z = p.bbox.dimensions[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = 1.0 control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = InteractiveMarkerControl.BUTTON control.markers.append(marker) im.controls.append(control) return im
def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker' int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb)
def makeMarker(self, callback=None, marker=None, pose=[0, 0, 0], controls=[], fixed=False, name=None, frame="map", description="", imode=0, rot=[0, 0, 0, 1]): if marker is None: marker = self.mg.marker() if callback is None: callback = default_callback if name is None: name = "control%d" % self.c self.c += 1 int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.pose.position.x = pose[0] int_marker.pose.position.y = pose[1] int_marker.pose.position.z = pose[2] int_marker.pose.orientation.x = rot[0] int_marker.pose.orientation.y = rot[1] int_marker.pose.orientation.z = rot[2] int_marker.pose.orientation.w = rot[3] int_marker.scale = 1 int_marker.name = name int_marker.description = description control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = imode control.markers.append(marker) int_marker.controls.append(control) for control_name in controls: data = TYPEDATA[control_name] control = InteractiveMarkerControl() control.orientation.w = data[0] / SQRT2 control.orientation.x = data[1] / SQRT2 control.orientation.y = data[2] / SQRT2 control.orientation.z = data[3] / SQRT2 control.name = control_name control.interaction_mode = data[4] if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.server.insert(int_marker, callback) self.markers[name] = int_marker self.server.applyChanges()
def _get_object_marker(self, index, mesh=None): """Generate and return a marker for world objects. Args: index (int): ID for the new marker. mesh (Mesh, optional): Mesh to use for the marker. Only utilized if not None. Defaults to None. Returns: InteractiveMarker """ int_marker = InteractiveMarker() int_marker.name = self._objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = self._objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=index, lifetime=MARKER_DURATION, scale=self._objects[index].object.dimensions, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ, pose=self._objects[index].object.pose) self.create_sides(index) if mesh is not None: object_marker = _get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) else: for item in range((6 * (index)), (6 * (index)) + 6): rospy.loginfo("Item: " + str(item) + "\nlen mark cont: " + str(len(self._marker_controllers))) int_marker.controls.append(self._marker_controllers[item]) text_pos = Point() text_pos.x = self._objects[index].object.pose.position.x text_pos.y = self._objects[index].object.pose.position.y text_pos.z = (self._objects[index].object.pose.position.z + self._objects[index].object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z) button_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=SCALE_TEXT, text=int_marker.name, color=COLOR_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) #rospy.loginfo("Int_marker controls: " + str(int_marker.controls)) return int_marker
def _get_object_marker(self, index, mesh=None): '''Generate and return a marker for world objects. Args: index (int): ID for the new marker. mesh (Mesh, optional): Mesh to use for the marker. Only utilized if not None. Defaults to None. Returns: InteractiveMarker ''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=index, lifetime=MARKER_DURATION, scale=World.objects[index].object.dimensions, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ, pose=World.objects[index].object.pose ) if mesh is not None: object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = ( World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z) button_control.markers.append( Marker( type=Marker.TEXT_VIEW_FACING, id=index, scale=SCALE_TEXT, text=int_marker.name, color=COLOR_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)) ) ) int_marker.controls.append(button_control) return int_marker
def _add_poi(self, name, position): ''' Internal implementation of add_poi, which is NOT thread safe and does NOT update clients of change ''' if self.interactive_marker_server.get(name) is not None: return False poi = POI() poi.name = name poi.position = position self.pois.pois.append(poi) point_marker = Marker() point_marker.type = Marker.SPHERE point_marker.scale.x = self.marker_scale point_marker.scale.y = self.marker_scale point_marker.scale.z = self.marker_scale point_marker.color.r = 1.0 point_marker.color.g = 1.0 point_marker.color.b = 1.0 point_marker.color.a = 1.0 text_marker = Marker() text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.orientation.w = 1.0 text_marker.pose.position.x = 1.5 text_marker.text = poi.name text_marker.scale.z = 1.0 text_marker.color.r = 1.0 text_marker.color.g = 1.0 text_marker.color.b = 1.0 text_marker.color.a = 1.0 int_marker = InteractiveMarker() int_marker.header.frame_id = self.global_frame int_marker.pose.orientation.w = 1.0 int_marker.pose.position = poi.position int_marker.scale = 1 int_marker.name = poi.name # insert a box control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.always_visible = True control.markers.append(point_marker) control.markers.append(text_marker) int_marker.controls.append(control) self.interactive_marker_server.insert(int_marker, self.process_feedback) return True
def __init__(self, arm, gripper, im_server): self.arm = arm self.gripper = gripper self.currentPose = None self.currentPoseFeasible = False self._im_server = im_server gripper_im = InteractiveMarker() gripper_im.name = "da grip" #gripper_im.header.frame_id = "wrist_roll_link" gripper_im.header.frame_id = "base_link" gripper_im.scale = .5 gripper_im.pose.orientation.w = 1.0 markers = init_markers(0.166, 0, 0) # create gripper mesh # Adds the gripper markers to the interactive markers marker_control = InteractiveMarkerControl() for marker in markers: marker_control.markers.append(marker) marker_control.always_visible = True marker_control.interaction_mode = InteractiveMarkerControl.MENU gripper_im.controls.append(marker_control) # Add the 6 degree of freedom controls add_6dof_controls(gripper_im) # Add the menu commands entry1 = MenuEntry() entry1.id = 1 entry1.title = 'open' entry1.command_type = entry1.FEEDBACK gripper_im.menu_entries.append(entry1) entry2 = MenuEntry() entry2.id = 2 entry2.title = 'close' entry2.command_type = entry2.FEEDBACK gripper_im.menu_entries.append(entry2) entry3 = MenuEntry() entry3.id = 3 entry3.title = 'move_gripper' entry3.command_type = entry3.FEEDBACK gripper_im.menu_entries.append(entry3) self._im_server.insert(gripper_im, feedback_cb = self.handle_feedback) self._im_server.applyChanges()
def createInteractiveMarker(name, x=0, y=0, z=0, ox=0, oy=0, oz=0, ow=1, frame_id = "/map"): int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id int_marker.name = name int_marker.scale = 0.3 int_marker.description = name int_marker.pose.position.x = x int_marker.pose.position.y = y int_marker.pose.position.z = z int_marker.pose.orientation.x = ox int_marker.pose.orientation.y = oy int_marker.pose.orientation.z = oz int_marker.pose.orientation.w = ow return int_marker
def add_marker(self, init_position, callback, controls=[], scale=1., marker=None, name=None, fixed_orientation=False): if name is None: name = 'control%d' % (self.marker_ctr) self.marker_ctr += 1 if marker is None: marker = make_markers.make_marker() marker.header.frame_id = self.frame_id marker.header.frame_id = self.frame_id int_marker = InteractiveMarker() int_marker.header.frame_id = self.frame_id int_marker.pose.position = init_position int_marker.scale = scale int_marker.name = name vm_control = InteractiveMarkerControl() vm_control.always_visible = True vm_control.markers.append(marker) int_marker.controls.append(vm_control) #int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D for control_name in controls: data = TYPEDATA[control_name] control = InteractiveMarkerControl() control.orientation.w = data[0] control.orientation.x = data[1] control.orientation.y = data[2] control.orientation.z = data[3] control.name = control_name control.interaction_mode = data[4] if fixed_orientation: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.server.insert(int_marker, callback) self.markers[name] = int_marker
def create_6dof_marker(self): imarker = InteractiveMarker() imarker.header.frame_id = self.frame_id imarker.pose.orientation.w = 1 imarker.name = "tool_calib" imarker.name = "Tool Calibration" imarker.scale = 0.2 control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS imarker.controls.append(control) return imarker
def update(self): '''Updates marker/arm loop''' self._menu_handler = MenuHandler() # Inset main menu entries. self._menu_handler.insert('Move gripper here', callback=self._move_to_cb) self._menu_handler.insert('Move marker to current gripper pose', callback=self._move_pose_to_cb) if self._is_hand_open(): self._menu_handler.insert('Close gripper', callback=self._close_gripper_cb) else: self._menu_handler.insert('Open gripper', callback=self._open_gripper_cb) frame_id = REF_FRAME pose = self.get_pose() # if self._marker_moved() or self._menu_control is None: rospy.loginfo("Marker moved") menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) self._menu_control = menu_control # Make and add interactive marker. int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose.pose int_marker.scale = INT_MARKER_SCALE self._add_6dof_marker(int_marker, True) int_marker.controls.append(self._menu_control) ArmControlMarker._im_server.insert(int_marker, self._marker_feedback_cb) self._menu_handler.apply(ArmControlMarker._im_server, self._get_name()) ArmControlMarker._im_server.applyChanges()
def _get_surface_marker(pose, dimensions): '''Returns a surface marker with provided pose and dimensions. Args: pose (Pose) dimensions (Vector3) Returns: InteractiveMarker ''' int_marker = InteractiveMarker() int_marker.name = 'surface' int_marker.header.frame_id = BASE_LINK int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=2000, lifetime=MARKER_DURATION, scale=dimensions, header=Header(frame_id=BASE_LINK), color=COLOR_SURFACE, pose=pose ) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker( type=Marker.TEXT_VIEW_FACING, id=2001, scale=SCALE_TEXT, text=int_marker.name, color=COLOR_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)) ) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker
def interactive_gripper_marker(pose_stamped, finger_distance): # gripper marker gripper_marker = Marker() gripper_marker.pose.position.x = 0.166 # wrist_roll_link/gripper_link Translation: [0.166, 0.000, 0.000] gripper_marker.pose.orientation.w = 1 # wrist_roll_link/gripper_link Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000] gripper_marker.type = Marker.MESH_RESOURCE gripper_marker.mesh_resource = GRIPPER_MESH gripper_marker.mesh_use_embedded_materials = True finger_distance = max(CLOSE_FINGER_POS, min(finger_distance, OPEN_FINGER_POS)) # left finger marker l_marker = Marker() l_marker.pose.position.x = 0.166 l_marker.pose.position.y = -finger_distance / 2.0 l_marker.pose.orientation.w = 1 l_marker.type = Marker.MESH_RESOURCE l_marker.mesh_resource = L_FINGER_MESH l_marker.mesh_use_embedded_materials = True # right finger marker r_marker = Marker() r_marker.pose.position.x = 0.166 r_marker.pose.position.y = finger_distance / 2.0 r_marker.pose.orientation.w = 1 r_marker.type = Marker.MESH_RESOURCE r_marker.mesh_resource = R_FINGER_MESH r_marker.mesh_use_embedded_materials = True control = InteractiveMarkerControl() control.orientation.w = 1 control.interaction_mode = InteractiveMarkerControl.NONE control.always_visible = True control.markers.append(gripper_marker) control.markers.append(l_marker) control.markers.append(r_marker) interactive_marker = InteractiveMarker() interactive_marker.header = pose_stamped.header interactive_marker.pose = pose_stamped.pose interactive_marker.controls.append(control) interactive_marker.scale = 0.3 return interactive_marker
def _make_revolute_marker(self, revolute_joint: RevoluteJoint_): int_marker = InteractiveMarker() int_marker.header.frame_id = revolute_joint.child_body().body_frame().name() int_marker.name = revolute_joint.name() int_marker.scale = 0.3 int_marker.pose.position.x = 0. int_marker.pose.position.y = 0. int_marker.pose.position.z = 0. int_marker.pose.orientation.w = 1. int_marker.pose.orientation.x = 0. int_marker.pose.orientation.y = 0. int_marker.pose.orientation.z = 0. # Drake revolute axis is in frame F on parent axis_hat = revolute_joint.revolute_axis() self._joint_axis_in_child[revolute_joint.name()] = axis_hat # What rotation would get the parent X axis to align with the joint axis? # https://math.stackexchange.com/q/476311 x_axis = (1, 0, 0) v = numpy.cross(x_axis, axis_hat) c = numpy.dot(x_axis, axis_hat) v_sub_x = numpy.array(( (0, -v[2], v[1]), (v[2], 0, -v[0]), (-v[1], v[0], 0)), dtype=numpy.float64) v_sub_x_squared = numpy.dot(v_sub_x, v_sub_x) rotation_matrix = numpy.eye(3) + v_sub_x + v_sub_x_squared * (1.0 / (1.0 + c)) pydrake_quat = RotationMatrix(rotation_matrix).ToQuaternion() joint_control = InteractiveMarkerControl() joint_control.orientation.w = pydrake_quat.w() joint_control.orientation.x = pydrake_quat.x() joint_control.orientation.y = pydrake_quat.y() joint_control.orientation.z = pydrake_quat.z() joint_control.always_visible = True joint_control.name = f'rotate_axis_{revolute_joint.name()}' joint_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(joint_control) return int_marker
def __init__(self, server, x, y, name, driver): # ... Initialization, marker creation, etc. ... self._driver = driver int_marker = InteractiveMarker() int_marker.header.frame_id = "odom" int_marker.name = name int_marker.description = name int_marker.pose.position.x = x int_marker.pose.position.y = y int_marker.pose.orientation.w = 1 int_marker.scale = 1.1 # print name arrow_marker = Marker() arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 1 arrow_marker.scale.x = 0.7 arrow_marker.scale.y = 0.07 arrow_marker.scale.z = 0.07 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(copy.deepcopy(control)) control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE control.always_visible = True control.markers.append(arrow_marker) int_marker.controls.append(control) #int_marker_control.markers.append(arrow_marker) self._server = server self._server.insert(int_marker, self._callback) self._server.applyChanges()
def build_landmark_marker(landmark): """Generate and return a marker for world landmarks. Args: landmark (WorldLandmark): The landmark to generate a marker for. Returns: InteractiveMarker """ int_marker = InteractiveMarker() int_marker.name = landmark.name() int_marker.header.frame_id = BASE_LINK int_marker.pose = landmark.object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, lifetime=MARKER_DURATION, scale=landmark.object.dimensions, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ, pose=landmark.object.pose) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = landmark.object.pose.position.x text_pos.y = landmark.object.pose.position.y text_pos.z = (landmark.object.pose.position.z + landmark.object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z) button_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, scale=SCALE_TEXT, text=int_marker.name, color=COLOR_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) return int_marker
def gripper_interactive_marker(pose_stamped, finger_distance): gripper_marker = Marker() gripper_marker.pose.position.x = 0.166 gripper_marker.pose.orientation.w = 1 gripper_marker.type = Marker.MESH_RESOURCE gripper_marker.mesh_resource = GRIPPER_MESH gripper_marker.mesh_use_embedded_materials = True finger_distance = max(0, min(finger_distance, 0.1)) l_marker = Marker() l_marker.pose.position.x = 0.166 l_marker.pose.position.y = finger_distance / 2.0 - 0.1 l_marker.pose.orientation.w = 1 l_marker.type = Marker.MESH_RESOURCE l_marker.mesh_resource = L_FINGER_MESH l_marker.mesh_use_embedded_materials = True r_marker = Marker() r_marker.pose.position.x = 0.166 r_marker.pose.position.y = -finger_distance / 2.0 + 0.1 r_marker.pose.orientation.w = 1 r_marker.type = Marker.MESH_RESOURCE r_marker.mesh_resource = R_FINGER_MESH r_marker.mesh_use_embedded_materials = True control = InteractiveMarkerControl() control.orientation.w = 1 control.interaction_mode = InteractiveMarkerControl.NONE control.always_visible = True control.markers.append(gripper_marker) control.markers.append(l_marker) control.markers.append(r_marker) interactive_marker = InteractiveMarker() interactive_marker.header = pose_stamped.header interactive_marker.pose = pose_stamped.pose interactive_marker.controls.append(control) interactive_marker.scale = 0.25 return interactive_marker
def make_waypoint_marker(id, x, y): int_marker = InteractiveMarker() int_marker.header.frame_id = 'path' int_marker.pose.position = Point(x, y, 0) int_marker.scale = 0.1 int_marker.name = 'waypoint_' + str(id) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE int_marker.controls.append(copy.deepcopy(control)) control.markers.append( make_sphere(int_marker) ) control.always_visible = True int_marker.controls.append(control) return int_marker
def start(self): obj_marker = Marker() obj_marker.type = Marker.CUBE obj_marker.pose.orientation.w = 1 obj_marker.scale.x = 0.05 obj_marker.scale.y = 0.05 obj_marker.scale.z = 0.05 obj_marker.color.r = 1 obj_marker.color.g = 1 obj_marker.color.a = 0.7 obj_control = InteractiveMarkerControl() obj_control.interaction_mode = InteractiveMarkerControl.MENU obj_control.always_visible = True obj_control.markers.append(obj_marker) menu_entry1 = MenuEntry() menu_entry1.id = 1 menu_entry1.title = 'Pick from front' menu_entry1.command_type = MenuEntry.FEEDBACK menu_entry2 = MenuEntry() menu_entry2.id = 2 menu_entry2.title = 'Open gripper' menu_entry2.command_type = MenuEntry.FEEDBACK obj_im = InteractiveMarker() obj_im.name = 'object' obj_im.header.frame_id = 'base_link' obj_im.pose.position.x = 0.7 obj_im.pose.position.z = 0.5 obj_im.pose.orientation.w = 1 obj_im.scale = 0.25 obj_im.menu_entries.append(menu_entry1) obj_im.menu_entries.append(menu_entry2) obj_im.controls.append(obj_control) obj_im.controls.extend(make_6dof_controls()) self._im_server.insert(obj_im, feedback_cb=self.handle_feedback) self._im_server.applyChanges() self._update_grippers()
def _get_object_marker(self, index, mesh=None): '''Generate a marker for world objects''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.2, 0.8, 0.0, 0.6), pose=World.objects[index].object.pose) if (mesh != None): object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = (World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06) button_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) return int_marker
def makeStaticMarker(self, position): int_marker = InteractiveMarker() int_marker.header.frame_id = "odom" int_marker.pose.position = position int_marker.scale = 1 int_marker.name = "movement_controller" int_marker.description = "Click to make Kuri move" box_marker = self.makeBox(int_marker) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.always_visible = True control.interaction_mode = InteractiveMarkerControl.BUTTON control.markers.append(box_marker) int_marker.controls.append(copy.deepcopy(control)) self._server.insert(int_marker, self.handleInput) self._server.applyChanges()
def makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.from_frame int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame # insert a box, well, an arrow self.makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.menu_handler.insert("Publish transform", callback=self.processFeedback) self.menu_handler.insert("Stop publishing transform", callback=self.processFeedback) self.server.insert(int_marker, self.processFeedback) self.menu_handler.apply(self.server, int_marker.name)
def _update_viz_core(self): '''Updates visualization after a change''' menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = self._get_ref_name() pose = self.get_pose() if (self.action_step.type == ActionStep.ARM_TARGET): menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.02, 0.02), header=Header(frame_id=frame_id), color=ColorRGBA(0.8, 0.4, 0.0, 0.8), points=point_list) menu_control.markers.append(main_marker) menu_control.markers.append(ActionStepMarker.make_sphere_marker( self.get_uid() + 2000, self._get_traj_pose(0), frame_id, 0.05)) last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append(ActionStepMarker.make_sphere_marker( self.get_uid() + 3000, self._get_traj_pose(last_index), frame_id, 0.05)) else: rospy.logerr('Non-handled action step type ' + str(self.action_step.type)) ref_frame = World.get_ref_from_name(frame_id) if (ref_frame == ArmState.OBJECT): menu_control.markers.append(Marker(type=Marker.ARROW, id=(1000 + self.get_uid()), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.03, 0.04), header=Header(frame_id=frame_id), color=ColorRGBA(1.0, 0.8, 0.2, 0.5), points=[pose.position, Point(0, 0, 0)])) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=Vector3(0, 0, 0.03), text='Step' + str(self.step_number), color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)
def add_6DOF(self, init_position = Point( 0.0, 0.0, 0.0), frame_id = 'map'): marker = InteractiveMarker() marker.header.frame_id = frame_id marker.pose.position = init_position marker.scale = 0.3 marker.name = 'camera_marker' marker.description = 'Camera 6-DOF pose control' # X axis rotation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS marker.controls.append(control) # X axis traslation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS marker.controls.append(control) # Y axis rotation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS marker.controls.append(control) # Y axis traslation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS marker.controls.append(control) # Z axis rotation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS marker.controls.append(control) # Z axis traslation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS marker.controls.append(control) # Add marker to server self.server.insert(marker, self.marker_feedback) self.server.applyChanges()
def _update_viz_core(self): '''Updates visualization after a change''' menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = self._get_ref_name() pose = self.get_pose() if (self.action_step.type == ArmStepType.ARM_TARGET): menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY): point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.02, 0.02), header=Header(frame_id=frame_id), color=ColorRGBA(0.8, 0.4, 0.0, 0.8), points=point_list) menu_control.markers.append(main_marker) menu_control.markers.append(ArmStepMarker.make_sphere_marker( self.get_uid() + 2000, self._get_traj_pose(0), frame_id, 0.05)) last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append(ArmStepMarker.make_sphere_marker( self.get_uid() + 3000, self._get_traj_pose(last_index), frame_id, 0.05)) else: rospy.logerr('Non-handled action step type ' + str(self.action_step.type)) ref_frame = World.get_world().get_ref_from_name(frame_id) if (ref_frame == ArmState.OBJECT): # The following is needed to properly display the arrtow in browser, due to the fact that ros3djs # displays all nested markers in the reference frame of the interactive marker. # Thus, we need to calculate the position of the object in the reference frame of the interactive marker. quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] inv_quat_matrix = quaternion_matrix(quaternion_inverse(quat)) pose_vec = numpy.array((pose.position.x, pose.position.y, pose.position.z, 0)) new_pose = numpy.dot(inv_quat_matrix, pose_vec) menu_control.markers.append(Marker(type=Marker.ARROW, id=(1000 + self.get_uid()), lifetime=rospy.Duration(2), scale=Vector3(0.01, 0.01, 0.0001), header=Header(frame_id=frame_id), color=ColorRGBA(0.8, 0.8, 0.0, 0.6), points=[Point(0, 0, 0), Point(-new_pose[0], -new_pose[1], -new_pose[2])])) # Calculate text position so that they "orbit" around the marker; # this is done so that poses in identical or similar positions # have non-overlapping text. Note that to do this without moving # the text around as the camera is moved, we assume that the viewer # is always looking directly at the robot, so we assume the x dimension # is constant and "orbin" in the y-z plane. n_orbitals = 8 # this should be a constant offset = 0.15 # this should be a constant orbital = (self.step_number - 1) % n_orbitals # - 1 to make 0-based angle_rad = (float(orbital) / n_orbitals) * (-2 * numpy.pi) + \ (numpy.pi / 2.0) # start above, at pi/2 (90 degrees) text_pos = Point() text_pos.x = 0 text_pos.y = numpy.cos(angle_rad) * offset text_pos.z = numpy.sin(angle_rad) * offset r,g,b = self.get_marker_color() menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=Vector3(0.06, 0.06, 0.06), lifetime=rospy.Duration(1.5), text='Step ' + str(self.step_number), color=ColorRGBA(r, g, b, 1.0), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) self.parent_step_sequence.im_server.insert(int_marker, self.marker_feedback_cb)
def make6DofMarker(frame_id, fixed=False, description="Simple 6-DOF Control"): int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id int_marker.scale = 1 int_marker.name = "simple_6dof" int_marker.description = description # insert a box makeBoxControl(int_marker) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def make_marker_flexible(fixed, ps, scale, color, mtype, ignore_rotation, ignore_x=False, ignore_y=False, ignore_z=False): int_marker = InteractiveMarker() int_marker.header.frame_id = ps.header.frame_id int_marker.pose = ps.pose int_marker.scale = scale int_marker.name = 'simple_6dof' int_marker.description = '' # insert a marker control = InteractiveMarkerControl() control.always_visible = True control.markers.append(make_marker(scale, color, mtype)) int_marker.controls.append(control) if fixed: int_marker.name += '_fixed' int_marker.description += '\n(fixed orientation)' if not ignore_x: control = InteractiveMarkerControl() control.orientation = Quaternion(1,0,0,1) control.name = 'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_y: control = InteractiveMarkerControl() control.orientation = Quaternion(0,0,1,1) control.name = 'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_z: control = InteractiveMarkerControl() control.orientation = Quaternion(0,1,0,1) control.name = 'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_rotation: control = InteractiveMarkerControl() control.orientation = Quaternion(1,0,0,1) control.name = 'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0,0,1,1) control.name = 'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0,1,0,1) control.name = 'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def make_6dof_gripper(fixed, ps, scale, color, robot_type = "pr2", ignore_rotation = False, ignore_x=False, ignore_y=False, ignore_z=False): int_marker = InteractiveMarker() int_marker.header.frame_id = ps.header.frame_id int_marker.pose = ps.pose int_marker.scale = scale int_marker.name = 'gripper_6dof' control = InteractiveMarkerControl() control.always_visible = True control.name = 'pr2_gripper_control' if robot_type == "pr2": control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7], control=control) int_marker.description = 'pr2_gripper_control' elif robot_type == "cody": control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control) int_marker.description = 'cody_ee_control' elif robot_type == "darci": control = make_darci_ee_marker(ps, [1, 1, 1, 0.4], control=control) int_marker.description = 'darci_ee_control' int_marker.controls.append( control ) if not ignore_x: control = InteractiveMarkerControl() control.orientation = Quaternion(1,0,0,1) control.name = 'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_y: control = InteractiveMarkerControl() control.orientation = Quaternion(0,0,1,1) control.name = 'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_z: control = InteractiveMarkerControl() control.orientation = Quaternion(0,1,0,1) control.name = 'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_rotation: control = InteractiveMarkerControl() control.orientation = Quaternion(1,0,0,1) control.name = 'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0,0,1,1) control.name = 'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0,1,0,1) control.name = 'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def setup_marker(self, frame="velodyne", name = "capture vehicle", translation=True): int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.name = name int_marker.description = name int_marker.scale = 3 marker_control = InteractiveMarkerControl() marker_control.always_visible = True marker_control.markers.append(self.marker) int_marker.controls.append(marker_control) control = InteractiveMarkerControl() control.name = "rotate_x" control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "rotate_z" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "rotate_y" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) if not translation : #int_marker.pose.position = Point(0,0,0) return int_marker control = InteractiveMarkerControl() control.name = "move_x" control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "move_z" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "move_y" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) return int_marker