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_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 _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_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 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 getMarkersFromPose(poseStamped): int_marker = InteractiveMarker() int_marker.header.frame_id = poseStamped.header.frame_id int_marker.pose = poseStamped.pose int_control = InteractiveMarkerControl() int_control.markers = createMarker() int_marker.controls.append(int_control) return int_marker
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_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 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 _make_marker(self, name, pose): """Creates a new interactive marker. Args: name: The name of the marker. pose: The geometry_msgs/Pose of the marker. """ int_marker = InteractiveMarker() int_marker.header.frame_id = 'map' int_marker.name = name int_marker.description = name int_marker.pose = pose arrow_marker = Marker() arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 1 arrow_marker.scale.x = 0.45 arrow_marker.scale.y = 0.05 arrow_marker.scale.z = 0.05 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 text_marker = Marker() text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.orientation.w = 1 text_marker.pose.position.z = 1.5 text_marker.scale.x = 0.2 text_marker.scale.y = 0.2 text_marker.scale.z = 0.2 text_marker.color.r = 0.5 text_marker.color.g = 0.5 text_marker.color.b = 0.5 text_marker.color.a = 1 text_marker.text = name arrow_control = InteractiveMarkerControl() arrow_control.orientation.w = 1 arrow_control.orientation.y = 1 arrow_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE arrow_control.markers.append(arrow_marker) arrow_control.markers.append(text_marker) arrow_control.always_visible = True int_marker.controls.append(arrow_control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.y = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) return int_marker
def _marker_style(self, name, pose): int_marker = InteractiveMarker() int_marker.header.frame_id = 'map' int_marker.name = name int_marker.description = name int_marker.pose = pose arrow_marker = Marker() arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 1 arrow_marker.scale.x = 0.5 arrow_marker.scale.y = 0.07 arrow_marker.scale.z = 0.05 arrow_marker.color.r = 0.5 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 # text_marker = Marker() # text_marker.type = Marker.TEXT_VIEW_FACING # text_marker.pose.orientation.w = 1 # text_marker.pose.position.z = 1.5 # text_marker.scale.x = 0.2 # text_marker.scale.y = 0.2 # text_marker.scale.z = 0.2 # text_marker.color.r = 0.5 # text_marker.color.g = 0.5 # text_marker.color.b = 0.5 # text_marker.color.a = 1 # text_marker.text = name arrow_control = InteractiveMarkerControl() arrow_control.orientation.w = 1 arrow_control.orientation.y = 1 arrow_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE arrow_control.markers.append(arrow_marker) # arrow_control.markers.append(text_marker) arrow_control.always_visible = True int_marker.controls.append(arrow_control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.y = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) return int_marker
def create_im(self, marker, pose, name): # create the new interactive marker int_marker = InteractiveMarker() int_marker.pose = copy.deepcopy(pose) int_marker.header.frame_id = 'base_link' int_marker.name = name # move freely on the X-Y plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE control.markers.append(marker) control.always_visible = True int_marker.controls.append(control) return int_marker
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 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 __init__(self, server, name, pose_set): # ... Initialization, marker creation, etc. ... self.pose = pose_set if pose_set == None: self.pose = geometry_msgs.msg.Pose() int_marker = InteractiveMarker() int_marker.header.frame_id = "odom" int_marker.name = name int_marker.description = name int_marker.pose = self.pose 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 arrow_marker.pose.position.z = 0.1 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 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 _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 __init__(self, server): self.marker_name = "IMU" self.imu_publisher = self.create_publisher(Imu, "/imu/data", 1) self.server = server self.pose = Pose() self.pose.orientation.w = 1 int_marker = InteractiveMarker() int_marker.header.frame_id = "imu_frame" int_marker.pose = self.pose int_marker.scale = 1 int_marker.name = self.marker_name int_marker.description = "Rotate 2DOF to simulate IMU orientation to ground" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 normalize_quaternion(control.orientation) control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.always_visible = True int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 normalize_quaternion(control.orientation) control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # we want to use our special callback function self.server.insert(int_marker, self.process_feedback)
def __create_int_marker__(self, name, pose): print("creating int marker: " + name) int_marker = InteractiveMarker() int_marker.header.frame_id = "map" int_marker.name = name int_marker.description = name int_marker.pose = pose # Move it 0.25 meters up to make it easier to click int_marker.pose.position.z = 0.25 text_marker = Marker() text_marker.text = name text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.position.z = 2 text_marker.scale.x = 0.4 text_marker.scale.y = 0.4 text_marker.scale.z = 0.4 text_marker.color.r = 0.0 text_marker.color.g = 0.5 text_marker.color.b = 0.5 text_marker.color.a = 1.0 text_control = InteractiveMarkerControl() text_control.name = "text_control" text_control.markers.append(text_marker) text_control.always_visible = True text_control.interaction_mode = InteractiveMarkerControl.NONE int_marker.controls.append(text_control) rotation_ring_control = InteractiveMarkerControl() rotation_ring_control.name = "position_control" rotation_ring_control.always_visible = True rotation_ring_control.orientation.x = 0 rotation_ring_control.orientation.w = 1 rotation_ring_control.orientation.y = 1 rotation_ring_control.orientation.z = 0 rotation_ring_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(rotation_ring_control) arrow_marker = Marker() arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 1 arrow_marker.pose.position.z = 0.15 arrow_marker.pose.position.x = -0.5 arrow_marker.scale.x = 1 arrow_marker.scale.y = 0.25 arrow_marker.scale.z = 0.25 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 position_control = InteractiveMarkerControl() position_control.name = "rotation_control" position_control.always_visible = True position_control.markers.append(arrow_marker) position_control.orientation.w = 1 position_control.orientation.x = 0 position_control.orientation.y = 1 position_control.orientation.z = 0 position_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE int_marker.controls.append(position_control) self._int_marker_server.insert(int_marker, self.__update_marker_pose__) self._int_marker_server.applyChanges()
def getMarkersFromPose(poseStamped, preGraspStatus): int_marker = InteractiveMarker() int_marker.name = 'gripper' int_marker.header.frame_id = poseStamped.header.frame_id int_marker.pose = poseStamped.pose int_control = InteractiveMarkerControl() int_control.always_visible = True int_control.orientation.w = 1 int_control.orientation.x = 0 int_control.orientation.y = 1 int_control.orientation.z = 0 # ADD GRIPPER CONTROLS gripper_markers = createMarker(0, 0) int_control.interaction_mode = InteractiveMarkerControl.MENU int_control.markers.extend(gripper_markers) int_marker.scale = 0.35 # ADD AUTO PICKING if preGraspStatus: #pre-grasp int_control.markers.extend(createMarker(OFFSET_X, 0)) #lift-grasp int_control.markers.extend(createMarker(0, OFFSET_Z)) #sample-box box_marker = Marker() box_marker.type = Marker.CUBE box_marker.pose.position.x += GRIPPER_OFFSET box_marker.scale.x = 0.05 box_marker.scale.y = 0.05 box_marker.scale.z = 0.05 box_marker.color.r = 1 box_marker.color.g = 1.0 box_marker.color.b = 0.0 box_marker.color.a = 0.5 int_control.markers.append(box_marker) int_marker.controls.append(int_control) # ADD DOF dof_controls = make_6dof_controls() int_marker.controls.extend(dof_controls) # ADD MENU ITEMS menu_entries = [] open_gripper = MenuEntry() open_gripper.id = GRIPPER_OPEN open_gripper.command_type = MenuEntry.FEEDBACK open_gripper.title = 'OPEN GRIPPER' menu_entries.append(open_gripper) close_gripper = MenuEntry() close_gripper.id = GRIPPER_CLOSE close_gripper.command_type = MenuEntry.FEEDBACK close_gripper.title = 'CLOSE GRIPPER' menu_entries.append(close_gripper) move_gripper = MenuEntry() move_gripper.id = GRIPPER_MOVETO move_gripper.command_type = MenuEntry.FEEDBACK if preGraspStatus: move_gripper.title = 'Commence AutoPick' else: move_gripper.title = 'MOVE TO THIS POSE' menu_entries.append(move_gripper) int_marker.menu_entries.extend(menu_entries) return int_marker
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 talker(): rospy.init_node('talker', anonymous=True) # desired_pose_publisher = rospy.Publisher('/equilibrium_pose', Pose, queue_size=10) interactive_marker_init_publisher = rospy.Publisher( '/equilibrium_pose_marker/update_full', InteractiveMarkerInit, queue_size=10) rate = rospy.Rate(30) # Hz seq_num = 1693 # interactive_marker.type = 1 # MAY NEED TO UNCOMMENT IF IT DOESNT WORK # Topic to be published interactive_marker_init = InteractiveMarkerInit() interactive_marker_init.server_id = "/interactive_marker" interactive_marker = InteractiveMarker() interactive_marker.name = "equilibrium_pose" interactive_marker.header.frame_id = "panda_link0" interactive_marker.scale = 0.3 interactive_marker.description = "Equilibrium Pose\nBE CAREFUL! If you move the \nequilibrium pose the robot will follow\ \ it\nso be aware of potential collisions" # Controls - X direction imc = InteractiveMarkerControl() imc.name = "rotate_x" imc.orientation.x = 1.0 imc.orientation.y = 0 imc.orientation.z = 0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 5 interactive_marker.controls.append(imc) imc = InteractiveMarkerControl() imc.name = "move_x" imc.orientation.x = 1.0 imc.orientation.y = 0 imc.orientation.z = 0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 3 interactive_marker.controls.append(imc) # Controls - Y direction imc = InteractiveMarkerControl() imc.name = "rotate_y" imc.orientation.x = 0 imc.orientation.y = 1.0 imc.orientation.z = 0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 5 interactive_marker.controls.append(imc) imc = InteractiveMarkerControl() imc.name = "move_y" imc.orientation.x = 0 imc.orientation.y = 1.0 imc.orientation.z = 0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 3 interactive_marker.controls.append(imc) # Controls - Z direction imc = InteractiveMarkerControl() imc.name = "rotate_z" imc.orientation.x = 0 imc.orientation.y = 0 imc.orientation.z = 1.0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 5 interactive_marker.controls.append(imc) imc = InteractiveMarkerControl() imc.name = "move_z" imc.orientation.x = 0 imc.orientation.y = 0 imc.orientation.z = 1.0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 3 interactive_marker.controls.append(imc) desired_pose = Pose() desired_pose.position.x = 0.441047421421 desired_pose.position.y = 0.00039597222295 desired_pose.position.z = 0.428872718226 desired_pose.orientation.x = 0.998920857906 desired_pose.orientation.y = -0.0855322959381 desired_pose.orientation.z = 0.0364367915829 desired_pose.orientation.w = -0.00293869199231 interactive_marker.pose = desired_pose interactive_marker_init.markers.append(InteractiveMarker()) positive_y_thresh = 0.20 negative_y_thresh = -0.10 rospy.loginfo("Publishing!!") while desired_pose.position.y < positive_y_thresh: desired_pose.position.y += 0.001 rospy.loginfo("Pose(%f, %f, %f || %f, %f, %f, %f)", desired_pose.position.x, desired_pose.position.y, desired_pose.position.z, desired_pose.orientation.x, desired_pose.orientation.y, desired_pose.orientation.z, desired_pose.orientation.w) interactive_marker_init.seq_num = seq_num interactive_marker.pose = desired_pose interactive_marker_init.markers[0] = interactive_marker interactive_marker_init_publisher.publish(interactive_marker_init) seq_num = seq_num + 1 rate.sleep() while desired_pose.position.y > negative_y_thresh: desired_pose.position.y -= 0.001 rospy.loginfo("Pose(%f, %f, %f || %f, %f, %f, %f)", desired_pose.position.x, desired_pose.position.y, desired_pose.position.z, desired_pose.orientation.x, desired_pose.orientation.y, desired_pose.orientation.z, desired_pose.orientation.w) interactive_marker_init.seq_num = seq_num interactive_marker.pose = desired_pose interactive_marker_init.markers[0] = interactive_marker interactive_marker_init_publisher.publish(interactive_marker_init) seq_num = seq_num + 1 rate.sleep()
def create_interactive_6dof(name, color=(1, 0, 0, 1), frame_id='base_link', transform=None, scale=0.05): if transform is None: transform = np.eye(4) int_marker = InteractiveMarker() int_marker.header.stamp = get_safe_stamp() int_marker.header.frame_id = frame_id int_marker.name = name int_marker.scale = scale int_marker.pose = criros.conversions.to_pose(transform) # Move X 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 int_marker.controls.append(control) # Move Y 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 int_marker.controls.append(control) # Move Z 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 int_marker.controls.append(control) # Rotate X 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 int_marker.controls.append(control) # Rotate Y 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 int_marker.controls.append(control) # Rotate Z 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 int_marker.controls.append(control) return int_marker
def publish_gripper(server, pose_stamped, name): """Publishes a marker representing a gripper. Code taken from action_step_marker.py in PR2/PbD. Args: server: An InteractiveMarkerServer pose_stamped: A PoseStamped giving the wrist_roll_link pose. name: string, a unique name for this gripper. """ # Set angle of meshes based on gripper open vs closed. angle = 28 * math.pi / 180.0 # Fully open. STR_MESH_GRIPPER_FOLDER = 'package://pr2_description/meshes/gripper_v0/' STR_GRIPPER_PALM_FILE = STR_MESH_GRIPPER_FOLDER + 'gripper_palm.dae' STR_GRIPPER_FINGER_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger.dae' STR_GRIPPER_FINGERTIP_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger_tip.dae' # Make transforms in preparation for meshes 1, 2, and 3. # NOTE(mbforbes): There are some magic numbers in here that are # used a couple times. Seems like a good candidate for # refactoring to constants, but I think they're more clear if # left in here as (a) they likely won't be changed, and (b) it's # easier to understand the computations with them here. transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) # Create mesh 1 (palm). mesh1 = Marker() mesh1.header.frame_id = pose_stamped.header.frame_id mesh1.mesh_use_embedded_materials = True mesh1.type = Marker.MESH_RESOURCE mesh1.scale.x = 1.0 mesh1.scale.y = 1.0 mesh1.scale.z = 1.0 mesh1.mesh_resource = STR_GRIPPER_PALM_FILE mesh1.pose = pose_stamped.pose # Create mesh 2 (finger). mesh2 = Marker() mesh2.mesh_use_embedded_materials = True mesh2.type = Marker.MESH_RESOURCE mesh2.scale.x = 1.0 mesh2.scale.y = 1.0 mesh2.scale.z = 1.0 mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE mesh2.pose = _get_pose_from_transform(t_proximal) # Create mesh 3 (fingertip). mesh3 = Marker() mesh3.mesh_use_embedded_materials = True mesh3.type = Marker.MESH_RESOURCE mesh3.scale.x = 1.0 mesh3.scale.y = 1.0 mesh3.scale.z = 1.0 mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE mesh3.pose = _get_pose_from_transform(t_distal) # Make transforms in preparation for meshes 4 and 5. quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(math.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) # Create mesh 4 (other finger). mesh4 = Marker() mesh4.mesh_use_embedded_materials = True mesh4.type = Marker.MESH_RESOURCE mesh4.scale.x = 1.0 mesh4.scale.y = 1.0 mesh4.scale.z = 1.0 mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE mesh4.pose = _get_pose_from_transform(t_proximal) # Create mesh 5 (other fingertip). mesh5 = Marker() mesh5.mesh_use_embedded_materials = True mesh5.type = Marker.MESH_RESOURCE mesh5.scale.x = 1.0 mesh5.scale.y = 1.0 mesh5.scale.z = 1.0 mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE mesh5.pose = _get_pose_from_transform(t_distal) # Append all meshes we made. control = InteractiveMarkerControl() control.markers = [mesh1, mesh2, mesh3, mesh4, mesh5] control.interaction_mode = InteractiveMarkerControl.NONE interactive_marker = InteractiveMarker() interactive_marker.controls = [control] interactive_marker.header.frame_id = pose_stamped.header.frame_id interactive_marker.pose = pose_stamped.pose interactive_marker.name = name server.insert(interactive_marker) server.applyChanges()
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
while not initial_pose_found: rospy.sleep(1) state_sub.unregister() pose_pub = rospy.Publisher( "equilibrium_pose", PoseStamped, queue_size=10) server = InteractiveMarkerServer("equilibrium_pose_marker") int_marker = InteractiveMarker() int_marker.header.frame_id = link_name int_marker.scale = 0.3 int_marker.name = "equilibrium_pose" int_marker.description = ("Equilibrium Pose\nBE CAREFUL! " "If you move the \nequilibrium " "pose the robot will follow it\n" "so be aware of potential collisions") int_marker.pose = marker_pose.pose # run pose publisher rospy.Timer(rospy.Duration(0.005), lambda msg: publisherCallback(msg, link_name)) # insert a box 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 int_marker.controls.append(control) control = InteractiveMarkerControl()
def _update_viz_core(self): '''Updates visualization after a change.''' # Create a new IM control. menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = self._get_ref_name() pose = self.get_pose() # Multiplex marker types added based on action step type. if self.action_step.type == ActionStep.ARM_TARGET: # Handle "normal" steps (saved poses). menu_control = self._make_gripper_marker( menu_control, self._is_hand_open()) elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # Handle trajectories. # First, get all trajectory positions. point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) # Add a main maker for all points in the trajectory (sphere # list). menu_control.markers.append( Marker( type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=TRAJ_MARKER_LIFETIME, scale=SCALE_TRAJ_STEP_SPHERES, header=Header(frame_id=frame_id), color=COLOR_TRAJ_STEP_SPHERES, points=point_list ) ) # Add a marker for the first step in the trajectory. menu_control.markers.append( ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_FIRST, self._get_traj_pose(0), frame_id, TRAJ_ENDPOINT_SCALE ) ) # Add a marker for the last step in the trajectory. last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append( ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_LAST, self._get_traj_pose(last_index), frame_id, TRAJ_ENDPOINT_SCALE ) ) else: # Neither "normal" pose nor trajectory; error... rospy.logerr( 'Non-handled action step type ' + str(self.action_step.type)) # Add an arrow to the relative object, if there is one. ref_frame = World.get_ref_from_name(frame_id) if ref_frame == ArmState.OBJECT: menu_control.markers.append( Marker( type=Marker.ARROW, id=(ID_OFFSET_REF_ARROW + self.get_uid()), lifetime=TRAJ_MARKER_LIFETIME, scale=SCALE_OBJ_REF_ARROW, header=Header(frame_id=frame_id), color=COLOR_OBJ_REF_ARROW, points=[pose.position, Point(0, 0, 0)] ) ) # Make and add the text for this step ('Step X'). text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + TEXT_Z_OFFSET menu_control.markers.append( Marker( type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=SCALE_STEP_TEXT, text='Step ' + str(self.step_number), color=COLOR_STEP_TEXT, header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)) ) ) # 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 int_marker.scale = INT_MARKER_SCALE self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert( int_marker, self.marker_feedback_cb)
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)
from visualization_msgs.msg import InteractiveMarker, InteractiveMarkerControl from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander from interactive_markers.interactive_marker_server import * from geometry_msgs.msg import Pose, PoseStamped if __name__=="__main__": rospy.init_node('marker_teleop') pub = rospy.Publisher('robot_interaction_interactive_marker_topic', InteractiveMarker) robot = MoveGroupCommander("sia5d"); server = InteractiveMarkerServer("simple_marker") # create interactive marker int_marker = InteractiveMarker() int_marker.header.frame_id = "world" int_marker.name = "my_marker" int_marker.description = "Teleop Control" p = robot.get_current_pose() rate = rospy.Rate(1) while not rospy.is_shutdown(): p.pose.position.x = p.pose.position.x+0.05 int_marker.pose = p.pose pub.publish(int_marker) print 'heh' rate.sleep()
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 makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.frame_id int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "" # "EEF 6DOF control" # 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 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 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 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 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 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 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 int_marker.controls.append(control) # self.menu_handler.insert("Do stuff", callback=self.processFeedback) # This makes the floating text appear # make one control using default visuals # control = InteractiveMarkerControl() # control.interaction_mode = InteractiveMarkerControl.MENU # control.description="Menu" # control.name = "menu_only_control" # int_marker.controls.append(copy.deepcopy(control)) # marker = self.makeArrow( int_marker ) # control.markers.append( marker ) # control.always_visible = False # int_marker.controls.append(control) self.server.insert(int_marker, self.processFeedback)
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 _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 create_interactive_6dof(name, color=(1,0,0,1), frame_id='base_link', transform=None, scale=0.05): """ Create a 6-DoF interactive marker Parameters ---------- name: string Marker name color: array_like Marker color is a 4 elements iterable to be used as RGBA color frame_id: str Reference frame of the marker frame. This a frame that must exist in TF transform: array_like Homogenous transformation (4x4) of the marker with respect to the reference frame ``frame_id`` scale: float Scale of the marker applied before the position/orientation. Returns ---------- int_marker: visualization_msgs.InteractiveMarker The 6-DoF interactive marker """ if transform is None: transform = np.eye(4) int_marker = InteractiveMarker() int_marker.header.stamp = get_safe_stamp() int_marker.header.frame_id = frame_id int_marker.name = name int_marker.scale = scale int_marker.pose = criconv.to_pose(transform) # Move X 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 int_marker.controls.append(control) # Move Y 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 int_marker.controls.append(control) # Move Z 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 int_marker.controls.append(control) # Rotate X 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 int_marker.controls.append(control) # Rotate Y 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 int_marker.controls.append(control) # Rotate Z 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 int_marker.controls.append(control) return int_marker
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 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