def create_interactive_marker(self, T): im = InteractiveMarker() im.header.frame_id = "world" im.name = "target" im.description = "Controller Target" im.scale = 0.2 im.pose.position = Point(*T[0:3, 3]) im.pose.orientation = Quaternion(*tf.quaternion_from_matrix(T)) self.process_marker_feedback(im) # set target to initial pose # Create a control to move a (sphere) marker around with the mouse control = InteractiveMarkerControl() control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.markers.extend( frame(numpy.identity(4), scale=0.1, frame_id='').markers) im.controls.append(control) # Create arrow controls to move the marker for dir in 'xyz': control = InteractiveMarkerControl() control.name = "move_" + dir control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation.x = 1 if dir == 'x' else 0 control.orientation.y = 1 if dir == 'y' else 0 control.orientation.z = 1 if dir == 'z' else 0 control.orientation.w = 1 im.controls.append(control) # Add the marker to the server and indicate that processMarkerFeedback should be called self.insert(im, self.process_marker_feedback) # Publish all changes self.applyChanges()
def __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 initialize_object_marker(self, object_id, pose): print "Adding interactive marker for object: ", object_id color = self.object_id_color(object_id) marker = InteractiveMarker() marker.header.frame_id = "map" marker.name = "object_marker_" + str(object_id) marker.description = "Object " + str(object_id) click_marker = InteractiveMarker() click_marker.header.frame_id = "map" click_marker.name = "button_object_marker_" + str(object_id) click_marker.description = "" # the marker in the middle box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.25 box_marker.scale.y = 0.25 box_marker.scale.z = 0.25 box_marker.color.r = color[0] box_marker.color.g = color[1] box_marker.color.b = color[2] box_marker.color.a = 1. box_marker.id = 1000 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True #box_control.always_visible = False box_control.markers.append(box_marker) box_control.name = "button" box_control.interaction_mode = InteractiveMarkerControl.BUTTON marker.controls.append(box_control) # move x control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.always_visible = True # control.name = "move_x" # control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.name = "move_plane" control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE marker.controls.append(control) self.marker_poses[object_id] = pose self.previous_poses[object_id] = pose self.marker_server.insert(marker, self.marker_feedback) self.marker_server.applyChanges() pose.position.z = 0.15 self.marker_server.setPose( marker.name, pose ) self.marker_server.applyChanges()
def get_2_dof_interactive_marker(marker_name, frame, x=0.0, y=0.0): """Return an interactive marker with 2 degree of freedom (X and Y axis) in `frame` at (`x`, `y`, 0.0) position named `name` :marker_name: string :frame: string :x: int :y: int :returns: visualization_msgs.InteractiveMarker """ # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.name = marker_name int_marker.pose.position.x = x int_marker.pose.position.y = y # int_marker.description = "Simple 2-DOF Control" # create a grey box marker box_marker = Marker() box_marker.type = Marker.SPHERE box_marker.scale.x = box_marker.scale.y = box_marker.scale.z = 0.1 box_marker.color.r = box_marker.color.a = 1.0 box_marker.color.g = box_marker.color.b = 0.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append(box_marker) # add the control to the interactive marker int_marker.controls.append(box_control) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows rotate_control = InteractiveMarkerControl() rotate_control.name = "move_x" rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS # add the control to the interactive marker int_marker.controls.append(rotate_control) rotate_control2 = InteractiveMarkerControl() rotate_control2.orientation.z = rotate_control2.orientation.w = 0.707 rotate_control2.name = "move_y" rotate_control2.interaction_mode = InteractiveMarkerControl.MOVE_AXIS # add the control to the interactive marker int_marker.controls.append(rotate_control2) return int_marker
def _make_markers(self): int_marker = InteractiveMarker() int_marker.header.frame_id = self._frame_id int_marker.name = 'golden_snitch' int_marker.description = 'tool tip target' # Create a sphere to mark where the tool tip should go snitch = Marker() snitch.type = Marker.SPHERE snitch.scale.x = 0.1 snitch.scale.y = 0.1 snitch.scale.z = 0.1 snitch.color.r = 0.8 snitch.color.g = 0.8 snitch.color.b = 0.0 snitch.color.a = 0.7 snitch_control = InteractiveMarkerControl() snitch_control.always_visible = True snitch_control.markers.append(snitch) int_marker.controls.append(snitch_control) x_snitch_control = InteractiveMarkerControl() x_snitch_control.name = 'move_x' x_snitch_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS y_snitch_control = InteractiveMarkerControl() y_snitch_control.name = 'move_y' y_snitch_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS y_snitch_control.orientation.w = 0.7071068 y_snitch_control.orientation.x = 0.0 y_snitch_control.orientation.y = 0.0 y_snitch_control.orientation.z = 0.7071068 z_snitch_control = InteractiveMarkerControl() z_snitch_control.name = 'move_z' z_snitch_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS z_snitch_control.orientation.w = 0.7071068 z_snitch_control.orientation.x = 0.0 z_snitch_control.orientation.y = 0.7071068 z_snitch_control.orientation.z = 0.0 int_marker.controls.append(x_snitch_control) int_marker.controls.append(y_snitch_control) int_marker.controls.append(z_snitch_control) return int_marker
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 _make_revolute_marker(self, revolute_joint: RevoluteJoint_): int_marker = InteractiveMarker() int_marker.header.frame_id = revolute_joint.child_body().body_frame().name() int_marker.name = revolute_joint.name() int_marker.scale = 0.3 int_marker.pose.position.x = 0. int_marker.pose.position.y = 0. int_marker.pose.position.z = 0. int_marker.pose.orientation.w = 1. int_marker.pose.orientation.x = 0. int_marker.pose.orientation.y = 0. int_marker.pose.orientation.z = 0. # Drake revolute axis is in frame F on parent axis_hat = revolute_joint.revolute_axis() self._joint_axis_in_child[revolute_joint.name()] = axis_hat # What rotation would get the parent X axis to align with the joint axis? rotation_matrix = ComputeBasisFromAxis(0, axis_hat) pydrake_quat = RotationMatrix(rotation_matrix).ToQuaternion() joint_control = InteractiveMarkerControl() joint_control.orientation.w = pydrake_quat.w() joint_control.orientation.x = pydrake_quat.x() joint_control.orientation.y = pydrake_quat.y() joint_control.orientation.z = pydrake_quat.z() joint_control.always_visible = True joint_control.name = f'rotate_axis_{revolute_joint.name()}' joint_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(joint_control) return int_marker
def CreateVisualControlFromMarker(marker, always_visible=True, interaction_mode=InteractiveMarkerControl.MENU): control = InteractiveMarkerControl() control.name = "visual" control.always_visible = always_visible control.interaction_mode = interaction_mode control.markers.append(marker) return control
def make_interactive_marker(pose, model): int_marker = InteractiveMarker() int_marker.name = model int_marker.description = model int_marker.header.frame_id = "map" int_marker.pose = pose int_marker.pose.position.z = max(int_marker.pose.position.z, 0.01) # ensure marker is above ground int_marker.pose.orientation = quaternion_msg_from_yaw( yaw(pose)) # discard all but yaw to ensure marker is flat int_marker.scale = 1 make_model_control(int_marker, model) control = InteractiveMarkerControl() control.name = "drag" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 normalize_quaternion(control.orientation) control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE int_marker.controls.append(copy.deepcopy(control)) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) server.insert(int_marker, process_feedback) menu_handler.apply(server, int_marker.name) return int_marker
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 get_3_dof_interactive_marker(marker_name, frame, x=0.0, y=0.0, theta=0.0): """Return an interactive marker with 2 degree of freedom (X and Y axis) in `frame` at (`x`, `y`, 0.0) position named `name` :marker_name: string :frame: string :x: int :y: int :returns: visualization_msgs.InteractiveMarker """ # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.name = marker_name int_marker.pose.position.x = x int_marker.pose.position.y = y quat = tf.transformations.quaternion_from_euler(0.0, 0.0, theta) int_marker.pose.orientation = Quaternion(*quat) int_marker.description = marker_name # create a white box marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 1.0 box_marker.scale.y = 0.1 box_marker.scale.z = 1.0 box_marker.color.r = box_marker.color.a = box_marker.color.g = box_marker.color.b = 1.0 box_marker.pose.position.z = -0.5 box_marker.pose.orientation.w = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append(box_marker) box_control.name = "move_x_y" box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE box_control.orientation.w = box_control.orientation.y = 1.0 int_marker.controls.append(box_control) rotate_control = InteractiveMarkerControl() rotate_control.name = "rotate_z" rotate_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS rotate_control.orientation.y = rotate_control.orientation.w = 1.0 int_marker.controls.append(rotate_control); return int_marker
def CreatePrimitiveControl(name, scaleFactor, marker_type, id=randint(0,10000)): marker = CreatePrimitiveMarker(name, scaleFactor, marker_type, id) control = InteractiveMarkerControl() control.name = name control.always_visible = True control.interaction_mode = InteractiveMarkerControl.MENU control.markers.append(marker) return control
def add_menu_handler(int_marker, menu_handler, server): control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.MENU control.description = "Options" control.name = "menu_only_control" int_marker.controls.append(control) menu_handler.apply(server, int_marker.name) server.applyChanges()
def add_menu_handler(int_marker, menu_handler, server): control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.MENU control.description="Options" control.name = "menu_only_control" int_marker.controls.append(control) menu_handler.apply(server, int_marker.name) server.applyChanges()
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 axis_control(name, q): cr = InteractiveMarkerControl() cr.orientation.x = q[0] cr.orientation.y = q[1] cr.orientation.z = q[2] cr.orientation.w = q[3] cr.name = name return cr
def makeMarker(self, callback=None, marker=None, pose=[0, 0, 0], controls=[], fixed=False, name=None, frame="map", description="", imode=0, rot=[0, 0, 0, 1]): if marker is None: marker = self.mg.marker() if callback is None: callback = default_callback if name is None: name = "control%d" % self.c self.c += 1 int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.pose.position.x = pose[0] int_marker.pose.position.y = pose[1] int_marker.pose.position.z = pose[2] int_marker.pose.orientation.x = rot[0] int_marker.pose.orientation.y = rot[1] int_marker.pose.orientation.z = rot[2] int_marker.pose.orientation.w = rot[3] int_marker.scale = 1 int_marker.name = name int_marker.description = description control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = imode control.markers.append(marker) int_marker.controls.append(control) for control_name in controls: data = TYPEDATA[control_name] control = InteractiveMarkerControl() control.orientation.w = data[0] / SQRT2 control.orientation.x = data[1] / SQRT2 control.orientation.y = data[2] / SQRT2 control.orientation.z = data[3] / SQRT2 control.name = control_name control.interaction_mode = data[4] if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.server.insert(int_marker, callback) self.markers[name] = int_marker self.server.applyChanges()
def add_new_marker(self, name, body, callback, selected=True, delay_update=False, frame=None, pose=None): """Adds a new interactive marker to the node. :param name: Name of the marker :type name: str :param body: Body controlled by the marker :type body: RigidBody, MultiBody :param callback: Callback function for the marker :type callback: function :param selected: Should the marker be selected upon creation :type selected: bool :param delay_update: Delay the update to the server :type delay_update: bool :param frame: Name of reference frame :type frame: NoneType, str :param pose: Initial pose of the marker :type pose: NoneType, Frame """ intMarker = InteractiveMarker() intMarker.name = name intMarker.header.frame_id = frame if frame is not None else name intMarker.header.stamp = rospy.Time(0) intMarker.scale = 1.0 if pose is not None: intMarker.pose.position.x = pose.position[0] intMarker.pose.position.y = pose.position[1] intMarker.pose.position.z = pose.position[2] intMarker.pose.orientation.x = pose.quaternion[0] intMarker.pose.orientation.y = pose.quaternion[1] intMarker.pose.orientation.z = pose.quaternion[2] intMarker.pose.orientation.w = pose.quaternion[3] control = InteractiveMarkerControlMsg() control.interaction_mode = InteractiveMarkerControlMsg.MOVE_3D control.always_visible = True control.orientation.w = 1.0 control.name = 'visual' control.description = name intMarker.controls.append(control) make6DOFGimbal(intMarker) self.create_visual_marker(name, body, intMarker, control) activateMarker(intMarker, selected) self.marker_server.insert(intMarker, callback) self.markers[name] = (intMarker, callback) body.register_deletion_cb(self.on_obj_deleted) if not delay_update: self.marker_server.applyChanges()
def make_rotate_axis_fixed(axis_name, orientation): controlR = InteractiveMarkerControl() controlR.orientation.w = orientation[0] controlR.orientation.x = orientation[1] controlR.orientation.y = orientation[2] controlR.orientation.z = orientation[3] controlR.name = "rotate_" + axis_name controlR.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed_orientation: controlR.orientation_mode = InteractiveMarkerControl.FIXED controlM = InteractiveMarkerControl() controlM.orientation.w = orientation[0] controlM.orientation.x = orientation[1] controlM.orientation.y = orientation[2] controlM.orientation.z = orientation[3] controlM.name = "move_" + axis_name controlM.interaction_mode = InteractiveMarkerControl.MOVE_AXIS controlM.orientation_mode = InteractiveMarkerControl.FIXED return (controlR, controlM)
def CreateVisualControlFromMarker( marker, always_visible=True, interaction_mode=InteractiveMarkerControl.MENU): control = InteractiveMarkerControl() control.name = "visual" control.always_visible = always_visible control.interaction_mode = interaction_mode control.markers.append(marker) return control
def initialize_room_markers(self): for room_id in range(0, len(self.regions)): pose = Pose() pose.position.x = self.centers[room_id, 0] pose.position.y = self.centers[room_id, 1] pose.position.z = 0.2 pose.orientation.x = 0. pose.orientation.y = 0. pose.orientation.z = 0. pose.orientation.w = 1. marker = InteractiveMarker() marker.header.frame_id = "map" marker.name = "room_marker_" + str(room_id) marker.description = "Room " + str(room_id) # the marker in the middle box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.35 box_marker.scale.y = 0.35 box_marker.scale.z = 0.35 box_marker.color.r = 0. box_marker.color.g = 0. box_marker.color.b = 1. box_marker.color.a = 1. box_marker.id = 1000 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True #box_control.always_visible = False box_control.markers.append(box_marker) box_control.name = "button" box_control.interaction_mode = InteractiveMarkerControl.BUTTON marker.controls.append(box_control) #marker.controls.append(box_control) # move x #control = InteractiveMarkerControl() #control.orientation.w = 1 #control.orientation.x = 0 #control.orientation.y = 1 #control.orientation.z = 0 #control.always_visible = True # control.name = "move_x" # control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.marker_server.insert(marker, self.room_feedback) self.marker_server.applyChanges() self.marker_server.setPose( marker.name, pose ) self.marker_server.applyChanges()
def CreatePrimitiveControl(name, scaleFactor, marker_type, id=randint(0, 10000)): marker = CreatePrimitiveMarker(name, scaleFactor, marker_type, id) control = InteractiveMarkerControl() control.name = name control.always_visible = True control.interaction_mode = InteractiveMarkerControl.MENU control.markers.append(marker) 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 six_dof_controls(): """Returns a list of 6 InteractiveMarkerControls """ controls = [] # Create 6 DOF controls rx_control = InteractiveMarkerControl() rx_control.orientation.w = 1 rx_control.orientation.x = 1 rx_control.orientation.y = 0 rx_control.orientation.z = 0 rx_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS rx_control.name = 'rotate_x' controls.append(rx_control) mx_control = InteractiveMarkerControl() mx_control.orientation.w = 1 mx_control.orientation.x = 1 mx_control.orientation.y = 0 mx_control.orientation.z = 0 mx_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS mx_control.name = 'move_x' controls.append(mx_control) rz_control = InteractiveMarkerControl() rz_control.orientation.w = 1 rz_control.orientation.x = 0 rz_control.orientation.y = 1 rz_control.orientation.z = 0 rz_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS rz_control.name = 'rotate_z' controls.append(rz_control) mz_control = InteractiveMarkerControl() mz_control.orientation.w = 1 mz_control.orientation.x = 0 mz_control.orientation.y = 1 mz_control.orientation.z = 0 mz_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS mz_control.name = 'move_z' controls.append(mz_control) ry_control = InteractiveMarkerControl() ry_control.orientation.w = 1 ry_control.orientation.x = 0 ry_control.orientation.y = 0 ry_control.orientation.z = 1 ry_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS ry_control.name = 'rotate_y' controls.append(ry_control) my_control = InteractiveMarkerControl() my_control.orientation.w = 1 my_control.orientation.x = 0 my_control.orientation.y = 0 my_control.orientation.z = 1 my_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS my_control.name = 'move_y' controls.append(my_control) return controls
def make_interactive_marker(name, pose, frame='base', color=(0.5, 0.5, 0.5)): rospy.loginfo("Creating imarker...") int_marker = interactive_marker_server.InteractiveMarker() int_marker.header.frame_id = frame int_marker.header.stamp = rospy.Time.now() int_marker.scale = 0.25 int_marker.name = name int_marker.description = "interactive marker" cylinder_marker = Marker() cylinder_marker.type = Marker.CYLINDER cylinder_marker.scale.x = 0.25 cylinder_marker.scale.y = 0.25 cylinder_marker.scale.z = 0.25 cylinder_marker.color.r = color[0] cylinder_marker.color.g = color[1] cylinder_marker.color.b = color[2] cylinder_marker.color.a = 0.5 cylinder_control = InteractiveMarkerControl() cylinder_control.always_visible = True cylinder_control.markers.append(cylinder_marker) int_marker.controls.append(cylinder_control) for axis, orientation in zip(['x', 'y', 'z'], [(1, 1, 0, 0), (1, 0, 1, 0), (1, 0, 0, 1)]): control = InteractiveMarkerControl() for attr, val in zip(['w', 'x', 'y', 'z'], orientation): setattr(control.orientation, attr, val) control.name = "rotate_{}".format(axis) control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control.name = "move_{}".format(axis) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) int_marker.pose = pose rospy.loginfo("Imarker created") return int_marker
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_cont(self, parent): """Creates the controller for the marker for the side of an object. Args: parent (Marker) Returns: control (MarkerController) """ control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.BUTTON control.always_visible = True control.name = parent.ns return control
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 __init__(self): self.int_marker = InteractiveMarker() self.int_marker.header.frame_id = "/world" self.int_marker.name = "my_marker" self.int_marker.description = "Simple 2-DOF Control" self.int_marker.scale = 0.5 self.mesh_marker = MeshMarker() self.mesh_marker.set_color(color=(1.0, 1.0, 1.0, 0.5)) # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = False box_control.markers.append(self.mesh_marker.marker) # add the control to the interactive marker self.int_marker.controls.append(box_control) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.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 self.int_marker.controls.append(control) self.set_position() self.set_orientation()
def __init__(self): self.int_marker = InteractiveMarker() self.int_marker.header.frame_id = "/world" self.int_marker.name = "my_marker" self.int_marker.description = "Simple 2-DOF Control" self.int_marker.scale = 0.5 self.mesh_marker = MeshMarker() self.mesh_marker.set_color(color=(1.0,1.0,1.0,0.5)) # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = False box_control.markers.append(self.mesh_marker.marker) # add the control to the interactive marker self.int_marker.controls.append(box_control) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.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 self.int_marker.controls.append(control) self.set_position() self.set_orientation()
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 make_6dof_controls(): """Returns a list of 6 InteractiveMarkerControls """ controls = [] # Add 6 DOF controls control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.name = 'rotate_x' controls.append(copy.deepcopy(control)) control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.name = 'move_x' controls.append(copy.deepcopy(control)) control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.name = 'rotate_z' controls.append(copy.deepcopy(control)) control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.name = 'move_z' controls.append(copy.deepcopy(control)) control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.name = 'rotate_y' controls.append(copy.deepcopy(control)) control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.name = 'move_y' controls.append(copy.deepcopy(control)) return controls
def add_marker(self, init_position, callback, controls=[], scale=1., marker=None, name=None, fixed_orientation=False): if name is None: name = 'control%d' % (self.marker_ctr) self.marker_ctr += 1 if marker is None: marker = make_markers.make_marker() marker.header.frame_id = self.frame_id marker.header.frame_id = self.frame_id int_marker = InteractiveMarker() int_marker.header.frame_id = self.frame_id int_marker.pose.position = init_position int_marker.scale = scale int_marker.name = name vm_control = InteractiveMarkerControl() vm_control.always_visible = True vm_control.markers.append(marker) int_marker.controls.append(vm_control) #int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D for control_name in controls: data = TYPEDATA[control_name] control = InteractiveMarkerControl() control.orientation.w = data[0] control.orientation.x = data[1] control.orientation.y = data[2] control.orientation.z = data[3] control.name = control_name control.interaction_mode = data[4] if fixed_orientation: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.server.insert(int_marker, callback) self.markers[name] = int_marker
def init_marker(self): self.server = InteractiveMarkerServer("control_markers") control_marker = InteractiveMarker() # control_marker.header.frame_id = self.robot.get_root() control_marker.header.frame_id = "base" control_marker.name = "cc_marker" move_control = InteractiveMarkerControl() move_control.name = "move_x" move_control.orientation.w = 1 move_control.orientation.x = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "move_y" move_control.orientation.w = 1 move_control.orientation.y = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "move_z" move_control.orientation.w = 1 move_control.orientation.z = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_x" move_control.orientation.w = 1 move_control.orientation.x = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_y" move_control.orientation.w = 1 move_control.orientation.z = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_z" move_control.orientation.w = 1 move_control.orientation.y = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) control_marker.scale = 0.25 self.server.insert(control_marker, self.control_marker_feedback) # 'commit' changes and send to all clients self.server.applyChanges()
def __init__(self): rospy.sleep(1.0) self.items = [ 'pinger_surface', 'pinger_shooter', 'vampire_slayer', 'start_gate1', 'start_gate2' ] self.guess_service = rospy.Service('guess_location', GuessRequest, self.request_location) self.markers_subscribers = [] self.markers_locations = dict.fromkeys(self.items) self.markers_servers = [] self.markers = [] box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.45 box_marker.scale.y = 0.45 box_marker.scale.z = 0.45 box_marker.color.r = 0.0 box_marker.color.g = 0.5 box_marker.color.b = 0.5 box_marker.color.a = 1.0 box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append(box_marker) rotate_control = InteractiveMarkerControl() rotate_control.name = "move_x" rotate_control.orientation.w = 0.707 rotate_control.orientation.x = 0 rotate_control.orientation.y = 0.707 rotate_control.orientation.z = 0 rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE spacer = 0 for i in self.items: self.markers.append(InteractiveMarker()) self.markers[spacer].header.frame_id = "/map" self.markers[spacer].name = i self.markers[spacer].description = i self.markers[spacer].controls.append(box_control) self.markers[spacer].controls.append(rotate_control) self.markers[spacer].pose.position.x = spacer self.markers[spacer].pose.position.y = 0 self.markers[spacer].pose.position.z = 0 spacer = spacer + 1
def __init__(self): rospy.sleep(1.0) self.items = [ 'pinger1', 'pinger2', 'dice', 'start_gate1', 'start_gate2' ] self.guess_service = rospy.Service('guess_location', GuessRequest, self.request_location) self.markers_subscribers = [] self.markers_locations = dict.fromkeys(self.items) self.markers_servers = [] self.markers = [] box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.45 box_marker.scale.y = 0.45 box_marker.scale.z = 0.45 box_marker.color.r = 0.0 box_marker.color.g = 0.5 box_marker.color.b = 0.5 box_marker.color.a = 1.0 box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append(box_marker) rotate_control = InteractiveMarkerControl() rotate_control.name = "move_x" rotate_control.orientation.w = 0.707 rotate_control.orientation.x = 0 rotate_control.orientation.y = 0.707 rotate_control.orientation.z = 0 rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE spacer = 0 for i in self.items: self.markers.append(InteractiveMarker()) self.markers[spacer].header.frame_id = "map" self.markers[spacer].name = i self.markers[spacer].description = i self.markers[spacer].controls.append(box_control) self.markers[spacer].controls.append(rotate_control) self.markers[spacer].pose.position.x = spacer self.markers[spacer].pose.position.y = 0 self.markers[spacer].pose.position.z = 0 spacer = spacer + 1
def talker(): pub = rospy.Publisher("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update", InteractiveMarkerUpdate) rospy.init_node('talker', anonymous=True) r = rospy.Rate(1) int_marker = InteractiveMarker() int_marker.name = "EE:goal_link_t" controller = InteractiveMarkerControl() controller.name = '_u1' int_marker.controls = controller p = InteractiveMarkerPose() updater = InteractiveMarkerUpdate() while not rospy.is_shutdown(): p.pose.position.x += 0.5 updater.markers = int_marker updater.poses = p print updater pub.publish(updater) r.sleep()
def init_marker(self): control_marker = InteractiveMarker() control_marker.header.frame_id = "/world_link" control_marker.name = "cg_marker" move_control = InteractiveMarkerControl() move_control.name = "move_x" move_control.orientation.w = 1 move_control.orientation.x = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "move_y" move_control.orientation.w = 1 move_control.orientation.y = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "move_z" move_control.orientation.w = 1 move_control.orientation.z = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_x" move_control.orientation.w = 1 move_control.orientation.x = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_y" move_control.orientation.w = 1 move_control.orientation.z = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_z" move_control.orientation.w = 1 move_control.orientation.y = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) control_marker.scale = 0.25 self.server.insert(control_marker, self.control_marker_feedback) self.server.applyChanges()
def make6DOFControls(): translation_x_control = InteractiveMarkerControl() translation_x_control.name = "move_x" translation_x_control.orientation.w = 1 translation_x_control.orientation.x = 1 translation_x_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS translation_y_control = InteractiveMarkerControl() translation_y_control.name = "move_y" translation_y_control.orientation.w = 1 translation_y_control.orientation.y = 1 translation_y_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS translation_z_control = InteractiveMarkerControl() translation_z_control.name = "move_z" translation_z_control.orientation.w = 1 translation_z_control.orientation.z = 1 translation_z_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS rotation_x_control = InteractiveMarkerControl() rotation_x_control.name = "rotate_x" rotation_x_control.orientation.w = 1 rotation_x_control.orientation.x = 1 rotation_x_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS rotation_y_control = InteractiveMarkerControl() rotation_y_control.name = "rotate_y" rotation_y_control.orientation.w = 1 rotation_y_control.orientation.y = 1 rotation_y_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS rotation_z_control = InteractiveMarkerControl() rotation_z_control.name = "rotate_z" rotation_z_control.orientation.w = 1 rotation_z_control.orientation.z = 1 rotation_z_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS return [translation_x_control, translation_y_control, translation_z_control, rotation_x_control, rotation_y_control, rotation_z_control]
box_control.always_visible = True box_control.markers.append(box_marker) # add the control to the interactive marker interactive_marker.controls.append(box_control) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows translate_x = InteractiveMarkerControl() translate_x.interaction_mode = InteractiveMarkerControl.MOVE_AXIS translate_x.orientation.w = 1 translate_x.orientation.x = 1 translate_x.orientation.y = 0 translate_x.orientation.z = 0 translate_x.name = "translate_x" interactive_marker.controls.append(translate_x) translate_y = InteractiveMarkerControl() translate_y.interaction_mode = InteractiveMarkerControl.MOVE_AXIS translate_y.orientation.w = 1 translate_y.orientation.x = 0 translate_y.orientation.y = 0 translate_y.orientation.z = 1 translate_y.name = "translate_y" interactive_marker.controls.append(translate_y) translate_z = InteractiveMarkerControl() translate_z.interaction_mode = InteractiveMarkerControl.MOVE_AXIS translate_z.orientation.w = 1 translate_z.orientation.x = 0
def createMoveControls(fixed=False): controls = [] ## rotate control 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 if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## rotate control y control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## rotate control z control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## move control 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 if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## move control y control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## move control z control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) return controls
def setup_marker(self, frame="velodyne", name = "capture vehicle", translation=True): int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.name = name int_marker.description = name int_marker.scale = 3 marker_control = InteractiveMarkerControl() marker_control.always_visible = True marker_control.markers.append(self.marker) int_marker.controls.append(marker_control) control = InteractiveMarkerControl() control.name = "rotate_x" control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "rotate_z" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "rotate_y" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) if not translation : #int_marker.pose.position = Point(0,0,0) return int_marker control = InteractiveMarkerControl() control.name = "move_x" control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "move_z" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "move_y" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) return int_marker
def init_marker(self): self.server = InteractiveMarkerServer("control_markers") control_marker = InteractiveMarker() control_marker.header.frame_id = "/world_link" control_marker.name = "cc_marker" move_control = InteractiveMarkerControl() move_control.name = "move_x" move_control.orientation.w = 1 move_control.orientation.x = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "move_y" move_control.orientation.w = 1 move_control.orientation.y = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "move_z" move_control.orientation.w = 1 move_control.orientation.z = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_x" move_control.orientation.w = 1 move_control.orientation.x = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_y" move_control.orientation.w = 1 move_control.orientation.z = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_z" move_control.orientation.w = 1 move_control.orientation.y = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True control_marker.controls.append(menu_control) control_marker.scale = 0.25 self.server.insert(control_marker, self.control_marker_feedback) self.menu_handler = MenuHandler() self.menu_handler.insert("Run grader", callback=self.run_grader_cb) self.menu_handler.apply(self.server, "cc_marker",) redundancy_marker = InteractiveMarker() redundancy_marker.header.frame_id = "/lwr_arm_1_link" redundancy_marker.name = "red_marker" rotate_control = InteractiveMarkerControl() rotate_control.name = "rotate_z" rotate_control.orientation.w = 1 rotate_control.orientation.y = 1 rotate_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS redundancy_marker.controls.append(rotate_control) redundancy_marker.scale = 0.25 self.server.insert(redundancy_marker, self.redundancy_marker_feedback) # 'commit' changes and send to all clients self.server.applyChanges()
def __init__(self): self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() #get this from the frame_tracker parameters if rospy.has_param('cartesian_controller/chain_tip_link'): self.active_frame = rospy.get_param("cartesian_controller/chain_tip_link") else: rospy.logerr("No chain_tip_link specified. Aborting!") sys.exit() if rospy.has_param('cartesian_controller/tracking_frame'): self.tracking_frame = rospy.get_param("cartesian_controller/tracking_frame") else: rospy.logerr("No tracking_frame specified. Aborting!") sys.exit() if rospy.has_param('cartesian_controller/root_frame'): self.root_frame = rospy.get_param("cartesian_controller/root_frame") else: rospy.logerr("No root_frame specified. Setting to 'base_link'!") self.root_frame = "base_link" if rospy.has_param('cartesian_controller/movable_trans'): self.movable_trans = rospy.get_param("cartesian_controller/movable_trans") else: rospy.logerr("No movable_trans specified. Setting True!") self.movable_trans = True if rospy.has_param('cartesian_controller/movable_rot'): self.movable_rot = rospy.get_param("cartesian_controller/movable_rot") else: rospy.logerr("No movable_rot specified. Setting True!") self.movable_rot = True self.tracking = False print "Waiting for StartTrackingServer..." rospy.wait_for_service('frame_tracker/start_tracking') print "...done!" self.start_tracking_client = rospy.ServiceProxy('frame_tracker/start_tracking', SetString) print "Waiting for StopTrackingServer..." rospy.wait_for_service('frame_tracker/stop_tracking') print "...done!" self.stop_tracking_client = rospy.ServiceProxy('frame_tracker/stop_tracking', Empty) self.target_pose = PoseStamped() self.target_pose.header.stamp = rospy.Time.now() self.target_pose.header.frame_id = self.root_frame self.target_pose.pose.orientation.w = 1.0 ##give tf_listener some time to fill cache #try: #rospy.sleep(1.0) #except rospy.ROSInterruptException as e: ##print "ROSInterruptException" #pass transform_available = False while not transform_available: try: (trans,rot) = self.listener.lookupTransform(self.root_frame, self.active_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): #rospy.logwarn("Waiting for transform...") try: rospy.sleep(0.1) except rospy.ROSInterruptException as e: #print "ROSInterruptException" pass continue transform_available = True self.target_pose.pose.position.x = trans[0] self.target_pose.pose.position.y = trans[1] self.target_pose.pose.position.z = trans[2] self.target_pose.pose.orientation.x = rot[0] self.target_pose.pose.orientation.y = rot[1] self.target_pose.pose.orientation.z = rot[2] self.target_pose.pose.orientation.w = rot[3] self.ia_server = InteractiveMarkerServer("marker_server") self.int_marker = InteractiveMarker() self.int_marker.header.frame_id = self.root_frame self.int_marker.pose = self.target_pose.pose self.int_marker.name = "interactive_target" self.int_marker.description = self.tracking_frame self.int_marker.scale = 0.8 # create a grey box marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.1 box_marker.scale.y = 0.1 box_marker.scale.z = 0.1 box_marker.color.r = 0.0 box_marker.color.g = 0.5 box_marker.color.b = 0.5 box_marker.color.a = 1.0 control_3d = InteractiveMarkerControl() control_3d.always_visible = True control_3d.name = "move_rotate_3D" control_3d.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control_3d.markers.append( box_marker ) self.int_marker.controls.append(control_3d) control = InteractiveMarkerControl() control.always_visible = True control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 if(self.movable_trans): control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.int_marker.controls.append(deepcopy(control)) control.name = "move_y" control.orientation.x = 0 control.orientation.y = 1 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.int_marker.controls.append(deepcopy(control)) control.name = "move_z" control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.int_marker.controls.append(deepcopy(control)) if(self.movable_rot): 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 self.int_marker.controls.append(deepcopy(control)) control.name = "rotate_y" control.orientation.x = 0 control.orientation.y = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS self.int_marker.controls.append(deepcopy(control)) control.name = "rotate_z" control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS self.int_marker.controls.append(deepcopy(control)) self.ia_server.insert(self.int_marker, self.marker_fb) #create menu self.menu_handler = MenuHandler() self.menu_handler.insert( "StartTracking", callback=self.start_tracking ) self.menu_handler.insert( "StopTracking", callback=self.stop_tracking ) self.menu_handler.insert( "ResetTracking", callback=self.reset_tracking ) self.int_marker_menu = InteractiveMarker() self.int_marker_menu.header.frame_id = self.root_frame self.int_marker_menu.name = "marker_menu" self.int_marker_menu.description = rospy.get_namespace() self.int_marker_menu.scale = 1.0 self.int_marker_menu.pose.position.z = 1.2 control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.MENU control.name = "menu_control" control.description= "InteractiveTargetMenu" self.int_marker_menu.controls.append(copy.deepcopy(control)) self.ia_server.insert(self.int_marker_menu, self.menu_fb) self.menu_handler.apply( self.ia_server, self.int_marker_menu.name ) self.ia_server.applyChanges()
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 make6DofMarker( fixed, interaction_mode, position, show_6dof = False): global parentFrame, frame_name int_marker = InteractiveMarker() int_marker.header.frame_id = parentFrame int_marker.pose.position = position int_marker.scale = 0.5 int_marker.name = frame_name int_marker.description = "Place a frame with 6-DOF!" # insert a box makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = interaction_mode if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D : "MOVE_3D", InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D", InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" } int_marker.name += "_" + control_modes_dict[interaction_mode] int_marker.description = "3D Control" if show_6dof: int_marker.description += " + 6-DOF controls" int_marker.description += "\n" + control_modes_dict[interaction_mode] if show_6dof: control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback) menu_handler.apply( server, int_marker.name )
def __init__(self): #self.client = actionlib.SimpleActionClient('lookat_action', cob_lookat_action.msg.LookAtAction) #print "Waiting for Server..." ##self.client.wait_for_server() #print "...done!" print "Waiting for StartTrackingServer..." rospy.wait_for_service('/lookat_controller/start_tracking') print "...done!" self.start_tracking_client = rospy.ServiceProxy('/lookat_controller/start_tracking', SetString) print "Waiting for StopTrackingServer..." rospy.wait_for_service('/lookat_controller/stop_tracking') print "...done!" self.stop_tracking_client = rospy.ServiceProxy('/lookat_controller/stop_tracking', Empty) self.target_pose = PoseStamped() self.target_pose.header.stamp = rospy.Time.now() self.target_pose.header.frame_id = "base_link" self.target_pose.pose.orientation.w = 1.0 #self.viz_pub = rospy.Publisher('lookat_target', PoseStamped) self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() transform_available = False while not transform_available: try: (trans,rot) = self.listener.lookupTransform('/base_link', '/odometry_monitor_target', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn("Waiting for transform...") rospy.sleep(0.5) continue transform_available = True self.target_pose.pose.position.x = trans[0] self.target_pose.pose.position.y = trans[1] self.target_pose.pose.position.z = trans[2] self.ia_server = InteractiveMarkerServer("marker_server") self.int_marker = InteractiveMarker() self.int_marker.header.frame_id = "base_link" self.int_marker.pose = self.target_pose.pose self.int_marker.name = "lookat_target" self.int_marker.description = "virtual lookat target" self.int_marker.scale = 0.5 # create a grey box marker #box_marker = Marker() #box_marker.type = Marker.CUBE #box_marker.scale.x = 0.1 #box_marker.scale.y = 0.1 #box_marker.scale.z = 0.1 #box_marker.color.r = 0.0 #box_marker.color.g = 0.5 #box_marker.color.b = 0.5 #box_marker.color.a = 1.0 #box_control = InteractiveMarkerControl() #box_control.always_visible = True #box_control.markers.append( box_marker ) #self.int_marker.controls.append(box_control) #control = InteractiveMarkerControl() #control.always_visible = True #control.orientation.w = 1 #control.orientation.x = 1 #control.orientation.y = 0 #control.orientation.z = 0 #control.name = "move_3D" #control.interaction_mode = InteractiveMarkerControl.MOVE_3D #self.int_marker.controls.append(deepcopy(control)) #control.name = "move_x" #control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS #self.int_marker.controls.append(deepcopy(control)) #control.name = "rotate_x" #control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS #self.int_marker.controls.append(deepcopy(control)) #control.name = "move_y" #control.orientation.x = 0 #control.orientation.y = 1 #control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS #self.int_marker.controls.append(deepcopy(control)) #control.name = "rotate_y" #control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS #self.int_marker.controls.append(deepcopy(control)) #control.name = "move_z" #control.orientation.y = 0 #control.orientation.z = 1 #control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS #self.int_marker.controls.append(deepcopy(control)) #control.name = "rotate_z" #control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS #self.int_marker.controls.append(deepcopy(control)) # #self.ia_server.insert(self.int_marker, self.marker_fb) #create menu self.menu_handler = MenuHandler() #self.menu_handler.insert( "Lookat", callback=self.lookat ) self.menu_handler.insert( "StartTracking", callback=self.start_tracking ) self.menu_handler.insert( "StopTracking", callback=self.stop_tracking ) self.int_marker_menu = InteractiveMarker() self.int_marker_menu.header.frame_id = "base_link" self.int_marker_menu.name = "lookat_menu" self.int_marker_menu.description = "Menu" self.int_marker_menu.scale = 1.0 self.int_marker_menu.pose.position.z = 1.2 control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.MENU control.description="Lookat" control.name = "menu_only_control" self.int_marker_menu.controls.append(copy.deepcopy(control)) self.ia_server.insert(self.int_marker_menu, self.menu_fb) self.menu_handler.apply( self.ia_server, self.int_marker_menu.name ) self.ia_server.applyChanges()
box_marker.color.b = 0.5 box_marker.color.a = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append( box_marker ) # add the control to the interactive marker int_marker.controls.append( box_control ) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows rotate_control = InteractiveMarkerControl() rotate_control.name = "move_x" rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS # add the control to the interactive marker int_marker.controls.append(rotate_control); # add the interactive marker to our collection & # tell the server to call processFeedback() when feedback arrives for it server.insert(int_marker, processFeedback) # 'commit' changes and send to all clients server.applyChanges() rospy.spin()
def add_6DOF(self, init_position = Point( 0.0, 0.0, 0.0), frame_id = 'map'): marker = InteractiveMarker() marker.header.frame_id = frame_id marker.pose.position = init_position marker.scale = 0.3 marker.name = 'camera_marker' marker.description = 'Camera 6-DOF pose control' # X axis rotation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS marker.controls.append(control) # X axis traslation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS marker.controls.append(control) # Y axis rotation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS marker.controls.append(control) # Y axis traslation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS marker.controls.append(control) # Z axis rotation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS marker.controls.append(control) # Z axis traslation control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS marker.controls.append(control) # Add marker to server self.server.insert(marker, self.marker_feedback) self.server.applyChanges()
def __init__(self): rospy.init_node('revisualizer') self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2) self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2) self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2) self.kill_server = InteractiveMarkerServer("interactive_kill") # text marker # TODO: Clean this up, there should be a way to set all of this inline self.surface_marker = visualization_msgs.Marker() self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING self.surface_marker.color = ColorRGBA(1, 1, 1, 1) self.surface_marker.scale.z = 0.1 self.depth_marker = visualization_msgs.Marker() self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0) self.depth_marker.scale.z = 0.1 # create marker for displaying current battery voltage self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0) self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5) self.voltage_marker = visualization_msgs.Marker() self.voltage_marker.header.frame_id = "base_link" self.voltage_marker.lifetime = rospy.Duration(5) self.voltage_marker.ns = 'sub' self.voltage_marker.id = 22 self.voltage_marker.pose.position.x = -2.0 self.voltage_marker.scale.z = 0.2 self.voltage_marker.color.a = 1 self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING # create an interactive marker to display kill status and change it self.need_kill_update = True self.kill_marker = InteractiveMarker() self.kill_marker.header.frame_id = "base_link" self.kill_marker.pose.position.x = -2.3 self.kill_marker.name = "kill button" kill_status_marker = Marker() kill_status_marker.type = Marker.TEXT_VIEW_FACING kill_status_marker.text = "UNKILLED" kill_status_marker.id = 55 kill_status_marker.scale.z = 0.2 kill_status_marker.color.a = 1.0 kill_button_control = InteractiveMarkerControl() kill_button_control.name = "kill button" kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON kill_button_control.markers.append(kill_status_marker) self.kill_server.insert(self.kill_marker, self.kill_buttton_callback) self.kill_marker.controls.append(kill_button_control) self.killed = False # connect kill marker to kill alarm self.kill_listener = AlarmListener("kill") self.kill_listener.add_callback(self.kill_alarm_callback) self.kill_alarm = AlarmBroadcaster("kill") # distance to bottom self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback) # distance to surface self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback) # battery voltage self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
def make6DofMarker( fixed = False ): int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.scale = 0.05 int_marker.pose.orientation.x = frame_pose.orientation.x int_marker.pose.orientation.y = frame_pose.orientation.y int_marker.pose.orientation.z = frame_pose.orientation.z int_marker.pose.orientation.w = frame_pose.orientation.w int_marker.pose.position.x = frame_pose.position.x int_marker.pose.position.y = frame_pose.position.y int_marker.pose.position.z = frame_pose.position.z int_marker.name = FRAME_ID int_marker.description = "Place the writing surface" # create a grey box marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = SURFACE_WIDTH box_marker.scale.y = SURFACE_HEIGHT box_marker.scale.z = 0.001 box_marker.color.r = 1.0 box_marker.color.g = 1.0 box_marker.color.b = 1.0 box_marker.color.a = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append( writing_surface() ) # add the control to the interactive marker int_marker.controls.append( box_control ) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, 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 make6DofMarker( fixed = False ): int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.scale = 0.05 int_marker.pose.position.x = -0.100988589227 int_marker.pose.position.y = 0.035845387727 int_marker.pose.position.z = 0.266128748655 int_marker.name = "paper_sheet" int_marker.description = "Place the sheet of paper" # create a grey box marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.21 box_marker.scale.y = 0.297 box_marker.scale.z = 0.001 box_marker.color.r = 1.0 box_marker.color.g = 1.0 box_marker.color.b = 1.0 box_marker.color.a = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append( a4_sheet() ) # add the control to the interactive marker int_marker.controls.append( box_control ) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback)
def init_marker(self): self.server = InteractiveMarkerServer("control_markers") control_marker = InteractiveMarker() control_marker.header.frame_id = "/base" control_marker.name = "move_arm_marker" move_control = InteractiveMarkerControl() move_control.name = "move_x" move_control.orientation.w = 1 move_control.orientation.x = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "move_y" move_control.orientation.w = 1 move_control.orientation.y = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "move_z" move_control.orientation.w = 1 move_control.orientation.z = 1 move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_x" move_control.orientation.w = 1 move_control.orientation.x = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_y" move_control.orientation.w = 1 move_control.orientation.z = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) move_control = InteractiveMarkerControl() move_control.name = "rotate_z" move_control.orientation.w = 1 move_control.orientation.y = 1 move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control_marker.controls.append(move_control) menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True box = Marker() box.type = Marker.CUBE box.scale.x = 0.15 box.scale.y = 0.03 box.scale.z = 0.03 box.color.r = 0.5 box.color.g = 0.5 box.color.b = 0.5 box.color.a = 1.0 menu_control.markers.append(box) box2 = deepcopy(box) box2.scale.x = 0.03 box2.scale.z = 0.1 box2.pose.position.z=0.05 menu_control.markers.append(box2) control_marker.controls.append(menu_control) control_marker.scale = 0.25 self.server.insert(control_marker, self.control_marker_feedback) self.menu_handler = MenuHandler() self.menu_handler.insert("Move Arm", callback=self.move_arm_cb) obs_entry = self.menu_handler.insert("Obstacles") self.menu_handler.insert("No Obstacle", callback=self.no_obs_cb, parent=obs_entry) self.menu_handler.insert("Simple Obstacle", callback=self.simple_obs_cb, parent=obs_entry) self.menu_handler.insert("Hard Obstacle", callback=self.complex_obs_cb, parent=obs_entry) self.menu_handler.insert("Super-hard Obstacle", callback=self.super_obs_cb, parent=obs_entry) options_entry = self.menu_handler.insert("Options") self.plot_entry = self.menu_handler.insert("Plot trajectory", parent=options_entry, callback = self.plot_cb) self.menu_handler.setCheckState(self.plot_entry, MenuHandler.UNCHECKED) self.menu_handler.apply(self.server, "move_arm_marker",) self.server.applyChanges() Ttrans = tf.transformations.translation_matrix((0.6,0.2,0.2)) Rtrans = tf.transformations.rotation_matrix(3.14159,(1,0,0)) self.server.setPose("move_arm_marker", convert_to_message(numpy.dot(Ttrans,Rtrans))) self.server.applyChanges()
def make6DofMarker(frame_id, fixed=False, description="Simple 6-DOF Control"): int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id int_marker.scale = 1 int_marker.name = "simple_6dof" int_marker.description = description # insert a box makeBoxControl(int_marker) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def 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)