def __init__(self, root, name, description, server, ns="", position0=[0., 0., 0.]): # Make interactive marker self.int_marker = InteractiveMarker() self.int_marker.header.frame_id = root self.int_marker.pose.position = Point(*position0) self.int_marker.scale = 0.1 self.int_marker.name = name self.name = name self.int_marker.description = description # Make the way it should look mrkrmsg = Marker() mrkrmsg.type = Marker.CUBE mrkrmsg.scale.x = 0.03 mrkrmsg.scale.y = 0.03 mrkrmsg.scale.z = 0.03 mrkrmsg.color.r = 0.7 mrkrmsg.color.a = 1.0 # Make the Inner cube control mode: cubecntrl = InteractiveMarkerControl() cubecntrl.always_visible = True cubecntrl.interaction_mode = InteractiveMarkerControl.MOVE_3D cubecntrl.markers.append(mrkrmsg) controls = [cubecntrl] # Make XYZ linear motion control modes: for i in range(3): direction = [0] * i + [1] + [0] * (2 - i) control = InteractiveMarkerControl() control.name = "move_" + chr(ord("x") + i) control.orientation = Quaternion(*(direction + [1])) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS controls.append(control) # Make XYZ rotation control modes: for i in range(3): direction = [0] * i + [1] + [0] * (2 - i) control = InteractiveMarkerControl() control.name = "rotate_" + chr(ord("x") + i) control.orientation = Quaternion(*(direction + [1])) control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS controls.append(control) # Add them to the interactive marker self.int_marker.controls = controls # Setup pose publisher self.pose_pub = rospy.Publisher(ns + name, Pose, queue_size=1) # Setup the interactive marker in the server self.server = server self.server.insert(self.int_marker, self.callbackPublish) self.server.applyChanges()
def _make_6dof_control(self, name, orientation, is_move, is_fixed): '''Creates and returns one component of the 6dof controller. Args: name (str): Name for hte control orientation (Quaternion): How the control should be oriented. is_move (bool): Looks like whether the marker moves the object (?). Currently passed as True for moving markers, False for rotating ones. is_fixed (bool): Looks like whether position is fixed (?). Currently always passed as True. Returns: InteractiveMarkerControl ''' control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if self.is_control_visible: if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
def _CreateMarkerControl(name, orientation, marker_type): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.interaction_mode = marker_type control.always_visible = False return control
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 make_6dof_controls(): # Edit axes. # N.B. Follow order from tutorial? n = 1 / math.sqrt(2) axes = [ ('x', assign(Quaternion(), w=n, x=n, y=0, z=0)), ('z', assign(Quaternion(), w=n, x=0, y=0, z=n)), ('y', assign(Quaternion(), w=n, x=0, y=n, z=0)), ] controls = [] for name, quat in axes: control = InteractiveMarkerControl() control.orientation = quat control.name = "rotate_" + name control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS controls.append(control) control = InteractiveMarkerControl() control.orientation = quat control.name = "move_" + name control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS controls.append(control) return controls
def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
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 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 set_marker_settings(self, arrows, frame=None, scale=0.25): '''arrows is a list with any number of the following strings: ["x", "y", "z", "a", "b", "c"].''' if frame: style = frame.style else: style = "none" ## Marker int_marker = InteractiveMarker() int_marker.header.frame_id = "" int_marker.name = "" int_marker.description = "Frame Editor" int_marker.pose = Pose() int_marker.scale = scale if "x" in arrows: control = InteractiveMarkerControl() control.name = "move_x" control.orientation = NewQuaternion(1, 0, 0, 1) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) if "y" in arrows: control = InteractiveMarkerControl() control.name = "move_y" control.orientation = NewQuaternion(0, 1, 0, 1) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) if "z" in arrows: control = InteractiveMarkerControl() control.name = "move_z" control.orientation = NewQuaternion(0, 0, 1, 1) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) if "a" in arrows: control = InteractiveMarkerControl() control.name = "rotate_x" control.orientation = NewQuaternion(1, 0, 0, 1) control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) if "b" in arrows: control = InteractiveMarkerControl() control.name = "rotate_y" control.orientation = NewQuaternion(0, 1, 0, 1) control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) if "c" in arrows: control = InteractiveMarkerControl() control.name = "rotate_z" control.orientation = NewQuaternion(0, 0, 1, 1) control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) ## Style ## ## #style_marker = Marker() #style_marker.scale = NewVector3(0.75*scale, 0.75*scale, 0.75*scale) #style_marker.color = NewColor(0.0, 0.5, 0.5, 0.75) #if style != "none": # style_marker = frame.marker #style_control = InteractiveMarkerControl() #style_control.always_visible = True #style_control.markers.append(style_marker) #if style != "none": # int_marker.controls.append(style_control) self.int_marker = 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