def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"): marker = Marker() marker.mesh_resource = resource; marker.header.frame_id = frame_id marker.type = marker.MESH_RESOURCE marker.scale.x = scale*scales[0] marker.scale.y = scale*scales[1] marker.scale.z = scale*scales[2] marker.color.a = rgba[3] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] obj_control = InteractiveMarkerControl() obj_control.always_visible = True obj_control.markers.append( marker ) return obj_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 update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker' int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb)
def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str( pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker' int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb)
def 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_def(p, color=(0.5, 0.5, 0.5), desc=None): im = InteractiveMarker() im.header.frame_id = p.pose.header.frame_id im.pose = p.pose.pose im.name = p.name if desc is None: im.description = p.name im.scale = 1.2 * max(p.bbox.dimensions) marker = Marker() marker.type = Marker.CUBE marker.scale.x = p.bbox.dimensions[0] marker.scale.y = p.bbox.dimensions[1] marker.scale.z = p.bbox.dimensions[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = 1.0 control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = InteractiveMarkerControl.BUTTON control.markers.append(marker) im.controls.append(control) return im
def make_marker(self): self.int_marker = InteractiveMarker() self.int_marker.header.frame_id = "map" self.int_marker.pose = self.pose self.int_marker.scale = 1 self.int_marker.name = self.marker_name control = InteractiveMarkerControl() control.orientation.w = math.sqrt(2) / 2 control.orientation.x = 0 control.orientation.y = math.sqrt(2) / 2 control.orientation.z = 0 control.interaction_mode = self.interaction_mode self.int_marker.controls.append(copy.deepcopy(control)) # make a box which also moves in the plane markers = self.make_individual_markers(self.int_marker) for marker in markers: control.markers.append(marker) control.always_visible = True self.int_marker.controls.append(control) # we want to use our special callback function self.server.insert(self.int_marker, self.feedback)
def MakeMuneObject(self, MenuName, MenuPose): MenuInteractiveMarker = InteractiveMarker() MenuInteractiveMarker.name = MenuName MenuInteractiveMarker.header.frame_id = self.frame_id MenuInteractiveMarker.pose.position.z += self.MenuHight MenuInteractiveMarker.scale = self.MenuScale MenuControl = InteractiveMarkerControl() MenuControl.interaction_mode = InteractiveMarkerControl.MENU MenuControl.always_visible = False MenuMarker = Marker() MenuMarker.type = Marker.ARROW MenuMarker.scale.x = MenuInteractiveMarker.scale * 2 MenuMarker.scale.y = MenuInteractiveMarker.scale * 0.45 MenuMarker.scale.z = MenuInteractiveMarker.scale * 0.45 MenuMarker.color.r = 0.5 MenuMarker.color.g = 0.5 MenuMarker.color.b = 0.5 MenuMarker.color.a = 1.0 MenuMarker.pose = MenuPose MenuControl.markers.append(MenuMarker) MenuInteractiveMarker.controls.append(MenuControl) #print '###################MenuInteractiveMarker info:\n', MenuInteractiveMarker self.server.insert(MenuInteractiveMarker) rospy.loginfo('insert Menu Marker Object')
def main(): rospy.init_node('interactive_marker_node') server = InteractiveMarkerServer("simple_marker") # create an interative marker int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.name = "my_marker" int_marker.description = "Simple Click Control" int_marker.pose.position.x = 1 int_marker.pose.orientation.w = 1 # create a cube for the interactive marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.pose.orientation.w = 1 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 # create a control for the interactive marker button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True button_control.markers.append(box_marker) int_marker.controls.append(button_control) server.insert(int_marker, handle_viz_input) server.applyChanges() rospy.spin()
def create_marker(path_msg, color_msg, description, path_id, width=0.1, delta_z=0.1): int_marker = InteractiveMarker() int_marker.header.frame_id = path_msg.header.frame_id int_marker.name = str(path_id) int_marker.description = "Path {0}".format(path_id) # line_marker = Marker() # line_marker.type = Marker.LINE_STRIP # line_marker.scale.x = width # line_marker.color = color_msg # line_marker.points = [p.pose.position for p in path_msg.poses] # for point in line_marker.points: # point.z += delta_z control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = InteractiveMarkerControl.MENU # control.markers.append(line_marker) points = [node(pose, delta_z) for pose in path_msg.poses] for p1, p2 in zip(points[:-1], points[1:]): control.markers.append(cylinder_between(p1, p2, color_msg, width)) for p in points: control.markers.append(sphere_at(p, color_msg, width)) int_marker.controls.append(copy.deepcopy(control)) menu_handler = MenuHandler() # put all the information in the main menu #d = menu_handler.insert("Description") for line in description: menu_handler.insert(line)#, parent=d) return menu_handler, int_marker
def create_interactive_mesh(name, resource, color=(1, 0, 0, 1), frame_id='base_link', transform=None, scale=1): if transform is None: transform = np.eye(4) # Start with the interactive control int_marker = create_interactive_6dof(name, color, frame_id, transform) # Mesh control control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = InteractiveMarkerControl.NONE # Mesh marker marker = Marker() marker.type = Marker.MESH_RESOURCE marker.scale = Vector3(*[scale for _ in range(3)]) marker.color = ColorRGBA(*color) marker.action = Marker.ADD marker.mesh_resource = resource # Add mesh control control.markers.append(marker) int_marker.controls.append(control) return int_marker
def createCubeMarker(offset=(0,0,0), marker_id = 0, rgba=(1,0,0,1), orientation=(0,0,0,1), scale=(0.1,0.1,0.1), frame_id="/map"): marker = Marker() marker.header.frame_id = frame_id marker.type = marker.CUBE marker.id = marker_id marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.a = rgba[3] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] obj_control = InteractiveMarkerControl() obj_control.always_visible = True obj_control.markers.append( marker ) return obj_control
def make(self): int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.name = self._name int_marker.description = self._name int_marker.pose.position.x = self._x int_marker.pose.position.y = self._y int_marker.pose.orientation.w = 1 box_marker = Marker() box_marker.type = Marker.CUBE box_marker.pose.orientation.w = 1 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 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True button_control.markers.append(box_marker) int_marker.controls.append(button_control) self._server.insert(int_marker, self.handle_viz_input) self._server.applyChanges()
def MessControl(self, unit, mode): control=InteractiveMarkerControl() control.orientation.w = 1 control.orientation.y = 1 control.interaction_mode= mode control.always_visible=True control.markers.append(copy.deepcopy(unit))
def _get_surface_marker(pose, dimensions): ''' Function that generates a surface marker''' int_marker = InteractiveMarker() int_marker.name = 'surface' int_marker.header.frame_id = 'base_link' int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=2000, lifetime=rospy.Duration(2), scale=dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.8, 0.0, 0.4, 0.4), pose=pose) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1))) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker
def createCubeMarker(offset=(0,0,0), marker_id = 0, rgba=(1,0,0,1), orientation=(0,0,0,1), scale=(0.1,0.1,0.1), frame_id="/map", ns = ''): marker = Marker() marker.header.frame_id = frame_id marker.type = marker.CUBE marker.id = marker_id marker.ns = ns marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.a = rgba[3] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] obj_control = InteractiveMarkerControl() obj_control.always_visible = True obj_control.markers.append( marker ) return obj_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 _get_object_marker(self, index, mesh=None): '''Generate a marker for world objects''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.2, 0.8, 0.0, 0.6), pose=World.objects[index].object.pose) if (mesh != None): object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = (World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06) button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) return int_marker
def make_box_control(msg): control = InteractiveMarkerControl() control.always_visible = True control.orientation.w = 1 control.markers.append(make_box(msg)) msg.controls.append(control) return control
def MessControl(self, unit, mode): control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.y = 1 control.interaction_mode = mode control.always_visible = True control.markers.append(copy.deepcopy(unit))
def makeHumanTagMarker(self, position, name): """ Make coordinates and control for tag :param: position of tag :param: name for tag :returns: """ int_marker = InteractiveMarker() int_marker.pose.position = position int_marker.scale = 1 int_marker.name = name int_marker.description = name self.makeBoxControlHumanTag(int_marker) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE int_marker.controls.append(copy.deepcopy(control)) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) server.insert(int_marker, self.processFeedback)
def start(self): # create blue cube marker obj_marker = Marker() obj_marker.type = Marker.CUBE obj_marker.pose.orientation.w = 1 obj_marker.scale.x = 0.06 obj_marker.scale.y = 0.06 obj_marker.scale.z = 0.06 obj_marker.color.r = 0.09 obj_marker.color.g = 0.97 obj_marker.color.b = 2.39 obj_marker.color.a = 1.0 obj_control = InteractiveMarkerControl() obj_control.interaction_mode = InteractiveMarkerControl.MENU obj_control.always_visible = True obj_control.markers.append(obj_marker) obj_im = InteractiveMarker() obj_im.name = 'object' obj_im.header.frame_id = 'base_link' obj_im.pose.position.x = 0.7 obj_im.pose.position.y = 0 obj_im.pose.position.z = 0.5 obj_im.pose.orientation.w = 1 obj_im.scale = 0.3 obj_im.menu_entries.append(menu_entry(1, 'Pick from front')) obj_im.menu_entries.append(menu_entry(2, 'Open gripper')) obj_im.controls.append(obj_control) obj_im.controls.extend(six_dof_controls()) self._im_server.insert(obj_im, feedback_cb=self.handle_feedback) self._im_server.applyChanges() self.update_gripper()
def make_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 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 getMarkersFromPose(poseStamped): int_marker = InteractiveMarker() int_marker.header.frame_id = poseStamped.header.frame_id int_marker.pose = poseStamped.pose int_control = InteractiveMarkerControl() int_control.markers = createMarker() int_marker.controls.append(int_control) return int_marker
def 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 addControl(self, direction, control_type): control = InteractiveMarkerControl() quat = exo.KDLFrame([0, 0, 0] + direction + [1]).get_quaternion() control.orientation.x = quat[0] control.orientation.y = quat[1] control.orientation.z = quat[2] control.orientation.w = quat[3] control.interaction_mode = control_type self.int_marker.controls.append(control)
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 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 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_object_marker(self, index, mesh=None): """Generate and return a marker for world objects. Args: index (int): ID for the new marker. mesh (Mesh, optional): Mesh to use for the marker. Only utilized if not None. Defaults to None. Returns: InteractiveMarker """ int_marker = InteractiveMarker() int_marker.name = self._objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = self._objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=index, lifetime=MARKER_DURATION, scale=self._objects[index].object.dimensions, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ, pose=self._objects[index].object.pose) self.create_sides(index) if mesh is not None: object_marker = _get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) else: for item in range((6 * (index)), (6 * (index)) + 6): rospy.loginfo("Item: " + str(item) + "\nlen mark cont: " + str(len(self._marker_controllers))) int_marker.controls.append(self._marker_controllers[item]) text_pos = Point() text_pos.x = self._objects[index].object.pose.position.x text_pos.y = self._objects[index].object.pose.position.y text_pos.z = (self._objects[index].object.pose.position.z + self._objects[index].object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z) button_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=SCALE_TEXT, text=int_marker.name, color=COLOR_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) #rospy.loginfo("Int_marker controls: " + str(int_marker.controls)) return int_marker
def __init__(self, frame_id, name, description, is_manager=False, speed=0.2): InteractiveMarker.__init__(self) marker_scale_x = rospy.get_param('~marker_scale_x', 1.0) marker_scale_y = rospy.get_param('~marker_scale_y', 0.2) marker_scale_z = rospy.get_param('~marker_scale_z', 0.2) print "Marker scales " + str(marker_scale_x) + ", " + str( marker_scale_y) + ", " + str(marker_scale_z) self.header.frame_id = frame_id self.name = name self.description = description self.speed = speed self.marker = Marker() self.marker.type = Marker.ARROW self.marker.scale.x = marker_scale_x self.marker.scale.y = marker_scale_y self.marker.scale.z = marker_scale_z #self.marker.pose.position.z = 0.20 if is_manager: self.marker.color.r = 0.8 self.marker.color.g = 0.0 self.marker.color.b = 0.0 self.marker.color.a = 0.5 else: self.marker.color.r = 0.0 self.marker.color.g = 0.8 self.marker.color.b = 0.0 self.marker.color.a = 0.5 self.marker_control = InteractiveMarkerControl() self.marker_control.always_visible = True self.marker_control.orientation.w = 1 self.marker_control.orientation.x = 0 self.marker_control.orientation.y = 1 self.marker_control.orientation.z = 0 self.marker_control.name = "move_plane" self.marker_control.markers.append(self.marker) self.marker_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE self.controls.append(self.marker_control) self.marker_control2 = InteractiveMarkerControl() self.marker_control2.orientation.w = 1 self.marker_control2.orientation.x = 0 self.marker_control2.orientation.y = 1 self.marker_control2.orientation.z = 0 self.marker_control2.name = "rotate_z" self.marker_control2.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS self.controls.append(self.marker_control2)
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_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 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 _get_object_marker(self, index, mesh=None): '''Generate and return a marker for world objects. Args: index (int): ID for the new marker. mesh (Mesh, optional): Mesh to use for the marker. Only utilized if not None. Defaults to None. Returns: InteractiveMarker ''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=index, lifetime=MARKER_DURATION, scale=World.objects[index].object.dimensions, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ, pose=World.objects[index].object.pose ) if mesh is not None: object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = ( World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z) button_control.markers.append( Marker( type=Marker.TEXT_VIEW_FACING, id=index, scale=SCALE_TEXT, text=int_marker.name, color=COLOR_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)) ) ) int_marker.controls.append(button_control) return int_marker
def 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 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 create_object_marker(self, soma_obj, roi, soma_type, pose): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "map" int_marker.name = soma_obj int_marker.description = soma_type + ' (' + roi + ')' int_marker.pose = pose int_marker.pose.position.z = 0.01 marker = Marker() marker.type = Marker.SPHERE marker.scale.x = 0.25 marker.scale.y = 0.25 marker.scale.z = 0.25 int_marker.pose.position.z = (marker.scale.z / 2) random.seed(soma_type) val = random.random() marker.color.r = r_func(val) marker.color.g = g_func(val) marker.color.b = b_func(val) marker.color.a = 1.0 #marker.pose = pose # 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.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE if self._interactive: int_marker.controls.append(copy.deepcopy(control)) # add the control to the interactive marker int_marker.controls.append(control); # add menu control menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True menu_control.markers.append( marker) #makeBox(int_marker) ) int_marker.controls.append(menu_control) return int_marker
def create_object_marker(self, soma_obj, soma_type, pose): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = soma_obj int_marker.description = "id" + soma_obj int_marker.pose = pose mesh_marker = Marker() mesh_marker.type = Marker.MESH_RESOURCE mesh_marker.scale.x = 1 mesh_marker.scale.y = 1 mesh_marker.scale.z = 1 random.seed(soma_type) val = random.random() mesh_marker.color.r = r_func(val) mesh_marker.color.g = g_func(val) mesh_marker.color.b = b_func(val) mesh_marker.color.a = 1.0 #mesh_marker.pose = pose mesh_marker.mesh_resource = self.mesh[soma_type] # 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.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE if self._interactive: int_marker.controls.append(copy.deepcopy(control)) # add the control to the interactive marker int_marker.controls.append(control); # add menu control menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True menu_control.markers.append( mesh_marker) #makeBox(int_marker) ) int_marker.controls.append(menu_control) return int_marker
def create_im(self, marker, pose, name): # create the new interactive marker int_marker = InteractiveMarker() int_marker.pose = copy.deepcopy(pose) int_marker.header.frame_id = 'base_link' int_marker.name = name # move freely on the X-Y plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE control.markers.append(marker) control.always_visible = True int_marker.controls.append(control) return int_marker
def __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 create_interactive_marker(holder, id, pos, size, color, func): # Make interactive marker for mouse selection int_marker = InteractiveMarker() int_marker.header.frame_id = "base" int_marker.name = "object"+str(id) int_marker.pose.position.x = pos[0] int_marker.pose.position.y = pos[1] int_marker.pose.position.z = pos[2] #color = [1, 0, 0] # Add click control box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.interaction_mode = InteractiveMarkerControl.BUTTON create_shape(box_control, "object", id, pos, size=size, color=color) # add the control to the interactive marker int_marker.controls.append( box_control ) holder.insert(int_marker, lambda x: func(x, color))
def create_roi_marker(self, roi, soma_type, pose, points): #print "POINTS: " + str(points) int_marker = InteractiveMarker() int_marker.header.frame_id = "map" int_marker.name = "ROI-" + roi int_marker.description = roi int_marker.pose = pose marker = Marker() marker.type = Marker.LINE_STRIP marker.scale.x = 0.1 random.seed(soma_type) val = random.random() marker.color.r = r_func(val) marker.color.g = g_func(val) marker.color.b = b_func(val) marker.color.a = 1.0 control = InteractiveMarkerControl() control.always_visible = True control.markers.append( marker ) int_marker.controls.append(control ) marker.points = [] for point in points: p = Point() pose = self._soma_obj_pose[point] p.x = pose.position.x - int_marker.pose.position.x p.y = pose.position.y - int_marker.pose.position.y marker.points.append(p) p = Point() pose = self._soma_obj_pose[points[0]] p.x = pose.position.x - int_marker.pose.position.x p.y = pose.position.y - int_marker.pose.position.y marker.points.append(p) return int_marker
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 createPointMarker(points, colors, offset=(0,0,0), orientation=(0,0,0,1)): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.scale.x = 0.003 marker.scale.y = 0.003 marker.scale.z = 0.003 n = len(points)//3 for i in range(n): p = Point() p.x = points[i*3+0] p.y = points[i*3+1] p.z = points[i*3+2] marker.points.append(p) p = ColorRGBA() p.r = colors[i*3+0] p.g = colors[i*3+1] p.b = colors[i*3+2] p.a = 1 marker.colors.append(p) marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] obj_control = InteractiveMarkerControl() obj_control.always_visible = True obj_control.markers.append( marker ) return obj_control
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_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()
int_marker.name = "my_marker" int_marker.description = "Simple 1-DOF Control" # create a grey box marker 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 # 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);
left_offset = translation_matrix([0.01, 0.02 + foot_margin / 2.0, 0]) right_offset = translation_matrix([0.01, -0.02 - foot_margin / 2.0, 0]) foot_depth = 0.240 foot_width = 0.140 lleg_end_coords = "lleg_end_coords" rleg_end_coords = "rleg_end_coords" # create a grey box marker lleg_marker = makeFootCube(foot_depth, foot_width, left_offset) rleg_marker = makeFootCube(foot_depth, foot_width, right_offset) lleg_marker.color.g = 1.0 rleg_marker.color.r = 1.0 # rleg_marker.pose.position.y = - foot_margin / 2.0 # lleg_marker.pose.position.y = foot_margin / 2.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.extend([lleg_marker, rleg_marker]) # add the control to the interactive marker int_marker.controls.append( box_control ) int_marker.controls.extend(make6DOFControls()) # add the interactive marker to our collection & # tell the server to call processFeedback() when feedback arrives for it server.insert(int_marker, processFeedback) menu_handler.apply(server, int_marker.name)
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]
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__(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 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 _update_viz_core(self): '''Updates visualization after a change''' menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = self._get_ref_name() pose = self.get_pose() if (self.action_step.type == ActionStep.ARM_TARGET): menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.02, 0.02), header=Header(frame_id=frame_id), color=ColorRGBA(0.8, 0.4, 0.0, 0.8), points=point_list) menu_control.markers.append(main_marker) menu_control.markers.append(ActionStepMarker.make_sphere_marker( self.get_uid() + 2000, self._get_traj_pose(0), frame_id, 0.05)) last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append(ActionStepMarker.make_sphere_marker( self.get_uid() + 3000, self._get_traj_pose(last_index), frame_id, 0.05)) else: rospy.logerr('Non-handled action step type ' + str(self.action_step.type)) ref_frame = World.get_ref_from_name(frame_id) if (ref_frame == ArmState.OBJECT): menu_control.markers.append(Marker(type=Marker.ARROW, id=(1000 + self.get_uid()), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.03, 0.04), header=Header(frame_id=frame_id), color=ColorRGBA(1.0, 0.8, 0.2, 0.5), points=[pose.position, Point(0, 0, 0)])) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=Vector3(0, 0, 0.03), text='Step' + str(self.step_number), color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)