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 int_marker.pose.position.z = 0.01 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 if self.marker[soma_type] == '3D': control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 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 _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
def create_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 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 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 _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 _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 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 _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 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 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 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 __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 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 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 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]
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 create_menus(self): self.global_menu_handler = MenuHandler() g_art = self.global_menu_handler.insert("Artificial") self.global_menu_handler.insert("Add primitive", parent=g_art, callback=self.global_menu_add_prim_cb) self.global_menu_handler.insert("Clear all", parent=g_art, callback=self.global_menu_clear_all_cb) self.global_menu_handler.insert( "Clear all (permanent)", parent=g_art, callback=self.global_menu_clear_all_perm_cb) self.global_menu_handler.insert("Save all", parent=g_art, callback=self.global_menu_save_all_cb) self.global_menu_handler.insert("Reload", parent=g_art, callback=self.global_menu_reload_cb) g_det = self.global_menu_handler.insert("Detected") self.global_menu_handler.insert("Pause", parent=g_det, callback=self.global_menu_pause_cb) self.global_menu_handler.insert( "Clear all", parent=g_det, callback=self.global_menu_det_clear_all_cb) self.a_obj_menu_handler = MenuHandler() mov = self.a_obj_menu_handler.insert("Moveable", callback=self.menu_moveable_cb) self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED) sc = self.a_obj_menu_handler.insert("Scaleable", callback=self.menu_scaleable_cb) self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED) self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb) self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb) self.d_obj_menu_handler = MenuHandler() self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb) int_marker = InteractiveMarker() int_marker.header.frame_id = self.world_frame int_marker.pose.position.z = 1.2 int_marker.scale = 0.5 int_marker.name = "global_menu" int_marker.description = "Global Menu" control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.BUTTON control.always_visible = True marker = Marker() marker.type = Marker.CUBE marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 0.5 control.markers.append(marker) int_marker.controls.append(control) self.im_server.insert(int_marker, self.ignored_cb) self.global_menu_handler.apply(self.im_server, int_marker.name) self.im_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 init_marker(self): self.server = InteractiveMarkerServer("control_markers") control_marker = InteractiveMarker() control_marker.header.frame_id = self.robot.get_root() 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) global cc_entry, ik_entry, ik_go_entry self.menu_handler = MenuHandler() cc_entry = self.menu_handler.insert("Cartesian Control Mode", callback=self.set_cc_callback) ik_entry = self.menu_handler.insert("Inverse Kinematics Mode", callback=self.set_ik_callback) ik_go_entry = self.menu_handler.insert("IK to Here", callback=self.set_ik_go_callback) self.menu_handler.setCheckState( cc_entry, MenuHandler.CHECKED ) self.menu_handler.setCheckState( ik_entry, MenuHandler.UNCHECKED ) self.menu_handler.setVisible( ik_go_entry, False ) self.menu_handler.apply(self.server, "cc_marker",) (joint_name, first_link) = self.robot.child_map[self.robot.get_root()][0] redundancy_marker = InteractiveMarker() redundancy_marker.header.frame_id = first_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 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 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 make_6dof_marker(self, pose, size, frame_id, base_frame_id, interaction_mode): int_marker = InteractiveMarker() int_marker.header.frame_id = base_frame_id int_marker.pose.position.x = pose[0][0] int_marker.pose.position.y = pose[0][1] int_marker.pose.position.z = pose[0][2] int_marker.pose.orientation.x = pose[1][0] int_marker.pose.orientation.y = pose[1][1] int_marker.pose.orientation.z = pose[1][2] int_marker.pose.orientation.w = pose[1][3] int_marker.scale = 1 int_marker.name = frame_id int_marker.description = frame_id # insert a box self._make_box_control(int_marker, size) int_marker.controls[0].interaction_mode = interaction_mode n = norm([1, 1]) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 1 / n control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 1 / n control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 1 / n control.orientation.z = 0 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 1 / n control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 / n control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 / n control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) self._server.insert(int_marker, self.on_marker_feedback) self._server.applyChanges()
def makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.from_frame int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame # insert a box, well, an arrow self.makeBoxControl(int_marker) int_marker.controls[ 0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.menu_handler.insert("Publish transform", callback=self.processFeedback) self.menu_handler.insert("Stop publishing transform", callback=self.processFeedback) self.server.insert(int_marker, self.processFeedback) self.menu_handler.apply(self.server, int_marker.name)
"If you move the \nequilibrium " "pose the robot will follow it\n" "so be aware of potential collisions") int_marker.pose = marker_pose.pose # run pose publisher rospy.Timer(rospy.Duration(0.005), lambda msg: publisherCallback(msg, arm_id)) # insert a box control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0
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 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 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 QuadMain(isjoy): def make_state_cost(dsys, base, x): weight = base * np.ones((dsys.nX, )) weight[system.get_config('ballx').index] = 50 weight[system.get_config('bally').index] = 50 weight[system.get_config('ballz').index] = 25 weight[system.get_config('quad1bry').index] = 500 weight[system.get_config('quad1brx').index] = 100 weight[system.get_config('quad1ry').index] = 10 weight[system.get_config('quad1rx').index] = 10 weight[system.get_config('quad1rz').index] = 1 weight[system.get_config('quad2bry').index] = 500 weight[system.get_config('quad2brx').index] = 15 weight[system.get_config('quad2ry').index] = 10 weight[system.get_config('quad2rx').index] = 10 weight[system.get_config('quad2rz').index] = 1 return np.diag(weight) def make_input_cost(dsys, base, x): weight = base * np.ones((dsys.nU, )) # weight[system.get_input('x-force').index] = x return np.diag(weight) quad1cm_m = 1.0 quadmotor1_m = 1.0 quad2cm_m = 1.0 quadmotor2_m = 1.0 ball_m = 0.5 dt = 0.01 # timestep set to 10ms # Create a new trep system - 6 generalized coordinates for quadrotor: x,y,z,roll,pitch,yaw system = trep.System() trep.potentials.Gravity(system, name="Gravity") # define ball frame ballz = trep.Frame(system.world_frame, trep.TZ, "ballz") bally = trep.Frame(ballz, trep.TY, "bally") ballx = trep.Frame(bally, trep.TX, "ballx") ballx.set_mass(ball_m) # define quadrotor 1 frame quad1bry = trep.Frame(ballx, trep.RY, "quad1bry") quad1brx = trep.Frame(quad1bry, trep.RX, "quad1brx") quad1cm = trep.Frame(quad1brx, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 0, 2)), "quad1cm") quad1ry = trep.Frame(quad1cm, trep.RY, "quad1ry") quad1rx = trep.Frame(quad1ry, trep.RX, "quad1rx") quad1rz = trep.Frame(quad1rx, trep.RZ, "quad1rz") quad1cm.set_mass(quad1cm_m) # central point mass # define quadrotor 1 motor positions and mass quad1m1 = trep.Frame(quad1rz, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 0, 0)), "quad1m1") quad1m1.set_mass(quadmotor1_m) quad1m2 = trep.Frame(quad1rz, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (-1, 0, 0)), "quad1m2") quad1m2.set_mass(quadmotor1_m) quad1m3 = trep.Frame(quad1rz, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 1, 0)), "quad1m3") quad1m3.set_mass(quadmotor1_m) quad1m4 = trep.Frame(quad1rz, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, -1, 0)), "quad1m4") quad1m4.set_mass(quadmotor1_m) # set four thrust vectors with inputs u1,u2,u3,u4 trep.forces.BodyWrench(system, quad1m1, (0, 0, 'u1q1', 0, 0, 0), name='quad1w1') trep.forces.BodyWrench(system, quad1m2, (0, 0, 'u2q1', 0, 0, 0), name='quad1w2') trep.forces.BodyWrench(system, quad1m3, (0, 0, 'u3q1', 0, 0, 0), name='quad1w3') trep.forces.BodyWrench(system, quad1m4, (0, 0, 'u4q1', 0, 0, 0), name='quad1w4') # define quadrotor 2 frame quad2bry = trep.Frame(ballx, trep.RY, "quad2bry") quad2brx = trep.Frame(quad2bry, trep.RX, "quad2brx") quad2cm = trep.Frame(quad2brx, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 0, 2)), "quad2cm") quad2ry = trep.Frame(quad2cm, trep.RY, "quad2ry") quad2rx = trep.Frame(quad2ry, trep.RX, "quad2rx") quad2rz = trep.Frame(quad2rx, trep.RZ, "quad2rz") quad2cm.set_mass(quad2cm_m) # central point mass # define quadrotor 2 motor positions and mass quad2m1 = trep.Frame(quad2rz, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 0, 0)), "quad2m1") quad2m1.set_mass(quadmotor2_m) quad2m2 = trep.Frame(quad2rz, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (-1, 0, 0)), "quad2m2") quad2m2.set_mass(quadmotor2_m) quad2m3 = trep.Frame(quad2rz, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 1, 0)), "quad2m3") quad2m3.set_mass(quadmotor2_m) quad2m4 = trep.Frame(quad2rz, trep.CONST_SE3, ((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, -1, 0)), "quad2m4") quad2m4.set_mass(quadmotor2_m) # set four thrust vectors with inputs u1,u2,u3,u4 trep.forces.BodyWrench(system, quad2m1, (0, 0, 'u1q2', 0, 0, 0), name='quad2w1') trep.forces.BodyWrench(system, quad2m2, (0, 0, 'u2q2', 0, 0, 0), name='quad2w2') trep.forces.BodyWrench(system, quad2m3, (0, 0, 'u3q2', 0, 0, 0), name='quad2w3') trep.forces.BodyWrench(system, quad2m4, (0, 0, 'u4q2', 0, 0, 0), name='quad2w4') # set quadrotor initial altitude at 5m system.get_config("ballz").q = 0.0 system.get_config("quad1bry").q = (math.pi / 2 - math.acos(1.5 / 2.0)) system.get_config("quad2bry").q = -(math.pi / 2 - math.acos(1.5 / 2.0)) horzf = 0.5 * ball_m * 9.8 * math.tan((math.pi / 2 - math.acos(1.5 / 2.0))) vertf = (0.5 * ball_m + quad1cm_m + 4 * quadmotor1_m) * 9.8 quad1ang = math.atan(horzf / ((quad1cm_m + 4 * quadmotor1_m) * 9.8)) system.get_config( "quad1ry").q = -(math.pi / 2 - math.acos(1.5 / 2.0)) + quad1ang system.get_config("quad2ry").q = (math.pi / 2 - math.acos(1.5 / 2.0)) - quad1ang # Now we'll extract the current configuration into a tuple to use as # initial conditions for a variational integrator. q0 = system.q # Create the discrete system mvi = trep.MidpointVI(system) t = np.arange(0.0, 10.0, 0.01) dsys = discopt.DSystem(mvi, t) # Generate cost function Qcost = make_state_cost(dsys, 1, 0.00) Rcost = make_input_cost(dsys, 0.01, 0.01) totuf = math.sqrt(math.pow(horzf, 2) + math.pow(vertf, 2)) u0 = np.array([ totuf / 4, totuf / 4, totuf / 4, totuf / 4, totuf / 4, totuf / 4, totuf / 4, totuf / 4 ]) u = tuple(u0) mvi.initialize_from_configs(0.0, q0, dt, q0) pref = mvi.p2 Uint = np.zeros((len(t) - 1, system.nu)) qd = np.zeros((len(t), system.nQ)) pd = np.zeros((len(t), system.nQ)) for i, t in enumerate(t[:-1]): Uint[i, :] = u0 qd[i, :] = q0 pd[i, :] = pref qd[len(qd) - 1, :] = q0 pd[len(pd) - 1, :] = pref Qk = lambda k: Qcost (X, U) = dsys.build_trajectory(Q=qd, p=pd, u=Uint) Kstab = dsys.calc_feedback_controller(X, U, Qk) #print Kstab[0] # Create and initialize the variational integrator) u = tuple(u0) #mvi.initialize_from_configs(0.0, q0, dt, q0) # These are our simulation variables. q = mvi.q2 p = mvi.p2 pref = p t = mvi.t2 # start ROS node to broadcast transforms to rviz rospy.init_node('quad_simulator') broadcaster = tf.TransformBroadcaster() pub = rospy.Publisher('config', Float32MultiArray, queue_size=2) statepub = rospy.Publisher('joint_states', JointState, queue_size=2) jointstates = JointState() configs = Float32MultiArray() if isjoy == True: markerpub = rospy.Publisher('refmark', Marker, queue_size=2) refmark = Marker() if isjoy == False: ############### NEW ################################33 # create an interactive marker server on the topic namespace simple_marker server = InteractiveMarkerServer("simple_marker") # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/world" int_marker.name = "my_marker" int_marker.description = "Simple 1-DOF Control" int_marker.pose.position.z = 2.0 # create a grey box marker box_marker = Marker() box_marker.type = Marker.SPHERE box_marker.scale.x = 0.15 box_marker.scale.y = 0.15 box_marker.scale.z = 0.15 box_marker.color.r = 0.0 box_marker.color.g = 1.0 box_marker.color.b = 0.0 box_marker.color.a = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.orientation.w = 1 box_control.orientation.x = 0 box_control.orientation.y = 1 box_control.orientation.z = 0 box_control.name = "rotate_x" box_control.name = "move_x" box_control.always_visible = True box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE int_marker.controls.append(box_control) box_control = InteractiveMarkerControl() box_control.orientation.w = 1 box_control.orientation.x = 0 box_control.orientation.y = 1 box_control.orientation.z = 0 box_control.name = "rotate_x" box_control.name = "move_x" box_control.always_visible = True box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE box_control.markers.append(box_marker) int_marker.controls.append(box_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() ############### END NEW ############################### if isjoy == True: # subscribe to joystick topic from joy_node rospy.Subscriber("joy", Joy, joycall) r = rospy.Rate(100) # simulation rate set to 100Hz # Simulator Loop while not rospy.is_shutdown(): # reset simulator if trigger pressed if buttons[0] == 1: mvi.initialize_from_configs(0.0, q0, dt, q0) qref = [0, axis[1], axis[0], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] u = tuple(u0 - np.dot(Kstab[0], np.append(q - q0 - qref, p - pref))) # advance simluation one timestep using trep VI try: mvi.step(mvi.t2 + dt, u) except trep.ConvergenceError: print 'Trep simulation error - system resetting...' rospy.sleep(2.) mvi.initialize_from_configs(0.0, q0, dt, q0) q = mvi.q2 p = mvi.p2 t = mvi.t2 configs.data = tuple(q) + u if isjoy == True: refmark.header.frame_id = "/world" refmark.header.stamp = rospy.Time.now() refmark.type = 2 refmark.scale.x = 0.15 refmark.scale.y = 0.15 refmark.scale.z = 0.15 refmark.color.r = 0.0 refmark.color.g = 1.0 refmark.color.b = 0.0 refmark.color.a = 1.0 refmark.pose.position.y = axis[1] refmark.pose.position.x = axis[0] refmark.pose.position.z = 2.0 jointstates.header.stamp = rospy.Time.now() jointstates.name = [ "quad1bry", "quad1brx", "quad1ry", "quad1rx", "quad1rz", "quad2bry", "quad2brx", "quad2ry", "quad2rx", "quad2rz" ] jointstates.position = [ q[3], q[4], q[5], q[6], q[7], q[8], q[9], q[10], q[11], q[12] ] jointstates.velocity = [ system.configs[3].dq, system.configs[4].dq, system.configs[5].dq, system.configs[6].dq, system.configs[7].dq, system.configs[8].dq, system.configs[9].dq, system.configs[10].dq, system.configs[11].dq, system.configs[12].dq ] # send transform data to rviz broadcaster.sendTransform( (q[2], q[1], q[0] + 2), tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0), rospy.Time.now(), "ball", "world") statepub.publish(jointstates) pub.publish(configs) if isjoy == True: markerpub.publish(refmark) r.sleep()
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 create_interactive_6dof(name, color=(1,0,0,1), frame_id='base_link', transform=None, scale=0.05): """ Create a 6-DoF interactive marker Parameters ---------- name: string Marker name color: array_like Marker color is a 4 elements iterable to be used as RGBA color frame_id: str Reference frame of the marker frame. This a frame that must exist in TF transform: array_like Homogenous transformation (4x4) of the marker with respect to the reference frame ``frame_id`` scale: float Scale of the marker applied before the position/orientation. Returns ---------- int_marker: visualization_msgs.InteractiveMarker The 6-DoF interactive marker """ if transform is None: transform = np.eye(4) int_marker = InteractiveMarker() int_marker.header.stamp = get_safe_stamp() int_marker.header.frame_id = frame_id int_marker.name = name int_marker.scale = scale int_marker.pose = criconv.to_pose(transform) # Move X control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = 'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) # Move Y control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = 'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) # Move Z control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = 'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) # Rotate X control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = 'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # Rotate Y control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = 'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # Rotate Z control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = 'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) return int_marker
def 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 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
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) # 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 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 add_grasp_marker(self, frame_id, radius, height, init_position): # Descripcion basica int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id int_marker.pose.position = init_position int_marker.scale = max(radius * 2, height) + 0.02 int_marker.name = "grasp_marker" int_marker.description = "Graspable object" # Geometria marker = Marker() marker.type = Marker.CYLINDER marker.scale.x = radius * 2 marker.scale.y = radius * 2 marker.scale.z = height marker.color.r = random() marker.color.g = random() marker.color.b = random() marker.color.a = 1.0 # Control 6DOF control = InteractiveMarkerControl() control.always_visible = True control.markers.append(marker) int_marker.controls.append(control) int_marker.controls[ 0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D # Control roll control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "roll" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # Movimiento en X control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) # Control yaw control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "yaw" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # Movimiento en Z control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) # Control pitch control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "pitch" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) # Movimiento en y control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) self.server.insert(int_marker, self.process_feedback) self.menu_handler.apply(self.server, int_marker.name)
def make_6dof_gripper(fixed, ps, scale, color, robot_type="pr2", ignore_rotation=False, ignore_x=False, ignore_y=False, ignore_z=False): int_marker = InteractiveMarker() int_marker.header.frame_id = ps.header.frame_id int_marker.pose = ps.pose int_marker.scale = scale int_marker.name = 'gripper_6dof' control = InteractiveMarkerControl() control.always_visible = True control.name = 'pr2_gripper_control' if robot_type == "pr2": control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7], control=control) int_marker.description = 'pr2_gripper_control' elif robot_type == "cody": control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control) int_marker.description = 'cody_ee_control' elif robot_type == "darci": control = make_darci_ee_marker(ps, [1, 1, 1, 0.4], control=control) int_marker.description = 'darci_ee_control' int_marker.controls.append(control) if not ignore_x: control = InteractiveMarkerControl() control.orientation = Quaternion(1, 0, 0, 1) control.name = 'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_y: control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = 'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_z: control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = 'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_rotation: control = InteractiveMarkerControl() control.orientation = Quaternion(1, 0, 0, 1) control.name = 'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = 'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = 'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def _update_viz_core(self): '''Updates visualization after a change''' menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = self._get_ref_name() pose = self.get_pose() if (self.action_step.type == ActionStep.ARM_TARGET): menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif (self.action_step.type == ActionStep.ARM_TRAJECTORY): point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.02, 0.02), header=Header(frame_id=frame_id), color=ColorRGBA(0.8, 0.4, 0.0, 0.8), points=point_list) menu_control.markers.append(main_marker) menu_control.markers.append(ActionStepMarker.make_sphere_marker( self.get_uid() + 2000, self._get_traj_pose(0), frame_id, 0.05)) last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append(ActionStepMarker.make_sphere_marker( self.get_uid() + 3000, self._get_traj_pose(last_index), frame_id, 0.05)) else: rospy.logerr('Non-handled action step type ' + str(self.action_step.type)) ref_frame = World.get_ref_from_name(frame_id) if (ref_frame == ArmState.OBJECT): menu_control.markers.append(Marker(type=Marker.ARROW, id=(1000 + self.get_uid()), lifetime=rospy.Duration(2), scale=Vector3(0.02, 0.03, 0.04), header=Header(frame_id=frame_id), color=ColorRGBA(1.0, 0.8, 0.2, 0.5), points=[pose.position, Point(0, 0, 0)])) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=Vector3(0, 0, 0.03), text='Step' + str(self.step_number), color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)
def publish_gripper(server, pose_stamped, name): """Publishes a marker representing a gripper. Code taken from action_step_marker.py in PR2/PbD. Args: server: An InteractiveMarkerServer pose_stamped: A PoseStamped giving the wrist_roll_link pose. name: string, a unique name for this gripper. """ # Set angle of meshes based on gripper open vs closed. angle = 28 * math.pi / 180.0 # Fully open. STR_MESH_GRIPPER_FOLDER = 'package://pr2_description/meshes/gripper_v0/' STR_GRIPPER_PALM_FILE = STR_MESH_GRIPPER_FOLDER + 'gripper_palm.dae' STR_GRIPPER_FINGER_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger.dae' STR_GRIPPER_FINGERTIP_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger_tip.dae' # Make transforms in preparation for meshes 1, 2, and 3. # NOTE(mbforbes): There are some magic numbers in here that are # used a couple times. Seems like a good candidate for # refactoring to constants, but I think they're more clear if # left in here as (a) they likely won't be changed, and (b) it's # easier to understand the computations with them here. transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) # Create mesh 1 (palm). mesh1 = Marker() mesh1.header.frame_id = pose_stamped.header.frame_id mesh1.mesh_use_embedded_materials = True mesh1.type = Marker.MESH_RESOURCE mesh1.scale.x = 1.0 mesh1.scale.y = 1.0 mesh1.scale.z = 1.0 mesh1.mesh_resource = STR_GRIPPER_PALM_FILE mesh1.pose = pose_stamped.pose # Create mesh 2 (finger). mesh2 = Marker() mesh2.mesh_use_embedded_materials = True mesh2.type = Marker.MESH_RESOURCE mesh2.scale.x = 1.0 mesh2.scale.y = 1.0 mesh2.scale.z = 1.0 mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE mesh2.pose = _get_pose_from_transform(t_proximal) # Create mesh 3 (fingertip). mesh3 = Marker() mesh3.mesh_use_embedded_materials = True mesh3.type = Marker.MESH_RESOURCE mesh3.scale.x = 1.0 mesh3.scale.y = 1.0 mesh3.scale.z = 1.0 mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE mesh3.pose = _get_pose_from_transform(t_distal) # Make transforms in preparation for meshes 4 and 5. quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(math.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) # Create mesh 4 (other finger). mesh4 = Marker() mesh4.mesh_use_embedded_materials = True mesh4.type = Marker.MESH_RESOURCE mesh4.scale.x = 1.0 mesh4.scale.y = 1.0 mesh4.scale.z = 1.0 mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE mesh4.pose = _get_pose_from_transform(t_proximal) # Create mesh 5 (other fingertip). mesh5 = Marker() mesh5.mesh_use_embedded_materials = True mesh5.type = Marker.MESH_RESOURCE mesh5.scale.x = 1.0 mesh5.scale.y = 1.0 mesh5.scale.z = 1.0 mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE mesh5.pose = _get_pose_from_transform(t_distal) # Append all meshes we made. control = InteractiveMarkerControl() control.markers = [mesh1, mesh2, mesh3, mesh4, mesh5] control.interaction_mode = InteractiveMarkerControl.NONE interactive_marker = InteractiveMarker() interactive_marker.controls = [control] interactive_marker.header.frame_id = pose_stamped.header.frame_id interactive_marker.pose = pose_stamped.pose interactive_marker.name = name server.insert(interactive_marker) server.applyChanges()
def 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 _update_viz_core(self): """Updates visualization after a change.""" # Create a new IM control. menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True # Multiplex marker types added based on action step type. if self.action_step.type == ActionStep.ARM_TARGET: # Handle "normal" steps (saved poses). menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # Handle trajectories. # First, get all trajectory positions. point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) # Add a main maker for all points in the trajectory (sphere # list). # NOTE(jstn): This will not work anymore, trajectories are # unsupported for now. menu_control.markers.append( Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=TRAJ_MARKER_LIFETIME, scale=SCALE_TRAJ_STEP_SPHERES, header=Header(frame_id=frame_id), color=COLOR_TRAJ_STEP_SPHERES, points=point_list)) # Add a marker for the first step in the trajectory. menu_control.markers.append( ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_FIRST, self._get_traj_pose(0), frame_id, TRAJ_ENDPOINT_SCALE)) # Add a marker for the last step in the trajectory. last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append( ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_LAST, self._get_traj_pose(last_index), frame_id, TRAJ_ENDPOINT_SCALE)) else: # Neither "normal" pose nor trajectory; error... rospy.logerr('Non-handled action step type ' + str(self.action_step.type)) # Add an arrow to the relative object, if there is one. base_pose = world.get_absolute_pose(self.get_target()) # base_pose refers to the wrist link, which looks weird in # visualizations, so offset_pose is shifted forward to be in the # "middle" of the gripper offset_pose = ActionStepMarker._offset_pose(base_pose, 1) ref_frame = world.get_ref_from_name(self._get_ref_name()) if ref_frame == ArmState.OBJECT: menu_control.markers.append( Marker(type=Marker.ARROW, id=(ID_OFFSET_REF_ARROW + self.get_uid()), scale=SCALE_OBJ_REF_ARROW, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ_REF_ARROW, points=[ offset_pose.position, self.get_target().refFrameLandmark.pose.position ])) # Make and add the text for this step ('Step X'). text_pos = Point() text_pos.x = offset_pose.position.x text_pos.y = offset_pose.position.y text_pos.z = offset_pose.position.z + TEXT_Z_OFFSET menu_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=SCALE_STEP_TEXT, text='Step ' + str(self.step_number), color=COLOR_STEP_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) # Make and add interactive marker. int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = BASE_LINK int_marker.pose = offset_pose int_marker.scale = INT_MARKER_SCALE self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)