def __init__(self, parent_instance_handle, server_name, ik_solver): self.parent_instance_handle = parent_instance_handle self.server = InteractiveMarkerServer(server_name) self.ik_solver = ik_solver self.ik_position_xyz_bounds = [0.01, 0.01, 0.01] self.ik_position_rpy_bounds = [31416.0, 31416.0, 31416.0] # NOTE: This implements position-only IK self._menu_handlers = {} self._menu_cmds = {} self.menu_handler = MenuHandler() del_sub_menu_handle = self.menu_handler.insert("Delete Waypoint") self._menu_cmds['del_wp'] = self.menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback) self.menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback) ins_sub_menu_handle = self.menu_handler.insert("Insert Waypoint") self._menu_cmds['ins_wp_before'] = self.menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback) self._menu_cmds['ins_wp_after'] = self.menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback) self.menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback) self._int_marker_name_list = [] self.markers_created_cnt = 0 self.marker_cnt = 0 self.name_to_marker_dict = {} self.waypoint_to_marker_dict = {} self.name_to_waypoint_dict = {}
def __init__(self): self.goto_pub = rospy.Publisher('/move_base_simple/goal', geometry_msgs.msg.PoseStamped) self.pose_pub = rospy.Publisher('/pose_names', map_annotator.msg.PoseNames, latch=True) rospy.Subscriber('/amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped, self._handle_pose_callback) self.names = {} filename = "/home/team3/catkin_ws/src/cse481c/map_annotator/simstations.pickle" #filename = rospy.get_param('~pose_file') try: with open(filename, "r") as f: self.names = pickle.load(f) except EOFError: self.names = {} except IOError: self.names = {} self.pose = None self.server = InteractiveMarkerServer('simple_marker') self.name_markers = {} self.name_controls = {} pose_message = map_annotator.msg.PoseNames() pose_message.poses = list(self.names.keys()) self.pose_pub.publish(pose_message) print("initiated actuator nodes with", self.names, "from", filename)
def __init__(self, network_file): self.network_file = network_file with open(network_file, 'r') as file_obj: network = yaml.safe_load(file_obj) self.nodes = { node['id']: Node.from_dict(node) for node in network['nodes'] } self.connections = network['connections'] self.node_id_counter = max(self.nodes.keys()) # ros params self.frame = rospy.get_param('frame', 'map') self._topology_pub = rospy.Publisher('/topology', MarkerArray, queue_size=1) self.interactive_marker_server = InteractiveMarkerServer( "two_dof_marker") rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_cb) rospy.Subscriber('~command', String, self.command_cb) rospy.sleep(1.0) self._topology_pub.publish(self.get_marker_array()) rospy.sleep(1.0) for node in self.nodes.values(): self._add_interactive_marker(node.x, node.y, node.id) rospy.loginfo('Initialised')
def __init__(self): # Public attributes if World.tf_listener is None: World.tf_listener = TransformListener() self.surface = None # Private attributes self._lock = threading.Lock() self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer(TOPIC_IM_SERVER) rospy.wait_for_service(SERVICE_BB) self._bb_service = rospy.ServiceProxy( SERVICE_BB, FindClusterBoundingBox) self._object_action_client = actionlib.SimpleActionClient( ACTION_OBJ_DETECTION, UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo( 'Interactive object detection action server has responded.') # Setup other ROS machinery rospy.Subscriber( TOPIC_OBJ_RECOGNITION, GraspableObjectList, self.receive_object_info) rospy.Subscriber(TOPIC_TABLE_SEG, Marker, self.receive_table_marker) # Init self.clear_all_objects()
def __init__(self, from_frame, to_frame, position, orientation): self.server = InteractiveMarkerServer("posestamped_im") o = orientation r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("Publishing transform and PoseStamped from: " + from_frame + " to " + to_frame + " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) + " and orientation " + str(o.x) + " " + str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " + str(r) + " " + str(p) + " " + str(y)) self.menu_handler = MenuHandler() self.from_frame = from_frame self.to_frame = to_frame # Transform broadcaster self.tf_br = tf.TransformBroadcaster() self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) pose = Pose() pose.position = position pose.orientation = orientation self.last_pose = pose self.print_commands(pose) self.server.applyChanges()
def __init__(self): self.server = InteractiveMarkerServer("posestamped_im") self.menu_handler = MenuHandler() # self.pub = rospy.Publisher('/whole_body_kinematic_controler/wrist_right_ft_link_goal', PoseStamped, queue_size=1) # self.pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_7_link_goal', PoseStamped, queue_size=1) self.pub = rospy.Publisher('/pose_to_follow', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) #self.global_frame_id = 'world' self.global_frame_id = 'base_footprint' pose = Pose() pose.position.x = 0.2 # pose.position.y = 0.35 pose.position.y = -0.35 #pose.position.z = 0.1 pose.position.z = 1.1 pose.orientation.w = 1.0 # pose.orientation.x = 0.35762 # pose.orientation.y = 0.50398 # pose.orientation.z = 0.45213 # pose.orientation.w = 0.64319 #self.makeMenuMarker(pose) self.makeGraspIM(pose) self.server.applyChanges()
def __init__(self, rate=30.0, filename=None): # Marker server self.server = InteractiveMarkerServer('camera_marker') # TF broadcaster self.tf = TransformBroadcaster() # Marker pose self.pose_mutex = Lock() self.marker_position = (0.0, 0.0, 0.0) self.marker_orientation = (0.0, 0.0, 0.0, 1.0) # Load init position self.filename = filename if self.filename: with open(self.filename, 'r') as stream: init_data = yaml.load(stream)['init_position'] self.marker_position = (init_data['x'], init_data['y'], init_data['z']) self.marker_orientation = (0.0, 0.0, 0.0, 1.0) # Register shutdown callback rospy.on_shutdown(self.shutdown) # Add marker self.add_6DOF() # Timer for TF broadcaster rospy.Timer(rospy.Duration(1.0 / rate), self.publish_transform)
def __init__(self, root_tips, suffix=u''): """ :param root_tips: list containing root->tip tuple for each interactive marker. :type root_tips: list :param suffix: the marker will be called 'eef_control{}'.format(suffix) :type suffix: str """ self.enable_self_collision = rospy.get_param(u'~enable_self_collision', True) self.giskard = GiskardWrapper() if len(root_tips) > 0: self.roots, self.tips = zip(*root_tips) else: self.roots = [] self.tips = [] self.suffix = suffix self.markers = {} # marker server self.server = InteractiveMarkerServer(u'eef_control{}'.format( self.suffix)) self.menu_handler = MenuHandler() for root, tip in zip(self.roots, self.tips): int_marker = self.make_6dof_marker( InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip) self.server.insert( int_marker, self.process_feedback(self.server, int_marker.name, root, tip, self.giskard, self.enable_self_collision)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def __init__(self, name="interactive_markers"): self.server = InteractiveMarkerServer(name) self.mg = MarkerGenerator() self.mg.type = 1 self.mg.scale = [.25] * 3 self.c = 0 self.markers = {}
def main(args): server = InteractiveMarkerServer("guess_markers") guess = Guess() for i in range(len(guess.items)): server.insert(guess.markers[i], guess.process_feedback) server.applyChanges() rospy.spin()
def __init__(self): rospy.init_node('look_at_marker_node') self._server = InteractiveMarkerServer('look_at_marker_server') self.makeQuadrocopterMarker(Point(3, 0, 1)) self.makeStaticMarker(Point(0, 0, 1)) self._tf_listener = tf.TransformListener() self._head = robot_api.FullBodyLookAt(tf_listener=self._tf_listener) self._look_at_target = None
def __init__(self, marker_name): host = rospy.get_param("mongodb_host") port = rospy.get_param("mongodb_port") self._client = pymongo.MongoClient(host, port) self._traj = dict() self._retrieve_logs(marker_name) self._server = InteractiveMarkerServer(marker_name)
def main(): rospy.init_node('mmmm') # Initializing interactive markers marker_server = InteractiveMarkerServer("simple_server") monitor = InteractiveMarkerMonitor(marker_server) marker_server.applyChanges() rospy.spin()
def __init__(self, setup, world_frame): super(IntCollisionEnv, self).__init__(setup, world_frame) self.im_server = InteractiveMarkerServer(self.NS + "markers") self.check_attached_timer = rospy.Timer(rospy.Duration(0.1), self.check_attached_timer_cb) self.create_menus()
def main(): rospy.init_node('gripper_teleop') arm = Arm() gripper = Gripper() im_server = InteractiveMarkerServer('gripper_im_server') auto_pick_im_server = InteractiveMarkerServer('auto_pick_im_server') teleop = GripperTeleop(arm, gripper, im_server) auto_pick = AutoPickTeleop(arm, gripper, auto_pick_im_server) teleop.start() auto_pick.start() rospy.spin()
def __init__(self, server_ns='interactive_markers'): self.server = InteractiveMarkerServer(server_ns) self._menu_handlers = {} self._menu_cmds = {} self.markers_created_cnt = 0 self.marker_cnt = 0 self._int_marker_name_list = [] self.name_to_marker_dict = {} self.name_to_position_only_flag = {}
def __init__(self): self.transform = None super(InteractivePose2DTF, self).__init__() self.pub_tf = tf2_ros.TransformBroadcaster() self.interact = InteractiveMarkerServer("interactive_markers") self.marker_pose = Pose() self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))
def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() # Code for moving the robots joints switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy( switch_srv_name, SwitchController) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for RIGHT arm...' ) self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for LEFT arm...' ) self.l_traj_action_client.wait_for_server()
def __init__(self, robot, *robotargs): super(PickAndPlaceNode, self).__init__('pp_node') rospy.loginfo("PickAndPlaceNode") _post_perceive_trans = defaultdict(lambda: self._pick) _post_perceive_trans.update({'c': self._calibrate}) _preplace = defaultdict(lambda: self._preplace) self.transition_table = { # If calibration has already happened, allow skipping it 'initial': {'c': self._calibrate, 'q': self._perceive, 's': self._preplace}, 'calibrate': {'q': self._perceive, 'c': self._calibrate}, 'perceive': {'p': self._post_perceive, 'q': self._perceive, 's': self._stop_perceive, 'c': self._calibrate}, 'post_perceive': _post_perceive_trans, 'postpick': {'1': self._level, '2': self._level, '9': self._level}, 'level': _preplace, 'preplace': {'s': self._place}, 'place': {'q': self._perceive, 'c': self._calibrate} } rospy.loginfo("PickAndPlaceNode1") if callable(robot): self.robot = robot(*robotargs) else: self.robot = robot self.robot.level = 1 rospy.loginfo("PickAndPlaceNode2") # Hardcoded place for now self.place_pose = PoseStamped( Header(0, rospy.Time(0), self.robot.base), Pose(Point(0.526025806, 0.4780144, -0.161326153), Quaternion(1, 0, 0, 0))) self.tl = tf.TransformListener() self.num_objects = 0 # Would this work too? Both tf and tf2 have (c) 2008... # self.tf2 = tf2_ros.TransformListener() self.n_objects_sub = rospy.Subscriber("/num_objects", Int64, self.update_num_objects, queue_size=1) self.perception_pub = rospy.Publisher("/perception/enabled", Bool, queue_size=1) self.alignment_pub = rospy.Publisher("/alignment/doit", Bool, queue_size=1) self.br = tf.TransformBroadcaster() rospy.loginfo("PickAndPlaceNode3") self.int_marker_server = InteractiveMarkerServer('int_markers') # Dict to map imarker names and their updated poses self.int_markers = {} # Ideally this call would be in a Factory/Metaclass/Parent self.show_options() self.perceive = False
class AbstractInteractiveMarker(object): def __init__(self, name, X_init, box_scale=0.1): self.name = name self.X = X_init self.marker = createBoxMarker(scale=box_scale) self.interactive_marker = create6DofInteractive('/map', name, name, X_init, self.marker) self.marker_server = InteractiveMarkerServer(name + 'server') self.marker_server.insert(self.interactive_marker, self.markerMoved) self.marker_server.applyChanges() def markerMoved(self, feedback): self.X = transform.fromPose(feedback.pose)
def __init__(self): super(RvizPathServer, self).__init__() rospy.init_node("traversability_rviz_paths_node") self.server = InteractiveMarkerServer("paths") self.paths = {} self.delta_z = rospy.get_param('~offset', 0.15) self.width = rospy.get_param('~width', 0.15) self.pub = rospy.Publisher("selected_path", NavPath, queue_size=1) rospy.Subscriber("paths", Paths, self.updatePaths, queue_size=1) # self.add_marker(test_msg(), 0) # self.server.applyChanges() rospy.spin()
def __init__(self): ''' Create a POIServer ''' # TF bootstrap self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # Radius of interactive marker for POIs self.marker_scale = rospy.get_param("~marker_scale", 0.5) # Create intial empty POI array self.pois = POIArray() # Get the global frame to be used self.global_frame = rospy.get_param("~global_frame", "map") self.pois.header.frame_id = self.global_frame # Create publisher to notify clients of updates and interactive marker server self.pub = rospy.Publisher("points_of_interest", POIArray, queue_size=1, latch=True) self.interactive_marker_server = InteractiveMarkerServer( "points_of_interest") # Load initial POIs from params if rospy.has_param('~initial_pois'): pois = rospy.get_param('~initial_pois') assert isinstance(pois, dict) for key, value in pois.iteritems(): assert type(key) == str assert type(value) == list assert len(value) == 3 name = key position = numpy_to_point(value) self._add_poi(name, position) # Update clients / markers of changes from param self.update() # Create services to add / delete / move a POI self.add_poi_server = rospy.Service('~add', AddPOI, self.add_poi_cb) self.delete_poi_server = rospy.Service('~delete', DeletePOI, self.delete_poi_cb) self.move_poi_service = rospy.Service('~move', MovePOI, self.move_poi_cb) self.save_to_param = rospy.Service("~save_to_param", Trigger, self.save_to_param_cb)
def __init__(self, side_prefix): self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_contoller_name = None l_traj_contoller_name = None if self.side_prefix == 'r': r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient( r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() else: l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient( l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.ik = IK(side_prefix) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix) self._im_server.applyChanges() print self.ik
def __init__(self, whicharm): self.whicharm = whicharm self.robot_state = pr2_control_utilities.RobotState() self.joint_controller = pr2_control_utilities.PR2JointMover( self.robot_state) self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller) self.server = InteractiveMarkerServer("~trajectory_markers") self.tf_listener = self.planner.tf_listener self.visualizer_pub = rospy.Publisher("~trajectory_markers_path", MarkerArray) self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray) rospy.Subscriber("~overwrite_trajectory", PoseArray, self.overwrite_trajectory) rospy.Service("~execute_trajectory", Empty, self.execute_trajectory_srv) # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/base_link" int_marker.pose.position.x = 0.5 int_marker.pose.position.z = 1.0 int_marker.name = "move_" + whicharm + "_arm" int_marker.description = "Move the " + whicharm + " arm" int_marker.scale = 0.2 self.server.insert(int_marker, self.main_callback) # create the main marker shape #color = (1,0,0,1) color = None self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) #add the controls utils.make_6DOF_marker(int_marker) self.int_marker = int_marker self.create_menu() self.server.applyChanges() self.trajectory = PoseArray() self.trajectory.header.frame_id = "/base_link" self.last_gripper_pose = None if whicharm == "right": self.ik_utils = self.planner.right_ik else: self.ik_utils = self.planner.left_ik rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def init(self): # The parameter /robot_description must exist if not rospy.has_param('/robot_description'): rospy.logerr( "Parameter '/robot_description' must exist to continue") sys.exit(1) self.config = loadJSONConfig(self.args['calibration_file']) if self.config is None: sys.exit(1) # loadJSON should tell you why. # self.config = CalibConfig() # ok = self.config.loadJSON(self.args['calibration_file']) # # Exit if it fails to open a valid calibration file # if not ok: # sys.exit(1) # Load the urdf from /robot_description self.urdf = URDF.from_parameter_server() # ok = validateLinks(self.config.world_link, self.config.sensors, self.urdf) # if not ok: # sys.exit(1) print('Number of sensors: ' + str(len(self.config['sensors']))) # Init interaction self.server = InteractiveMarkerServer("interactive_first_guess") self.menu = MenuHandler() self.menu.insert("Save sensors configuration", callback=self.onSaveFirstGuess) self.menu.insert("Reset to initial configuration", callback=self.onReset) # For each node generate an interactive marker. for name, sensor in self.config['sensors'].items(): print(Fore.BLUE + '\nSensor name is ' + name + Style.RESET_ALL) params = { "frame_world": self.config['world_link'], "frame_opt_parent": sensor['parent_link'], "frame_opt_child": sensor['child_link'], "frame_sensor": sensor['link'], "marker_scale": self.args['marker_scale'] } # Append to the list of sensors self.sensors.append(Sensor(name, self.server, self.menu, **params)) self.server.applyChanges()
def __init__(self, side_prefix): if GripperMarkers._tf_listener is None: GripperMarkers._tf_listener = TransformListener() self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers') self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges()
def main(): rospy.init_node('gripper_teleop') wait_for_time() arm = fetch_api.Arm() gripper = fetch_api.Gripper() im_server = InteractiveMarkerServer('gripper_im_server', q_size=10) teleop = GripperTeleop(arm, gripper, im_server) teleop.start() pp_im_server = InteractiveMarkerServer('pick_place_im_server', q_size=10) pick_place = PickPlaceTeleop(arm, gripper, pp_im_server) pick_place.start() rospy.spin()
def __init__(self): if World.tf_listener == None: World.tf_listener = TransformListener() self._lock = threading.Lock() self.surface = None self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer('world_objects') bb_service_name = 'find_cluster_bounding_box' rospy.wait_for_service(bb_service_name) self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox) rospy.Subscriber('interactive_object_recognition_result', GraspableObjectList, self.receive_object_info) self._object_action_client = actionlib.SimpleActionClient( 'object_detection_user_command', UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo('Interactive object detection action ' + 'server has responded.') self.clear_all_objects() # The following is to get the table information rospy.Subscriber('tabletop_segmentation_markers', Marker, self.receive_table_marker) rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.receive_ar_markers) self.marker_dims = Vector3(0.04, 0.04, 0.01)
def main(): global pose_marker_server global nav_server global target_pose_pub global pose_names_pub rospy.init_node('web_teleop_navigation') wait_for_time() pose_marker_server = InteractiveMarkerServer('simple_marker') pose_names_pub = rospy.Publisher('/pose_names', PoseNames, queue_size=10, latch=True) nav_server = NavigationServer() # Create nav_server.loadMarkers() for name in nav_server.pose_names_list: marker = PoseMarker(pose_marker_server, name, nav_server.pose_names_list[name]) message = PoseNames() message.names = nav_server.pose_names_list pose_names_pub.publish(message) user_actions_sub = rospy.Subscriber('/user_actions', UserAction, handle_user_actions) rospy.spin()
def main(): rospy.init_node("rviz_stuff") server = InteractiveMarkerServer("rviz_stuff") cube = assign( Marker(), type=Marker.CUBE, scale=assign(x=0.1, y=0.1, z=0.1), color=assign(r=1, g=0, b=0, a=1), pose=assign(Pose(), position=assign(x=0.1, y=0, z=0)), ) sphere = assign( Marker(), type=Marker.SPHERE, scale=assign(x=0.1, y=0.1, z=0.1), color=assign(r=1, g=0, b=0, a=1), pose=assign(Pose(), position=assign(x=-0.1, y=0.1, z=0)), ) X_WA = assign(Pose(), position=assign(x=1, y=0, z=1), orientation=assign(w=1, x=0, y=0, z=0)) a = SimpleInteractiveMarker(server, "a", X_WA, [cube]) X_WB = assign(Pose(), position=assign(x=1, y=1, z=2), orientation=assign(w=1, x=0, y=0, z=0)) b = SimpleInteractiveMarker(server, "b", X_WB, [cube, sphere]) b.set_pose(assign(Pose(), position=assign(x=-1))) X_WC = Pose() c = SimpleInteractiveMarker(server, "c", X_WC, [sphere]) c.delete() c.delete() rospy.spin()
def main(): rospy.init_node('map_annotator_server') myargv = rospy.myargv() if len(myargv) < 2: print 'Usage: rosrun map_annotator server.py path/to/db' return db_path = myargv[1] wait_for_time() database = PoseDatabase(db_path) list_pub = rospy.Publisher('map_annotator/pose_names', PoseNames, latch=True, queue_size=1) im_server = InteractiveMarkerServer('map_annotator/map_poses') marker_server = PoseMarkers(database, im_server) move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction) server = Server(database, list_pub, marker_server, move_base) user_action_sub = rospy.Subscriber('map_annotator/user_actions', UserAction, server.handle_user_action) rospy.sleep(0.5) marker_server.start() server.start() def handle_shutdown(): pn = PoseNames() list_pub.publish(pn) database.save() rospy.on_shutdown(handle_shutdown) rospy.spin()
def main(): rospy.init_node('map_annotator') server = InteractiveMarkerServer("simple_server") pose_ctrl = PoseController() marker_ctrl = MarkerController(server, pose_ctrl) monitor = MapAnnotatorMonitor(pose_ctrl, marker_ctrl) rospy.spin()
def __init__(self, from_frame, to_frame, position, orientation): self.server = InteractiveMarkerServer("posestamped_im") o = orientation r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("Publishing transform and PoseStamped from: " + from_frame + " to " + to_frame + " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) + " and orientation " + str(o.x) + " " + str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " + str(r) + " " + str(p) + " " + str(y)) self.menu_handler = MenuHandler() self.from_frame = from_frame self.to_frame = to_frame # Transform broadcaster self.tf_br = tf.TransformBroadcaster() self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) pose = Pose() pose.position = position pose.orientation = orientation self.last_pose = pose self.print_commands(pose) self.makeGraspIM(pose) self.server.applyChanges()
def init_int_marker(self): self.ims = InteractiveMarkerServer("simple_marker") self.im = InteractiveMarker() self.im.header.frame_id = "/ned" self.im.name = "my_marker" self.im.description = "Simple 1-DOF control" bm = Marker() bm.type = Marker.CUBE bm.scale.x = bm.scale.y = bm.scale.z = 0.45 bm.color.r = 0.0 bm.color.g = 0.5 bm.color.b = 0.5 bm.color.a = 1.0 bc = InteractiveMarkerControl() bc.always_visible = True bc.markers.append(bm) self.im.controls.append(bc) rc = InteractiveMarkerControl() rc.name = "move_x" rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.im.controls.append(rc) self.ims.insert(self.im, self.process_marker_feedback) self.ims.applyChanges()
def __init__(self, rate = 30.0, filename = None): # Marker server self.server = InteractiveMarkerServer('camera_marker') # TF broadcaster self.tf = TransformBroadcaster() # Marker pose self.pose_mutex = Lock() self.marker_position = (0.0, 0.0, 0.0) self.marker_orientation = (0.0, 0.0, 0.0, 1.0) # Load init position self.filename = filename if self.filename: with open(self.filename, 'r') as stream: init_data = yaml.load(stream)['init_position'] self.marker_position = (init_data['x'], init_data['y'], init_data['z']) self.marker_orientation = (0.0, 0.0, 0.0, 1.0) # Register shutdown callback rospy.on_shutdown(self.shutdown) # Add marker self.add_6DOF() # Timer for TF broadcaster rospy.Timer(rospy.Duration(1.0/rate), self.publish_transform)
def __init__(self, step_number, arm_index, action_step, marker_click_cb): ''' Args: step_number (int): The 1-based index of the step. arm_index (int): Side.RIGHT or Side.LEFT action_step (ActionStep): The action step this marker marks. marker_click_cb (function(int,bool)): The function to call when a marker is clicked. Pass the uid of the marker (as calculated by get_uid(...) as well as whether it's selected. ''' if ActionStepMarker._im_server is None: im_server = InteractiveMarkerServer(TOPIC_IM_SERVER) ActionStepMarker._im_server = im_server self.action_step = action_step self.arm_index = arm_index self.arm_name = ARM_NAMES[arm_index] self.step_number = step_number self.is_requested = False self.is_deleted = False self.is_control_visible = False self.is_edited = False self.has_object = False self._sub_entries = None self._menu_handler = None self._prev_is_reachable = None ActionStepMarker._marker_click_cb = marker_click_cb
def __init__(self, name, X_init, box_scale=0.1): self.name = name self.X = X_init self.marker = createBoxMarker(scale=box_scale) self.interactive_marker = create6DofInteractive('/map', name, name, X_init, self.marker) self.marker_server = InteractiveMarkerServer(name + 'server') self.marker_server.insert(self.interactive_marker, self.markerMoved) self.marker_server.applyChanges()
def __init__(self, marker_name): host = rospy.get_param("mongodb_host") port = rospy.get_param("mongodb_port") self._client = pymongo.MongoClient(host, port) self._traj = dict() self._retrieve_logs() self._server = InteractiveMarkerServer(marker_name)
class PlanarInteractiveMarker(object): def __init__(self, name, frame_id, X_offset, text_offset, scale, axes_list=[0,1]): self.name = name self.scale = scale self.force_list = [0.] * len(axes_list) self.axes_list = axes_list # interactive marker self.marker = createBoxMarker(scale=0.001) intMarker = createVisInteractive(frame_id, name, name+' 2dcontrol', X_offset, self.marker) frame = toNumpy(Matrix3d.Identity()) for i in axes_list: control = createTranslationControlMarker('control'+str(i), frame[i,:].tolist()[0]) intMarker.controls.append(control) self.marker_server = InteractiveMarkerServer(name + 'server') self.marker_server.insert(intMarker, self.markerMoved) self.marker_server.applyChanges() # text display of forces self.text_marker = TextMarker(name, frame_id, text_offset) self.updateText() def updateText(self): self.text_marker.update(str(self.force_list[0]) + ' N \n' \ + str(self.force_list[1]) + ' N') def markerMoved(self, feedback): #TODO: better coding, check the ordering of x,y,z for i in self.axes_list: if i == 0: self.force_list[i] = feedback.pose.position.x * self.scale elif i == 1: self.force_list[i] = feedback.pose.position.z * self.scale elif i == 2: self.force_list[i] = feedback.pose.position.y * self.scale else: print 'error, expected 0-2' self.updateText() self.text_marker.publish()
def __init__(self, whicharm): self.whicharm = whicharm self.robot_state = pr2_control_utilities.RobotState() self.joint_controller = pr2_control_utilities.PR2JointMover(self.robot_state) self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller) self.server = InteractiveMarkerServer("~trajectory_markers") self.tf_listener = self.planner.tf_listener self.visualizer_pub = rospy.Publisher("~trajectory_markers_path", MarkerArray) self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray) self.gripper_pose_pub = rospy.Publisher("~gripper_pose", PoseStamped) rospy.Subscriber("~overwrite_trajectory", PoseArray, self.overwrite_trajectory) rospy.Service("~execute_trajectory", Empty, self.execute_trajectory_srv) # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/base_link" int_marker.pose.position.x = 0.5 int_marker.pose.position.z = 1.0 int_marker.name = "move_" + whicharm + "_arm" int_marker.description = "Move the " + whicharm + " arm" int_marker.scale = 0.2 self.server.insert(int_marker, self.main_callback) # create the main marker shape #color = (1,0,0,1) color = None self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker); #add the controls utils.make_6DOF_marker(int_marker) self.int_marker = int_marker self.create_menu() self.server.applyChanges() self.trajectory = PoseArray() self.trajectory.header.frame_id = "/base_link" self.last_gripper_pose = None if whicharm == "right": self.ik_utils = self.planner.right_ik else: self.ik_utils = self.planner.left_ik self.planning_waiting_time = 10.0 rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
class MassInteractiveMarker(object): def __init__(self, name, frame_id, X_offset, text_offset, mass, scale): self.name = name self.mass = mass self.scale = scale # interactive marker self.marker = createBoxMarker(scale=0.001) self.interactive_marker = create1DofTransInteractive(frame_id, name, name+' control', X_offset, self.marker, direction=-Vector3d.UnitY()) self.marker_server = InteractiveMarkerServer(name + 'server') self.marker_server.insert(self.interactive_marker, self.markerMoved) self.marker_server.applyChanges() self.text_marker = TextMarker(name, frame_id, text_offset) def markerMoved(self, feedback): self.mass = abs(feedback.pose.position.z) * self.scale self.text_marker.update(str(self.mass) + ' kg') self.text_marker.publish()
def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges()
class WrenchInteractiveMarker(object): def __init__(self, name, frame_id, X_init, text_offset, scale): self.name = name self.marker = createBoxMarker() self.interactive_marker = create6DofInteractive(frame_id, name, name, X_init, self.marker) self.marker_server = InteractiveMarkerServer(name + 'server') self.marker_server.insert(self.interactive_marker, self.markerMoved) self.marker_server.applyChanges() self.text_marker = TextMarker(name, frame_id, text_offset) self.X_init_inv = X_init.inv() self.scale = scale self.forces = Vector3d.Zero() self.torques = Vector3d.Zero() def markerMoved(self, feedback): X = transform.fromPose(feedback.pose) * self.X_init_inv self.forces = self.scale[0] * X.translation() self.torques = self.scale[1] * sva.rotationError(Matrix3d.Identity(), X.rotation()) self.text_marker.update(str(self.forces) + '\n' + str(self.torques)) self.text_marker.publish()
def __init__(self, side_prefix): self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] # Create a trajectory action client r_traj_contoller_name = None l_traj_contoller_name = None if self.side_prefix == 'r': r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() else: l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.ik = IK(side_prefix) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix) self._im_server.applyChanges() print self.ik
def __init__(self): if World.tf_listener is None: World.tf_listener = TransformListener() self._lock = threading.Lock() self.surface = None self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer('world_objects') self.clear_all_objects() self.relative_frame_threshold = 0.4 # rospy.Subscriber("ar_pose_marker", # AlvarMarkers, self.receive_ar_markers) self.is_looking_for_markers = False self.marker_dims = Vector3(0.04, 0.04, 0.01) World.world = self
def __init__(self): # create a simple TF listener self.tf_listener = tf.TransformListener() self.grasp_check = rospy.ServiceProxy('/interactive_world_hackathon/grasp_check', GraspCheck) self.speak = rospy.ServiceProxy('/tts/speak', Speak) self.play = rospy.ServiceProxy('/tts/play', Speak) # create the nav client self.nav = actionlib.SimpleActionClient('navigate_action', navigateAction) self.nav.wait_for_server() # create the backup client self.backup = actionlib.SimpleActionClient('backup_action', BackupAction) self.backup.wait_for_server() # create the place action client self.place = actionlib.SimpleActionClient('object_manipulator/object_manipulator_place', PlaceAction) self.place.wait_for_server() # Segmentation client self.segclient = actionlib.SimpleActionClient('/object_detection_user_command', UserCommandAction) self.segclient.wait_for_server() self.recognition=None # create the IMGUI action client self.imgui = actionlib.SimpleActionClient('imgui_action', IMGUIAction) self.imgui.wait_for_server() # listen for graspable objects rospy.Subscriber('/interactive_object_recognition_result', GraspableObjectList, self.proc_grasp_list) # create the save service rospy.Service('~save_template', SaveTemplate, self.save) self.load_server = actionlib.SimpleActionServer('load_template', LoadAction, execute_cb=self.load, auto_start=False) self.load_server.start() # create the IM server self.server = InteractiveMarkerServer('~fake_marker_server') # create return list of templates rospy.Service('~print_templates', PrintTemplates, self.get_templates) # used to get model meshes self.get_mesh = rospy.ServiceProxy('/objects_database_node/get_model_mesh', GetModelMesh) # hack to get the grasp rospy.Subscriber('/object_manipulator/object_manipulator_pickup/result', PickupActionResult, self.store_grasp) self.last_grasp = None self.objects = [] self.objects.append(OBJECT1) self.objects.append(OBJECT2) self.objects.append(OBJECT3) self.reset_objects() # check for saved templates try: self.templates = pickle.load(open(SAVE_FILE, 'rb')) rospy.loginfo('Loaded ' + str(len(self.templates.keys())) + ' template(s).') except: self.templates = dict() rospy.loginfo('New template file started.') self.play('/home/rctoris/wav/GLaDOS_generic_security_camera_destroyed-2.wav')
def __init__(self, name, frame_id, X_init, text_offset, scale): self.name = name self.marker = createBoxMarker() self.interactive_marker = create6DofInteractive(frame_id, name, name, X_init, self.marker) self.marker_server = InteractiveMarkerServer(name + 'server') self.marker_server.insert(self.interactive_marker, self.markerMoved) self.marker_server.applyChanges() self.text_marker = TextMarker(name, frame_id, text_offset) self.X_init_inv = X_init.inv() self.scale = scale self.forces = Vector3d.Zero() self.torques = Vector3d.Zero()
def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() # Code for moving the robots joints switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy(switch_srv_name, SwitchController) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server()
def __init__(self, name, frame_id, X_offset, text_offset, mass, scale): self.name = name self.mass = mass self.scale = scale # interactive marker self.marker = createBoxMarker(scale=0.001) self.interactive_marker = create1DofTransInteractive(frame_id, name, name+' control', X_offset, self.marker, direction=-Vector3d.UnitY()) self.marker_server = InteractiveMarkerServer(name + 'server') self.marker_server.insert(self.interactive_marker, self.markerMoved) self.marker_server.applyChanges() self.text_marker = TextMarker(name, frame_id, text_offset)
def __init__(self, side_prefix): if GripperMarkers._tf_listener is None: GripperMarkers._tf_listener = TransformListener() self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers') self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
def __init__(self): if World.tf_listener == None: World.tf_listener = TransformListener() self._lock = threading.Lock() self.surface = None self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer("world_objects") bb_service_name = "find_cluster_bounding_box" rospy.wait_for_service(bb_service_name) self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox) rospy.Subscriber("interactive_object_recognition_result", GraspableObjectList, self.receieve_object_info) self._object_action_client = actionlib.SimpleActionClient("object_detection_user_command", UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo("Interactive object detection action " + "server has responded.") self.clear_all_objects() # The following is to get the table information rospy.Subscriber("tabletop_segmentation_markers", Marker, self.receieve_table_marker)
def __init__(self, bag_file, md_path, calib_file, tracklets): self.timestamp_map = extract_bag_timestamps(bag_file) self.calib_file = calib_file self.frame_map = generate_frame_map(tracklets) md = None metadata = load_metadata(md_path) for obs in metadata: if obs['obstacle_name'] == 'obs1': md = obs assert md, 'obs1 metadata not found' self.metadata = md self.server = InteractiveMarkerServer("obstacle_marker") self.br = tf.TransformBroadcaster() self.offset = [0,0,0] self.rotation_offset = [0,0,0,1] self.orient = (0,0,0,1) self.marker = Marker() self.marker.type = Marker.CUBE self.marker.header.frame_id = "velodyne" md = self.metadata self.marker.scale.x = md['l'] self.marker.scale.y = md['w'] self.marker.scale.z = md['h'] self.marker.color.r = 0.2 self.marker.color.g = 0.5 self.marker.color.b = 0.2 self.marker.color.a = 0.7 outputName = '/image_bbox' self.imgOutput = rospy.Publisher(outputName, Image, queue_size=1) #self.markOutput = rospy.Publisher("bbox", Marker, queue_size=1) self.velodyne_marker = self.setup_marker(frame = "velodyne", name = "capture vehicle", translation=True) self.obs_marker = self.setup_marker(frame = "velodyne", name = "obstacle vehicle", translation=False)
def __init__(self, side_prefix): self._ik_service = IK(side_prefix) r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix)) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges()
def __init__(self, md_path, calib_file): self.reset() self.current_time = rospy.Time() self.calib_file = calib_file md = None metadata = load_metadata(md_path) for obs in metadata: if obs['obstacle_name'] == 'obs1': md = obs assert md, 'obs1 metadata not found' self.metadata = md self.server = InteractiveMarkerServer("obstacle_marker") self.br = tf.TransformBroadcaster() self.offset = [0,0,0] self.rotation_offset = [0,0,0,1] self.orient = (0,0,0,1) self.marker = Marker() self.marker.type = Marker.CUBE self.marker.header.frame_id = "obs_centroid" md = self.metadata self.marker.scale.x = md['l'] self.marker.scale.y = md['w'] self.marker.scale.z = md['h'] self.marker.color.r = 0.2 self.marker.color.g = 0.5 self.marker.color.b = 0.2 self.marker.color.a = 0.7 self.velodyne_marker = self.setup_marker(frame = "velodyne", name = "capture vehicle", translation=True) self.obs_marker = self.setup_marker(frame = "obs_centroid", name = "obstacle vehicle", translation=False)
def __init__(self, name, frame_id, X_offset, text_offset, scale, axes_list=[0,1]): self.name = name self.scale = scale self.force_list = [0.] * len(axes_list) self.axes_list = axes_list # interactive marker self.marker = createBoxMarker(scale=0.001) intMarker = createVisInteractive(frame_id, name, name+' 2dcontrol', X_offset, self.marker) frame = toNumpy(Matrix3d.Identity()) for i in axes_list: control = createTranslationControlMarker('control'+str(i), frame[i,:].tolist()[0]) intMarker.controls.append(control) self.marker_server = InteractiveMarkerServer(name + 'server') self.marker_server.insert(intMarker, self.markerMoved) self.marker_server.applyChanges() # text display of forces self.text_marker = TextMarker(name, frame_id, text_offset) self.updateText()
class AutosequencesVizModule(VizModuleBase): """ Visualization module for control_mode_autosequence. Provides: - polygon representing autosequence path - markers for autosequence points - labels - heading indicators at each point """ got_autosequence = False current_autosequence = None latest_autoseq_info = None latest_hover_info = None loaded_as_name = "" def __init__(self): super(AutosequencesVizModule, self).__init__(id="autoseq") def init_viz(self): self.as_poly = PolygonMarker('autosequence', (), color=Colors.BLUE+Alpha(0.5), width=0.02, closed=False) self.as_hover_point_sphere = SphereMarker('hover_point', (0,0,0), radius=0.02, color=Colors.BLUE+Alpha(0.5)) self.mg = MarkerGroup(VizModuleBase.mapub) self.mg.add(self.as_poly, self.as_hover_point_sphere) self.init_int_marker() def init_vars(self): pass def init_subscribers(self): prefix = self.subscriber_topic_prefix #self.llstatus_sub = rospy.Subscriber(prefix+'autopilot/LL_STATUS', LLStatus, self.llstatus_callback) # AscTec specific.. how to make more generic? self.autoseq_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/autosequence_info', control_mode_autosequence_info, self.autoseq_info_callback) self.hover_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/info', control_mode_hover_info, self.hover_info_callback) self.controller_status_sub = rospy.Subscriber(prefix+'controller/status', controller_status, self.controller_status_callback) def init_int_marker(self): self.ims = InteractiveMarkerServer("simple_marker") self.im = InteractiveMarker() self.im.header.frame_id = "/ned" self.im.name = "my_marker" self.im.description = "Simple 1-DOF control" bm = Marker() bm.type = Marker.CUBE bm.scale.x = bm.scale.y = bm.scale.z = 0.45 bm.color.r = 0.0 bm.color.g = 0.5 bm.color.b = 0.5 bm.color.a = 1.0 bc = InteractiveMarkerControl() bc.always_visible = True bc.markers.append(bm) self.im.controls.append(bc) rc = InteractiveMarkerControl() rc.name = "move_x" rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.im.controls.append(rc) self.ims.insert(self.im, self.process_marker_feedback) self.ims.applyChanges() def process_marker_feedback(self, feedback): p = feedback.pose.position print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z) def publish_timer_callback(self, event): if self.got_autosequence: as_points = [(p.hover_point.x, p.hover_point.y, 0) for p in self.current_autosequence.points] self.as_poly.set(points=as_points) if self.latest_hover_info is not None: slhi = self.latest_hover_info self.as_hover_point_sphere.set(origin=(slhi.north_cmd, slhi.east_cmd, 0)) now = rospy.Time.now() VizModuleBase.publish(self,now) def autoseq_info_callback(self, msg): #rospy.loginfo('got autoseq info: %s' % str(msg)) self.latest_autoseq_info = msg def hover_info_callback(self, msg): self.latest_hover_info = msg def _new_autosequence(self): return (self.latest_autoseq_info is not None and len(self.latest_autoseq_info.defined_autosequences) > 0 and (self.latest_autoseq_info.current_autosequence != self.loaded_as_name)) def controller_status_callback(self, msg): self.latest_controller_status = msg #rospy.loginfo('got controller status: %s' % str(msg)) if msg.active_mode == 'autosequence': if self._new_autosequence(): self.got_autosequence = False if (not self.got_autosequence and self.latest_autoseq_info is not None and len(self.latest_autoseq_info.defined_autosequences) > 0 and len(self.latest_autoseq_info.current_autosequence) > 0): as_name = self.latest_autoseq_info.current_autosequence self.get_autosequence(as_name) def get_autosequence(self, as_name): get_auto_proxy = rospy.ServiceProxy('control_mode_autosequence/get_autosequence', GetAutosequence) rospy.loginfo("Asking for autosequence '%s'..." % as_name) resp = get_auto_proxy(autosequence_name=as_name) if resp.found: self.current_autosequence = resp.autosequence rospy.loginfo('Got autosequence: %s' % str(self.current_autosequence)) self.got_autosequence = True self.loaded_as_name = as_name else: rospy.logerr("Service call failed: autosequence '%s' not found" % as_name)
class TFMarkerServer(object): """TFMarkerServer""" def __init__(self, rate = 30.0, filename = None): # Marker server self.server = InteractiveMarkerServer('camera_marker') # TF broadcaster self.tf = TransformBroadcaster() # Marker pose self.pose_mutex = Lock() self.marker_position = (0.0, 0.0, 0.0) self.marker_orientation = (0.0, 0.0, 0.0, 1.0) # Load init position self.filename = filename if self.filename: with open(self.filename, 'r') as stream: init_data = yaml.load(stream)['init_position'] self.marker_position = (init_data['x'], init_data['y'], init_data['z']) self.marker_orientation = (0.0, 0.0, 0.0, 1.0) # Register shutdown callback rospy.on_shutdown(self.shutdown) # Add marker self.add_6DOF() # Timer for TF broadcaster rospy.Timer(rospy.Duration(1.0/rate), self.publish_transform) 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 publish_transform(self, timer_event): time = rospy.Time.now() self.pose_mutex.acquire() self.tf.sendTransform(self.marker_position, self.marker_orientation, time, 'sensor_base', 'map') self.pose_mutex.release() def marker_feedback(self, feedback): rospy.loginfo('Feedback from ' + feedback.marker_name) # Check event if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: rospy.loginfo( 'Pose changed') # Update marker position self.pose_mutex.acquire() self.marker_position = (feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z) # Update marker orientation self.marker_orientation = (feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w) self.pose_mutex.release() def shutdown(self): data = { 'init_position' : { 'x': 0.0, 'y': 0.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, } } rospy.loginfo('Writing current position') with open(self.filename, 'w') as outfile: outfile.write(yaml.dump(data, default_flow_style=True))
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() def get_ee_pose(self): from_frame = '/base_link' to_frame = '/' + self.side_prefix + '_wrist_roll_link' try: t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t) except: rospy.logwarn('Could not get end effector pose through TF.') pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, dummy): rospy.loginfo('You pressed the `Move arm here` button from the menu.') target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_solution = self.ik.get_ik_for_ee(target_pose) #TODO: Send the arms to ik_solution def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) 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 _add_gripper_marker(self, control): is_hand_open=False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 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) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -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) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def get_pose_from_transform(transform): pos = transform[:3,3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) @staticmethod def _offset_pose(pose): transform = GripperMarkers.get_matrix_from_pose(pose) offset_array = [GripperMarkers._offset, 0, 0] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform) return GripperMarkers.get_pose_from_transform(hand_transform) @staticmethod def get_matrix_from_pose(pose): rotation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] transformation = tf.transformations.quaternion_matrix(rotation) position = [pose.position.x, pose.position.y, pose.position.z] transformation[:3, 3] = position return transformation def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_solution = self.ik.get_ik_for_ee(target_pose) if (ik_solution == None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) 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
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self._ik_service = IK(side_prefix) r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix)) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() def get_ee_pose(self): from_frame = '/base_link' to_frame = '/' + self.side_prefix + '_wrist_roll_link' try: t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t) except Exception as e: rospy.logerr('Could not get end effector pose through TF.') pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] import traceback traceback.print_exc() return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, dummy): target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_sol = self._ik_service.get_ik_for_ee(target_pose) self.move_to_joints(self.side_prefix, [ik_sol], 1.0) def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) def marker_cb(self, pose_markers): rospy.loginfo('AR Marker Pose updating') transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose) offset_array = [0, 0, .03] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform) self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform) self.update_viz() 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=%.3f y=%.3f z=%.3f" % (pose.position.x, pose.position.y, 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)))) label_pos = Point() label_pos.x = pose.position.x label_pos.y = pose.position.y label_pos.z = pose.position.z label = "{} arm".format(self.side_prefix) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=label, color=ColorRGBA(0.0, 0.0, 0.0, 0.9), header=Header(frame_id=frame_id), pose=Pose(label_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 _add_gripper_marker(self, control): is_hand_open=False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 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) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -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) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def _offset_pose(pose): transform = GripperMarkers.get_matrix_from_pose(pose) offset_array = [-GripperMarkers._offset, 0, 0] offset_transform = tf.transformations.translation_matrix(offset_array) hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform) return GripperMarkers.get_pose_from_transform(hand_transform) @staticmethod def get_matrix_from_pose(pose): rotation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] transformation = tf.transformations.quaternion_matrix(rotation) position = [pose.position.x, pose.position.y, pose.position.z] transformation[:3, 3] = position return transformation @staticmethod def get_pose_from_transform(transform): pos = transform[:3,3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_sol = self._ik_service.get_ik_for_ee(target_pose) if ik_sol == None: mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) 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 move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) time_move = time_to_joint print "using following positions %s" % positions for pose in positions: velocities = [0] * len(pose) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose, velocities=velocities, time_from_start=rospy.Duration(time_move))) time_move += time_to_joint if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal)