class AnnotatorServer(object): def __init__(self): self._annotator = Annotator() self._pose_names_pub = rospy.Publisher("/map_annotator/pose_names", PoseNames, queue_size=10, latch=True) # self._map_poses_pub = rospy.Publisher("/map_annotator/map_poses", # InteractiveMarker, queue_size=10, latch=True) self._int_marker_server = InteractiveMarkerServer("/map_annotator/map_poses") self.INITIAL_POSE = Pose() self.INITIAL_POSE.orientation.w = 1 print("Initializing saved markers: " + str(self._annotator.get_position_names())) for name, pose in self._annotator.get_position_items(): self.__create_int_marker__(name, pose) self.__pub_pose_info__() print("Initialization finished...") def __pub_pose_info__(self): pose_names = PoseNames() pose_names.names = self._annotator.get_position_names() pose_names.names.sort() self._pose_names_pub.publish(pose_names) def __update_marker_pose__(self, input): if (input.event_type == InteractiveMarkerFeedback.MOUSE_UP): name = input.marker_name new_pose = self._int_marker_server.get(name).pose # Overwrite the previous pose with the new pose self._annotator.save_position(name, new_pose) self._int_marker_server.setPose(name, new_pose) self._int_marker_server.applyChanges() print("updated pose for: " + name) def __create_int_marker__(self, name, pose): print("creating int marker: " + name) int_marker = InteractiveMarker() int_marker.header.frame_id = "map" int_marker.name = name int_marker.description = name int_marker.pose = pose # Move it 0.25 meters up to make it easier to click int_marker.pose.position.z = 0.25 text_marker = Marker() text_marker.text = name text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.position.z = 2 text_marker.scale.x = 0.4 text_marker.scale.y = 0.4 text_marker.scale.z = 0.4 text_marker.color.r = 0.0 text_marker.color.g = 0.5 text_marker.color.b = 0.5 text_marker.color.a = 1.0 text_control = InteractiveMarkerControl() text_control.name = "text_control" text_control.markers.append(text_marker) text_control.always_visible = True text_control.interaction_mode = InteractiveMarkerControl.NONE int_marker.controls.append(text_control) rotation_ring_control = InteractiveMarkerControl() rotation_ring_control.name = "position_control" rotation_ring_control.always_visible = True rotation_ring_control.orientation.x = 0 rotation_ring_control.orientation.w = 1 rotation_ring_control.orientation.y = 1 rotation_ring_control.orientation.z = 0 rotation_ring_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(rotation_ring_control) arrow_marker = Marker() arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 1 arrow_marker.pose.position.z = 0.15 arrow_marker.pose.position.x = -0.5 arrow_marker.scale.x = 1 arrow_marker.scale.y = 0.25 arrow_marker.scale.z = 0.25 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 position_control = InteractiveMarkerControl() position_control.name = "rotation_control" position_control.always_visible = True position_control.markers.append(arrow_marker) position_control.orientation.w = 1 position_control.orientation.x = 0 position_control.orientation.y = 1 position_control.orientation.z = 0 position_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE int_marker.controls.append(position_control) self._int_marker_server.insert(int_marker, self.__update_marker_pose__) self._int_marker_server.applyChanges() def create(self, name, pose=None): print("creating new pose: " + name) if pose is None: pose = self.INITIAL_POSE self._annotator.save_position(name, pose) self.__create_int_marker__(name, pose) self.__pub_pose_info__() def delete(self, name): if self._annotator.exists(name): print("deleting pose: " + name) self._annotator.delete_position(name) self._int_marker_server.erase(name) self._int_marker_server.applyChanges() self.__pub_pose_info__() def handle_callback(self, user_action_msg): cmd = user_action_msg.command name = user_action_msg.name if cmd == UserAction.CREATE: self.create(name) elif cmd == UserAction.DELETE: self.delete(name) elif cmd == UserAction.GOTO: print("going to pose: " + name) self._annotator.goto_position(name)
class MeasurementVisServer(object): def __init__(self): self.marker_pub = rospy.Publisher('measurement_markers', MarkerArray, queue_size=50) self.positions_pub = rospy.Publisher('object_positions', ObjectMeasurement, queue_size=50) self.init_positions_pub = rospy.Publisher('object_initialization_positions', ObjectMeasurement, queue_size=50) self.target_pub = rospy.Publisher('get_target_poses', PoseArray, queue_size=50) #self.object_pub = rospy.Publisher('measurement_markers', MarkerArray, queue_size=10)' self.nbr_targets = rospy.get_param('~number_targets', 2) self.nbr_noise = rospy.get_param('~number_noise', 4) self.markers = MarkerArray() self.object_counters = np.zeros((self.nbr_targets,), dtype=int) self.initialized = np.zeros((self.nbr_targets,), dtype=bool) self.marker_server = InteractiveMarkerServer("object_interactive_markers") #self.room_server = InteractiveMarkerServer("room_interactive_markers") self.marker_poses = [Pose() for j in range(0, self.nbr_targets)] self.previous_poses = [Pose() for j in range(0, self.nbr_targets)] self.did_move = np.zeros((self.nbr_targets,), dtype=bool) self.marker_times = np.zeros((self.nbr_targets,), dtype=np.int64) self.marker_clicked_times = np.zeros((self.nbr_targets,), dtype=np.int64) self.regions, self.centers = get_regions() self.room_time = 0 self.room_id = 0 for i, region in enumerate(self.regions): print self.centers[i] #self.clear_markers() self.timestep = 0 self.measurement_counter = 0 self.clear_markers() #rospy.Timer(rospy.Duration(0.1), callback=self.maybe_publish_poses) rospy.Timer(rospy.Duration(0.1), callback=self.maybe_publish_rooms) rospy.Subscriber("filter_measurements", ObjectMeasurement, self.callback) rospy.Subscriber("sim_filter_measurements", ObjectMeasurement, self.callback) rospy.Subscriber("set_target_poses", PoseArray, self.set_target_poses) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.clicked_callback) self.initialize_room_markers() def set_target_poses(self, poses): for j, p in enumerate(poses.poses): if p.position.x == 0 and p.position.x == 0: continue if not self.initialized[j]: self.initialized[j] = True self.initialize_object_marker(j, p) else: name = "object_marker_" + str(j) p.position.z = 0.15 self.marker_poses[j] = p self.marker_server.setPose(name, p) self.marker_server.applyChanges() def publish_target_poses(self): poses = PoseArray() for j in range(0, self.nbr_targets): poses.poses.append(self.marker_poses[j]) self.target_pub.publish(poses) def maybe_publish_rooms(self, event): self.publish_target_poses() seconds = rospy.Time.now().to_sec() for j in range(0, self.nbr_targets): mtime = self.marker_clicked_times[j] if mtime != 0 and seconds - mtime > 1: print "Publishing initial position for object ", j self.marker_clicked_times[j] = 0 pose = PoseStamped() pose.header.frame_id = "map" pose.header.stamp = rospy.Time.now() pose.pose = self.marker_poses[j] object_pos = ObjectMeasurement() object_pos.pose = pose object_pos.initialization_id = j object_pos.timestep = 0 object_pos.observation_id = 0 object_pos.negative_observation = False self.init_positions_pub.publish(object_pos) seconds = rospy.Time.now().to_sec() if self.room_time == 0 or seconds - self.room_time < 1: return room = self.regions[self.room_id] # ok, time to see if we have any objects within this region: vertices = np.zeros((len(room.posearray.poses), 2), dtype=float) for i, pose in enumerate(room.posearray.poses): vertices[i] = np.array([pose.position.x, pose.position.y]) #hull = ConvexHull(vertices) #print vertices #if not isinstance(hull, Delaunay): # hull = Delaunay(hull) hull = Delaunay(vertices) #hull = Delaunay(ConvexHull(vertices)) published = False shuffled = np.arange(self.nbr_targets) np.random.shuffle(shuffled) for j in shuffled:#range(0, self.nbr_targets): if not self.initialized[j]: continue pose = [self.marker_poses[j].position.x, self.marker_poses[j].position.y] if hull.find_simplex(pose) >= 0: pose = PoseStamped() pose.header.frame_id = "map" pose.header.stamp = rospy.Time.now() pose.pose = self.marker_poses[j] object_pos = ObjectMeasurement() object_pos.pose = pose object_pos.initialization_id = j object_pos.timestep = self.timestep object_pos.observation_id = self.measurement_counter object_pos.location_id = self.room_id object_pos.negative_observation = False self.measurement_counter += 1 self.positions_pub.publish(object_pos) self.room_time = 0 published = True # if self.did_move[j]: # pose = [self.previous_poses[j].position.x, self.previous_poses[j].position.y] # if hull.find_simplex(pose) >= 0: # print "Previous pose was inside, PUBLISHING!" # #self.did_move[j] = False # need to fix a history? # #self.previous_poses[j] = self.marker_poses[j] # pose = PoseStamped() # pose.header.frame_id = "map" # pose.header.stamp = rospy.Time.now() # pose.pose = self.previous_poses[j] # object_pos = ObjectMeasurement() # object_pos.pose = pose # object_pos.initialization_id = j # object_pos.timestep = self.timestep # object_pos.observation_id = self.measurement_counter # object_pos.location_id = self.room_id # object_pos.negative_observation = True # self.positions_pub.publish(object_pos) # published = True # compute min an max vertices in x and y to be able to sample uniform # then use rejection sampling to get points within the polygon maxs = np.amax(vertices, axis=0) mins = np.amin(vertices, axis=0) for i in range(0, self.nbr_noise): while True: x = random.uniform(mins[0], maxs[0]) y = random.uniform(mins[1], maxs[1]) pose = [x, y] if hull.find_simplex(pose) >= 0: pose = PoseStamped() pose.header.frame_id = "map" pose.header.stamp = rospy.Time.now() pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = 0. pose.pose.orientation.x = 0. pose.pose.orientation.x = 0. pose.pose.orientation.x = 0. pose.pose.orientation.w = 1. object_pos = ObjectMeasurement() object_pos.pose = pose object_pos.initialization_id = -1 object_pos.timestep = self.timestep object_pos.observation_id = self.measurement_counter object_pos.location_id = self.room_id object_pos.negative_observation = False self.measurement_counter += 1 self.positions_pub.publish(object_pos) self.room_time = 0 published = True break if published: self.timestep = self.timestep + 1 def maybe_publish_poses(self, event): seconds = rospy.Time.now().to_sec() for j in range(0, self.nbr_targets): mtime = self.marker_times[j] #if mtime != 0: # print "Time diff for ", j , " is: ", mtime - seconds if mtime != 0 and seconds - mtime > 1: self.did_move[j] = True self.marker_times[j] = 0 #pose = PoseStamped() #pose.header.frame_id = "map" #pose.header.stamp = rospy.Time.now() #pose.pose = self.marker_poses[j] #object_pos = ObjectMeasurement() #object_pos.pose = pose #object_pos.initialization_id = j #object_pos.observation_id = self.measurement_counter #self.measurement_counter += 1 #self.positions_pub.publish(object_pos) #self.marker_times[j] = 0 def object_id_color(self, object_id): colors = {"vivid_yellow": (255, 179, 0), "strong_purple": (128, 62, 117), "vivid_orange": (255, 104, 0), "very_light_blue": (166, 189, 215), "vivid_red": (193, 0, 32), "grayish_yellow": (206, 162, 98), "medium_gray": (129, 112, 102), # these aren't good for people with defective color vision: "vivid_green": (0, 125, 52), "strong_purplish_pink": (246, 118, 142), "strong_blue": (0, 83, 138), "strong_yellowish_pink": (255, 122, 92), "strong_violet": (83, 55, 122), "vivid_orange_yellow": (255, 142, 0), "strong_purplish_red": (179, 40, 81), "vivid_greenish_yellow": (244, 200, 0), "strong_reddish_brown": (127, 24, 13), "vivid_yellowish_green": (147, 170, 0), "deep_yellowish_brown": (89, 51, 21), "vivid_reddish_orange": (241, 58, 19), "dark_olive_green": (35, 44, 22)} color = np.array(colors[colors.keys()[object_id]], dtype=float) / 255.0 return color def clear_markers(self): clear_markers = MarkerArray() pose = Pose() pose.position.x = 0. pose.position.y = 0. pose.position.z = 0. pose.orientation.x = 0. pose.orientation.y = 0. pose.orientation.z = 0. pose.orientation.w = 1. for i in range(0, 1000): clear_marker = Marker() clear_marker.header.frame_id = "map" clear_marker.header.stamp = rospy.Time.now() clear_marker.type = Marker.SPHERE clear_marker.action = Marker.DELETE clear_marker.ns = "my_namespace" clear_marker.id = i clear_marker.pose = pose #clear_marker.lifetime = rospy.Time(0) clear_markers.markers.append(clear_marker) self.marker_pub.publish(clear_markers) def initialize_room_markers(self): for room_id in range(0, len(self.regions)): pose = Pose() pose.position.x = self.centers[room_id, 0] pose.position.y = self.centers[room_id, 1] pose.position.z = 0.2 pose.orientation.x = 0. pose.orientation.y = 0. pose.orientation.z = 0. pose.orientation.w = 1. marker = InteractiveMarker() marker.header.frame_id = "map" marker.name = "room_marker_" + str(room_id) marker.description = "Room " + str(room_id) # the marker in the middle box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.35 box_marker.scale.y = 0.35 box_marker.scale.z = 0.35 box_marker.color.r = 0. box_marker.color.g = 0. box_marker.color.b = 1. box_marker.color.a = 1. box_marker.id = 1000 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True #box_control.always_visible = False box_control.markers.append(box_marker) box_control.name = "button" box_control.interaction_mode = InteractiveMarkerControl.BUTTON marker.controls.append(box_control) #marker.controls.append(box_control) # move x #control = InteractiveMarkerControl() #control.orientation.w = 1 #control.orientation.x = 0 #control.orientation.y = 1 #control.orientation.z = 0 #control.always_visible = True # control.name = "move_x" # control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.marker_server.insert(marker, self.room_feedback) self.marker_server.applyChanges() self.marker_server.setPose( marker.name, pose ) self.marker_server.applyChanges() def room_feedback(self, feedback): room_id = int(feedback.marker_name.rsplit('_', 1)[-1]) print "Room id: ", room_id self.room_id = room_id self.room_time = rospy.Time.now().to_sec() def initialize_object_marker(self, object_id, pose): print "Adding interactive marker for object: ", object_id color = self.object_id_color(object_id) marker = InteractiveMarker() marker.header.frame_id = "map" marker.name = "object_marker_" + str(object_id) marker.description = "Object " + str(object_id) click_marker = InteractiveMarker() click_marker.header.frame_id = "map" click_marker.name = "button_object_marker_" + str(object_id) click_marker.description = "" # the marker in the middle box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.25 box_marker.scale.y = 0.25 box_marker.scale.z = 0.25 box_marker.color.r = color[0] box_marker.color.g = color[1] box_marker.color.b = color[2] box_marker.color.a = 1. box_marker.id = 1000 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True #box_control.always_visible = False box_control.markers.append(box_marker) box_control.name = "button" box_control.interaction_mode = InteractiveMarkerControl.BUTTON marker.controls.append(box_control) # move x control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.always_visible = True # control.name = "move_x" # control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.name = "move_plane" control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE marker.controls.append(control) self.marker_poses[object_id] = pose self.previous_poses[object_id] = pose self.marker_server.insert(marker, self.marker_feedback) self.marker_server.applyChanges() pose.position.z = 0.15 self.marker_server.setPose( marker.name, pose ) self.marker_server.applyChanges() def marker_feedback(self, feedback): if feedback.control_name == "button": object_id = int(feedback.marker_name.rsplit('_', 1)[-1]) self.marker_clicked_times[object_id] = rospy.Time.now().to_sec() print "Marker id:", feedback.marker_name print "Object id: ", object_id elif feedback.control_name == "move_plane": #self.in_feedback=True #vertex_name = feedback.marker_name.rsplit('-', 1) object_id = int(feedback.marker_name.rsplit('_', 1)[-1]) print "Marker id: ", object_id #self.topo_map.update_node_vertex(node_name, vertex_index, feedback.pose) #self.update_needed=True # just do something if there has been no updates for the # last x seconds feedback.pose.position.z = 0.15 self.marker_poses[object_id] = feedback.pose self.marker_times[object_id] = rospy.Time.now().to_sec() def clicked_callback(self, clicked_pose): non_initialized = np.nonzero(self.initialized == False)[0] if len(non_initialized) == 0: return ind = non_initialized[0] self.initialized[ind] = True self.initialize_object_marker(ind, clicked_pose.pose.pose) def callback(self, clicked_pose): if clicked_pose.initialization_id != -1 and not self.initialized[clicked_pose.initialization_id]: self.initialized[clicked_pose.initialization_id] = True self.initialize_object_marker(clicked_pose.initialization_id, clicked_pose.pose.pose) print "Got measurement, adding to GMMPoses ", clicked_pose.initialization_id color = self.object_id_color(clicked_pose.initialization_id) sphere_marker = Marker() sphere_marker.header.frame_id = "map" sphere_marker.header.stamp = rospy.Time.now() sphere_marker.ns = "my_namespace" sphere_marker.id = len(self.markers.markers) sphere_marker.type = Marker.SPHERE sphere_marker.action = Marker.ADD sphere_marker.pose.position.x = clicked_pose.pose.pose.position.x sphere_marker.pose.position.y = clicked_pose.pose.pose.position.y sphere_marker.pose.position.z = 0.2 sphere_marker.pose.orientation.x = 0. sphere_marker.pose.orientation.y = 0. sphere_marker.pose.orientation.z = 0. sphere_marker.pose.orientation.w = 1. sphere_marker.scale.x = 0.2 sphere_marker.scale.y = 0.2 sphere_marker.scale.z = 0.2 sphere_marker.color.a = 1. # Don't forget to set the alpha! sphere_marker.color.r = color[0] sphere_marker.color.g = color[1] sphere_marker.color.b = color[2] #sphere_marker.lifetime = rospy.Time(secs=1000) text_marker = Marker() text_marker.header.frame_id = "map" text_marker.header.stamp = rospy.Time.now() text_marker.ns = "my_namespace" text_marker.id = len(self.markers.markers)+1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = clicked_pose.pose.pose.position.x text_marker.pose.position.y = clicked_pose.pose.pose.position.y text_marker.pose.position.z = 0.4 text_marker.pose.orientation.x = 0. text_marker.pose.orientation.y = 0. text_marker.pose.orientation.z = 0. text_marker.pose.orientation.w = 1. text_marker.scale.z = 0.2 text_marker.color.a = 1. # Don't forget to set the alpha! text_marker.color.r = 0. text_marker.color.g = 0. text_marker.color.b = 0. if clicked_pose.negative_observation: text_marker.text = "Negative " + str(self.object_counters[clicked_pose.initialization_id]) else: text_marker.text = str(clicked_pose.timestep) #str(self.object_counters[clicked_pose.initialization_id]) #text_marker.lifetime = rospy.Time(secs=1000) if clicked_pose.initialization_id != -1: # noise! self.object_counters[clicked_pose.initialization_id] += 1 self.markers.markers.append(sphere_marker) self.markers.markers.append(text_marker) self.marker_pub.publish(self.markers)
control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 1 control.orientation.z = 1 control.name = "move_3D" control.always_visible = True control.markers.append(make_sphere()) control.interaction_mode = InteractiveMarkerControl.MOVE_3D int_marker.controls.append(control) server.insert(int_marker, process_feedback) server.applyChanges() # main loop while not rospy.is_shutdown(): publish_target_pose() if has_error: reset_marker_pose_blocking() publish_target_pose() server.setPose("centering_frame_pose", marker_pose.pose) server.applyChanges() rospy.sleep(0.5) rospy.sleep(0.1)
class CircleMarker(object): def __init__(self): self._server = InteractiveMarkerServer("simple_marker") def callback_marker(self, input): pass def align_marker(self, feedback): pose = feedback.pose self._server.setPose(feedback.marker_name, pose) print("Arrow Marker's name is ", feedback.marker_name) self._server.setPose("orientation_ring", pose) self._server.applyChanges() def orientation_ring_callback(self, feedback): if (feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE): print("Updating arrow marker's position ") self._server.setPose("int_arrow_marker", feedback.pose) self._server.applyChanges() def create_draggable_marker(self, position): int_arrow_marker = InteractiveMarker() int_arrow_marker.header.frame_id = "map" int_arrow_marker.name = "int_arrow_marker" int_arrow_marker.description = "right" int_arrow_marker.pose.position.y = position.y int_arrow_marker.pose.position.x = position.x int_arrow_marker.pose.position.z = position.z + 0.1 int_arrow_marker.scale = 1 arrow_marker = Marker() arrow_marker.ns = "arrow_marker" arrow_marker.type = Marker.ARROW arrow_marker.pose.orientation.w = 0 arrow_marker.scale.x = 0.5 arrow_marker.scale.y = 0.05 arrow_marker.scale.z = 0.05 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 1.0 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE button_control.always_visible = True button_control.orientation.w = 1 button_control.orientation.x = 0 button_control.orientation.y = 1 button_control.orientation.z = 0 button_control.markers.append(arrow_marker) int_arrow_marker.controls.append(button_control) orientation_ring_marker = InteractiveMarker() orientation_ring_marker.header.frame_id = "map" orientation_ring_marker.scale = 1 orientation_ring_marker.pose.position.y = position.y orientation_ring_marker.pose.position.z = position.z + 0.1 orientation_ring_marker.pose.position.x = position.x orientation_ring_marker.name = "orientation_ring" orientation_ring_marker.description = "Orientation Ring" orientation_ring_marker_control = InteractiveMarkerControl() orientation_ring_marker_control.always_visible = True orientation_ring_marker_control.orientation.w = 1 orientation_ring_marker_control.orientation.x = 0 orientation_ring_marker_control.orientation.y = 1 orientation_ring_marker_control.orientation.z = 0 orientation_ring_marker_control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE orientation_ring_marker.controls.append(orientation_ring_marker_control) self._server.insert(orientation_ring_marker, self.orientation_ring_callback) self._server.insert(int_arrow_marker, self.callback_marker) print"inserted" self._server.setCallback("int_arrow_marker", self.align_marker, InteractiveMarkerFeedback.POSE_UPDATE) self._server.applyChanges()
class PR2TrajectoryMarkers(object): """ A class to create and store a trajectory for one PR2 arm. The created trajectory can be published as a PoseArray message. This class published on the following topics: ~trajectory_markers are the main interactive markers. ~trajectory_poses a markerarray to display the trajectory. ~trajectory_poses a posesarray with the resulting pose The class subscribes to the topic ~overwrite_trajectory_ to change the stored trajectory. This is useful to resume working on a trajectory after re-starting the node. The message type is PoseArray. A std_srvs/Empty service named ~execute_trajectory is provided to externally trigger the execution of the trajectory. Constructor: TrajectoryMarkers(whicharm = "left") or TrajectoryMarkers(whicharm = "right") """ 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) def create_menu(self): """ Create and populates all the menu entries """ menu_handler = MenuHandler() menu_handler.insert("Point the head", callback = self.move_head) menu_handler.insert("Add position to trajectory", callback = self.add_point) menu_handler.insert("Place marker over gripper", callback = self.place_gripper) menu_handler.insert("Execute trjectory", callback = self.execute_trajectory) menu_handler.insert("Clear trajectory", callback = self.clear_trajectory) menu_handler.insert("Publish trajectory", callback = self.publish_trajectory) menu_handler.insert("Move the arm (planning)", callback = self.plan_arm) menu_handler.insert("Move the arm (collision-free)", callback = self.collision_free_arm) menu_handler.insert("Move the arm to trajectory start (collision-free)", callback = self.arm_trajectory_start) menu_handler.insert("Update planning scene", callback = self.update_planning) menu_handler.apply(self.server, self.int_marker.name) def main_callback(self, feedback): """ If the mouse button is released change the gripper color according to the IK result. Quite awkward, trying to get a nicer way to do it. """ #publish the gripper pose gripper_pos = PoseStamped() gripper_pos.header.frame_id = feedback.header.frame_id gripper_pos.pose = feedback.pose self.gripper_pose_pub.publish(gripper_pos) if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.last_gripper_pose = feedback.pose if (self.last_gripper_pose and feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP): self.last_gripper_pose = None pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): color = None else: color = (1,0,0,1) int_marker = self.server.get(self.int_marker.name) int_marker.controls.remove(self.gripper_marker) self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) #rospy.loginfo("Control: %s", int_marker.controls) self.server.insert(int_marker, self.main_callback) self.server.setPose(int_marker.name, self.last_gripper_pose) self.server.applyChanges() def overwrite_trajectory(self, msg): self.trajectory = msg def add_point(self, feedback): """ Add a point to self.trajectory if it is allowed by IK. """ pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): rospy.loginfo("Pose is reachable") self.trajectory.poses.append(feedback.pose) else: rospy.logerr("Pose is not reachable!") def place_gripper(self, feedback): """ Move the marker where the gripper is """ if self.whicharm == "right": gripper_pos = self.planner.get_right_gripper_pose() else: gripper_pos = self.planner.get_left_gripper_pose() self.server.setPose(self.int_marker.name, gripper_pos.pose, gripper_pos.header) self.server.applyChanges() def execute_trajectory(self, feedback): """ Executes the tracjectory memorized so far. It interpolates between the poses and creates suitable times and velocities. """ traj = self.interpolate_poses() if len(traj) == 0: rospy.logerr("Something went wrong when interpolating") return times, vels = self.ik_utils.trajectory_times_and_vels(traj) if len(vels) == 0 or len(times) == 0: rospy.logerr("Something went wrong when finding the times") return self.joint_controller.execute_trajectory(traj, times, vels, self.whicharm, wait = True) return def execute_trajectory_srv(self, _): """Same as execute_trajectory, but as a service. """ self.execute_trajectory(None) return EmptyResponse() def arm_trajectory_start(self, feedback): """ Move the gripper to the first pose in the trajectory. """ if len(self.trajectory.poses) == 0: rospy.logwarn("Empty trajectory!") return pose = self.trajectory.poses[0] if self.whicharm == "right": moveit = self.planner.move_right_arm_non_collision else: moveit = self.planner.move_left_arm_non_collision pos, quat = create_tuples_from_pose(pose) res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0) if not res: rospy.logerr("Something went wrong when moving") return def clear_trajectory(self, feedback): """ Removes all the points stored so far """ self.trajectory.poses = [] def move_head(self, feedback): """ Moves the head to face the marker """ frame = feedback.header.frame_id pos = (feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) print "Moving the head" self.joint_controller.time_to_reach = 1.0 self.joint_controller.point_head_to(pos, frame) def plan_arm(self, feedback): """ Moves the arm on the marker using motion collision-aware motion planning. """ frame = feedback.header.frame_id pos = (feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) orientation = (feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w, ) if self.whicharm == "right": rospy.loginfo("Moving the right arm") self.planner.move_right_arm(pos, orientation, frame, self.planning_waiting_time) else: rospy.loginfo("Moving the left arm") self.planner.move_left_arm(pos, orientation, frame, self.planning_waiting_time) def collision_free_arm(self, feedback): """ Moves the rm on the marker using motion NON-collision-aware inverse kinematiks. """ frame = feedback.header.frame_id pos = (feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) orientation = (feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w, ) if self.whicharm == "right": rospy.loginfo("Moving the right arm (non collision)") self.planner.move_right_arm_non_collision(pos, orientation, frame, 2.0) else: rospy.loginfo("Moving the left arm (non collision)") self.planner.move_left_arm_non_collision(pos, orientation, frame, 2.0) def update_planning(self, feedback): """ Updates the planning scene. """ self.planner.take_static_map() self.planner.update_planning_scene() def publish_trajectory(self, feedback): """ Publishes the trajectory as a PoseArray message """ self.trajectory_pub.publish(self.trajectory) def interpolate_poses(self): """ Refines the trajectory by interpolating between the joints. """ if len(self.trajectory.poses) < 2: rospy.logerr("At least two points in the trajectory are needed") return [] if self.whicharm == "right": starting_angles = self.robot_state.right_arm_positions else: starting_angles = self.robot_state.left_arm_positions all_trajectory = [starting_angles] for i in xrange(len(self.trajectory.poses) - 1): start = PoseStamped() start.header = self.trajectory.header start.pose = self.trajectory.poses[i] end = PoseStamped() end.header = self.trajectory.header end.pose = self.trajectory.poses[i+1] traj, errs = self.ik_utils.check_cartesian_path(start, end, all_trajectory[-1], #starting_angles, #pos_spacing = 0.05, collision_aware = 0, num_steps = 5, ) if any(e == 3 for e in errs): rospy.logerr("Error while running IK, codes are: %s", errs) return [] filtered_traj = [t for (t,e) in zip(traj,errs) if e == 0] all_trajectory.extend(filtered_traj) all_trajectory = normalize_trajectory(all_trajectory, starting_angles) rospy.loginfo("New interpolated trajectory with %d elements", len(all_trajectory)) return all_trajectory def publish_trajectory_markers(self, duration): """ Publishes markers to visualize the current trajectory. Paremeters: duration: how long should the markers visualization last. If this function is called from a loop they last at least the loop rate. """ if len(self.trajectory.poses) == 0: return msg = MarkerArray() marker_id = 0 #creating the path connecting the axes path = Marker() path.header.frame_id = self.trajectory.header.frame_id path.ns = "path" path.action = Marker.ADD path.type = Marker.LINE_STRIP path.lifetime = rospy.Duration(duration) path.color.r = 1 path.color.g = 0 path.color.b = 1 path.color.a = 1 path.scale.x = 0.01 path.id = marker_id marker_id += 1 for pose in self.trajectory.poses: pos = PoseStamped() pos.header.frame_id = self.trajectory.header.frame_id pos.pose = pose markers = utils.axis_marker(pos, marker_id, "axes") msg.markers.extend(markers) path.points.append(pose.position) marker_id += 3 #3 axes msg.markers.append(path) self.visualizer_pub.publish(msg)
class POIServer(object): ''' Node to act as a server to hold a list of points of interest which can be modified by services or interactive markers ''' 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 transform_position(self, ps): ''' Attempty to transform a PointStamped message into the global frame, returning the position of the transformed point or None if transform failed. ''' # If no frame, assume user wanted it in the global frame if ps.header.frame_id == "": return ps.point try: ps_tf = self.tf_buffer.transform(ps, self.global_frame, timeout=rospy.Duration(5)) return ps_tf.point except tf2_ros.TransformException as e: rospy.logwarn('Error transforming "{}" to "{}": {}'.format( ps.header.frame_id, self.global_frame, e)) return None def process_feedback(self, feedback): ''' Process interactive marker feedback, moving markers internally inresponse to markers moved in RVIZ ''' # Only look at changes when mouse button is unclicked if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP: return # Transform new position into global frame ps = PointStamped() ps.header = feedback.header ps.point = feedback.pose.position position = self.transform_position(ps) if position is None: return # Update position of marker self.update_position(feedback.marker_name, feedback.pose.position) @thread_lock(lock) def save_to_param_cb(self, req): rospy.set_param('~global_frame', self.global_frame) d = {} for poi in self.pois.pois: d[poi.name] = [ float(poi.position.x), float(poi.position.y), float(poi.position.z) ] rospy.set_param('~initial_pois', d) return {'success': True} def add_poi_cb(self, req): ''' Callback for AddPOI service ''' name = req.name # If frame is empty, just initialize it to zero if len(req.position.header.frame_id) == 0: position = numpy_to_point([0., 0., 0.]) # Otherwise transform position into global frame else: position = self.transform_position(req.position) if position is None: return {'success': False, 'message': 'tf error (bad poi)'} if not self.add_poi(name, position): return {'success': False, 'message': 'alread exists (bad poi)'} return {'success': True, 'message': 'good poi'} def delete_poi_cb(self, req): ''' Callback for DeletePOI service ''' # Return error if marker did not exist if not self.remove_poi(req.name): return {'success': False, 'message': 'does not exist (bad poi)'} return {'success': True, 'message': 'good poi'} def move_poi_cb(self, req): ''' Callback for MovePOI service ''' name = req.name # Transform position into global frame position = self.transform_position(req.position) if position is None: return {'success': False, 'message': 'tf error (bad poi)'} # Return error if marker did not exist if not self.update_position(name, position): return {'success': False, 'message': 'does not exist (bad poi)'} return {'success': True, 'message': 'good poi'} @thread_lock(lock) def add_poi(self, name, position): ''' Add a POI, updating clients / rviz when done @return False if POI already exists ''' if not self._add_poi(name, position): return False self.update() return True @thread_lock(lock) def remove_poi(self, name): ''' Remove a POI, updating clients / rviz when done @return False if POI with name does not exist ''' if not self._remove_poi(name): return False self.update() return True @thread_lock(lock) def update_position(self, name, position): ''' Update the position of a POI, updating clients / rviz when done @param position: a Point message of the new position in global frame @return False if POI with name does not exist ''' if not self._update_position(name, position): return False self.update() return True def update(self): ''' Update interactive markers server and POI publish of changes ''' self.pois.header.stamp = rospy.Time.now() self.pub.publish(self.pois) self.interactive_marker_server.applyChanges() def _update_position(self, name, position): ''' Internal implementation of update_position, which is NOT thread safe and does NOT update clients of change ''' # Find poi with specified name for poi in self.pois.pois: if poi.name == name: pose = Pose() pose.orientation.w = 1.0 pose.position = position if not self.interactive_marker_server.setPose(name, pose): return False # Set pose in message poi.position = position return True return False def _remove_poi(self, name): ''' Internal implementation of remove_poi, which is NOT thread safe and does NOT update clients of change ''' # Return false if marker with that name not added to interactive marker server if not self.interactive_marker_server.erase(name): return False # Find POI with specifed name and delete it from list for i, poi in enumerate(self.pois.pois): if poi.name == name: del self.pois.pois[i] return True return False def _add_poi(self, name, position): ''' Internal implementation of add_poi, which is NOT thread safe and does NOT update clients of change ''' if self.interactive_marker_server.get(name) is not None: return False poi = POI() poi.name = name poi.position = position self.pois.pois.append(poi) point_marker = Marker() point_marker.type = Marker.SPHERE point_marker.scale.x = self.marker_scale point_marker.scale.y = self.marker_scale point_marker.scale.z = self.marker_scale point_marker.color.r = 1.0 point_marker.color.g = 1.0 point_marker.color.b = 1.0 point_marker.color.a = 1.0 text_marker = Marker() text_marker.type = Marker.TEXT_VIEW_FACING text_marker.pose.orientation.w = 1.0 text_marker.pose.position.x = 1.5 text_marker.text = poi.name text_marker.scale.z = 1.0 text_marker.color.r = 1.0 text_marker.color.g = 1.0 text_marker.color.b = 1.0 text_marker.color.a = 1.0 int_marker = InteractiveMarker() int_marker.header.frame_id = self.global_frame int_marker.pose.orientation.w = 1.0 int_marker.pose.position = poi.position int_marker.scale = 1 int_marker.name = poi.name # insert a box control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.always_visible = True control.markers.append(point_marker) control.markers.append(text_marker) int_marker.controls.append(control) self.interactive_marker_server.insert(int_marker, self.process_feedback) return True
class PR2TrajectoryMarkers(object): """ A class to create and store a trajectory for one PR2 arm. The created trajectory can be published as a PoseArray message. This class published on the following topics: ~trajectory_markers are the main interactive markers. ~trajectory_poses a markerarray to display the trajectory. ~trajectory_poses a posesarray with the resulting pose The class subscribes to the topic ~overwrite_trajectory_ to change the stored trajectory. This is useful to resume working on a trajectory after re-starting the node. The message type is PoseArray. A std_srvs/Empty service named ~execute_trajectory is provided to externally trigger the execution of the trajectory. Constructor: TrajectoryMarkers(whicharm = "left") or TrajectoryMarkers(whicharm = "right") """ 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 create_menu(self): """ Create and populates all the menu entries """ menu_handler = MenuHandler() menu_handler.insert("Point the head", callback=self.move_head) menu_handler.insert("Add position to trajectory", callback=self.add_point) menu_handler.insert("Place marker over gripper", callback=self.place_gripper) menu_handler.insert("Execute trjectory", callback=self.execute_trajectory) menu_handler.insert("Clear trajectory", callback=self.clear_trajectory) menu_handler.insert("Publish trajectory", callback=self.publish_trajectory) menu_handler.insert("Move the arm (planning)", callback=self.plan_arm) menu_handler.insert("Move the arm (collision-free)", callback=self.collision_free_arm) menu_handler.insert( "Move the arm to trajectory start (collision-free)", callback=self.arm_trajectory_start) menu_handler.insert("Update planning scene", callback=self.update_planning) menu_handler.apply(self.server, self.int_marker.name) def main_callback(self, feedback): """ If the mouse button is released change the gripper color according to the IK result. Quite awkward, trying to get a nicer way to do it. """ if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.last_gripper_pose = feedback.pose #rospy.loginfo("Updating Marker: %d", feedback.event_type) #rospy.loginfo("POS: %s", feedback.pose.position) if (self.last_gripper_pose and feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP): self.last_gripper_pose = None pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): color = None else: color = (1, 0, 0, 1) int_marker = self.server.get(self.int_marker.name) int_marker.controls.remove(self.gripper_marker) self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) #rospy.loginfo("Control: %s", int_marker.controls) self.server.insert(int_marker, self.main_callback) self.server.setPose(int_marker.name, self.last_gripper_pose) self.server.applyChanges() def overwrite_trajectory(self, msg): self.trajectory = msg def add_point(self, feedback): """ Add a point to self.trajectory if it is allowed by IK. """ pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): rospy.loginfo("Pose is reachable") self.trajectory.poses.append(feedback.pose) else: rospy.logerr("Pose is not reachable!") def place_gripper(self, feedback): """ Move the marker where the gripper is """ if self.whicharm == "right": gripper_pos = self.planner.get_right_gripper_pose() else: gripper_pos = self.planner.get_left_gripper_pose() self.server.setPose(self.int_marker.name, gripper_pos.pose, gripper_pos.header) self.server.applyChanges() def execute_trajectory(self, feedback): """ Executes the tracjectory memorized so far. It interpolates between the poses and creates suitable times and velocities. """ traj = self.interpolate_poses() if len(traj) == 0: rospy.logerr("Something went wrong when interpolating") return times, vels = self.ik_utils.trajectory_times_and_vels(traj) if len(vels) == 0 or len(times) == 0: rospy.logerr("Something went wrong when finding the times") return self.joint_controller.execute_trajectory(traj, times, vels, self.whicharm, wait=True) return def execute_trajectory_srv(self, _): """Same as execute_trajectory, but as a service. """ self.execute_trajectory(None) return EmptyResponse() def arm_trajectory_start(self, feedback): """ Move the gripper to the first pose in the trajectory. """ if len(self.trajectory.poses) == 0: rospy.logwarn("Empty trajectory!") return pose = self.trajectory.poses[0] if self.whicharm == "right": moveit = self.planner.move_right_arm_non_collision else: moveit = self.planner.move_left_arm_non_collision pos, quat = create_tuples_from_pose(pose) res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0) if not res: rospy.logerr("Something went wrong when moving") return def clear_trajectory(self, feedback): """ Removes all the points stored so far """ self.trajectory.poses = [] def move_head(self, feedback): """ Moves the head to face the marker """ frame = feedback.header.frame_id pos = ( feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) print "Moving the head" self.joint_controller.time_to_reach = 1.0 self.joint_controller.point_head_to(pos, frame) def plan_arm(self, feedback): """ Moves the arm on the marker using motion collision-aware motion planning. """ frame = feedback.header.frame_id pos = ( feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) orientation = ( feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w, ) if self.whicharm == "right": rospy.loginfo("Moving the right arm") self.planner.move_right_arm(pos, orientation, frame, 2.0) else: rospy.loginfo("Moving the left arm") self.planner.move_left_arm(pos, orientation, frame, 2.0) def collision_free_arm(self, feedback): """ Moves the rm on the marker using motion NON-collision-aware inverse kinematiks. """ frame = feedback.header.frame_id pos = ( feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z, ) orientation = ( feedback.pose.orientation.x, feedback.pose.orientation.y, feedback.pose.orientation.z, feedback.pose.orientation.w, ) if self.whicharm == "right": rospy.loginfo("Moving the right arm (non collision)") self.planner.move_right_arm_non_collision(pos, orientation, frame, 2.0) else: rospy.loginfo("Moving the left arm (non collision)") self.planner.move_left_arm_non_collision(pos, orientation, frame, 2.0) def update_planning(self, feedback): """ Updates the planning scene. """ self.planner.take_static_map() self.planner.update_planning_scene() def publish_trajectory(self, feedback): """ Publishes the trajectory as a PoseArray message """ self.trajectory_pub.publish(self.trajectory) def interpolate_poses(self): """ Refines the trajectory by interpolating between the joints. """ if len(self.trajectory.poses) < 2: rospy.logerr("At least two points in the trajectory are needed") return [] if self.whicharm == "right": starting_angles = self.robot_state.right_arm_positions else: starting_angles = self.robot_state.left_arm_positions all_trajectory = [starting_angles] for i in xrange(len(self.trajectory.poses) - 1): start = PoseStamped() start.header = self.trajectory.header start.pose = self.trajectory.poses[i] end = PoseStamped() end.header = self.trajectory.header end.pose = self.trajectory.poses[i + 1] traj, errs = self.ik_utils.check_cartesian_path( start, end, all_trajectory[-1], #starting_angles, #pos_spacing = 0.05, collision_aware=0, num_steps=5, ) if any(e == 3 for e in errs): rospy.logerr("Error while running IK, codes are: %s", errs) return [] filtered_traj = [t for (t, e) in zip(traj, errs) if e == 0] all_trajectory.extend(filtered_traj) all_trajectory = normalize_trajectory(all_trajectory, starting_angles) rospy.loginfo("New interpolated a trajectory with %d elements", len(all_trajectory)) return all_trajectory def publish_trajectory_markers(self, duration): """ Publishes markers to visualize the current trajectory. Paremeters: duration: how long should the markers visualization last. If this function is called from a loop they last at least the loop rate. """ if len(self.trajectory.poses) == 0: return msg = MarkerArray() marker_id = 0 #creating the path connecting the axes path = Marker() path.header.frame_id = self.trajectory.header.frame_id path.ns = "path" path.action = Marker.ADD path.type = Marker.LINE_STRIP path.lifetime = rospy.Duration(duration) path.color.r = 1 path.color.g = 0 path.color.b = 1 path.color.a = 1 path.scale.x = 0.01 path.id = marker_id marker_id += 1 for pose in self.trajectory.poses: pos = PoseStamped() pos.header.frame_id = self.trajectory.header.frame_id pos.pose = pose markers = utils.axis_marker(pos, marker_id, "axes") msg.markers.extend(markers) path.points.append(pose.position) marker_id += 3 #3 axes msg.markers.append(path) self.visualizer_pub.publish(msg)