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 main(): rospy.init_node('interactive_marker_node') server = InteractiveMarkerServer("simple_marker") # create an interative marker int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.name = "my_marker" int_marker.description = "Simple Click Control" int_marker.pose.position.x = 1 int_marker.pose.orientation.w = 1 # create a cube for the interactive marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.pose.orientation.w = 1 box_marker.scale.x = 0.45 box_marker.scale.y = 0.45 box_marker.scale.z = 0.45 box_marker.color.r = 0.0 box_marker.color.g = 0.5 box_marker.color.b = 0.5 box_marker.color.a = 1.0 # create a control for the interactive marker button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True button_control.markers.append(box_marker) int_marker.controls.append(button_control) server.insert(int_marker, handle_viz_input) server.applyChanges() rospy.spin()
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)
class RvizPathServer(object): 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 add_marker(self, msg, path_id): menu, marker = create_marker(path_msg=msg.path, color_msg=msg.color, description=msg.description, path_id=path_id, width=self.width, delta_z=self.delta_z) self.server.insert(marker, ignore) menu.insert("FOLLOW", callback=self.goto(path_id)) menu.apply(self.server, marker.name) self.paths[path_id] = msg.path def goto(self, path_id): def f(msg): rospy.loginfo("Follow path %d", path_id) self.pub.publish(self.paths[path_id]) return f def updatePaths(self, msg): path_msg = NavPath() path_msg.header.frame_id = 'map' self.pub.publish(path_msg) self.server.clear() for i, m in enumerate(msg.paths): self.add_marker(m, i) self.server.applyChanges()
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()
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()
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 main(): rospy.init_node('interactive_marker_demo') wait_for_time() server = InteractiveMarkerServer("simple_marker") marker = InteractiveMarker() marker.header.frame_id = "base_link" marker.name = "adamson_marker" marker.description = "Simple Click Control" marker.pose.position.x = 1 marker.pose.orientation.w = 1 print("This is my interactive marker:") pprint(marker) box_marker = Marker() box_marker.type = Marker.CUBE box_marker.pose.orientation.w = 1 box_marker.scale.x = 0.45 box_marker.scale.y = 0.45 box_marker.scale.z = 0.45 box_marker.color.r = 0.0 box_marker.color.g = 0.5 box_marker.color.b = 0.5 box_marker.color.a = 1.0 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True button_control.markers.append(box_marker) marker.controls.append(button_control) print("\nAnd this is my normal marker") pprint(box_marker) server.insert(marker, handle_vix_input) server.applyChanges() rospy.spin()
translate_y.interaction_mode = InteractiveMarkerControl.MOVE_AXIS translate_y.orientation.w = 1 translate_y.orientation.x = 0 translate_y.orientation.y = 0 translate_y.orientation.z = 1 translate_y.name = "translate_y" interactive_marker.controls.append(translate_y) translate_z = InteractiveMarkerControl() translate_z.interaction_mode = InteractiveMarkerControl.MOVE_AXIS translate_z.orientation.w = 1 translate_z.orientation.x = 0 translate_z.orientation.y = 1 translate_z.orientation.z = 0 translate_z.name = "translate_z" interactive_marker.controls.append(translate_z) def update_object(feedback): p = feedback.pose.position sim_object.set_position(p.x, p.y, p.z) print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z) # add the interactive marker to our collection & # tell the server to call update_object() when feedback arrives for it server.insert(interactive_marker, update_object) # 'commit' changes and send to all clients server.applyChanges() rospy.spin()
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)
class PickAndPlaceNode(Manager): 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 # self.robot.home() # self.move_calib_position() def move_calib_position(self): # move arm to the calibration position self.calib_pose = PoseStamped( Header(0, rospy.Time(0), self.robot.base), Pose(Point(0.338520675898,-0.175860479474,0.0356775075197), Quaternion(-0.0183493755758,0.708424150944, 0.704712092876, 0.0343413949013))) self.robot.move_ik(self.calib_pose) def update_num_objects(self, msg): self.num_objects = msg.data def _calibrate(self): self.state = "calibrate" self.alignment_pub.publish(Bool(True)) def _perceive(self): self.state = "perceive" rospy.loginfo("Asking for perception...") self.perception_pub.publish(Bool(True)) def _stop_perceive(self): self.state = "perceive" self.perception_pub.publish(Bool(False)) def _post_perceive(self): self.state = "post_perceive" rospy.loginfo("Asking to stop perception...") self.perception_pub.publish(Bool(False)) def _preplace(self): # State not modified until placing succeeds try: obj_to_get = int(self.character) except ValueError: rospy.logerr("Please provide a number in placing mode") return self.state = "preplace" frame_name = "object_pose_{}".format(obj_to_get) rospy.loginfo( "Placing object on top of object {}...".format(obj_to_get)) if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name): t = self.tl.getLatestCommonTime(self.robot.base, frame_name) position, quaternion = self.tl.lookupTransform(self.robot.base, frame_name, t) position = list(position) # Height of cubelet position[2] += self.robot.level * 0.042 # YCB # position[2] += self.robot.level * 0.026 # Update pose position from perception self.place_pose.pose.position = Point(*position) rospy.loginfo("Adjusting place position...") imarker_name = 'place_pose' self.int_markers[imarker_name] = self.place_pose imarker = make_interactive_marker(imarker_name, self.place_pose.pose) # TODO delete imarker at post place self.int_marker_server.insert(imarker, self.imarker_callback) self.int_marker_server.setCallback(imarker_name, self.imarker_callback) self.int_marker_server.applyChanges() rospy.loginfo("imarker stuff done") def imarker_callback(self, msg): # http://docs.ros.org/jade/api/visualization_msgs/html/msg/InteractiveMarkerFeedback.html # noqa rospy.loginfo('imarker_callback called') name = msg.marker_name new_pose = msg.pose self.int_markers[name] = PoseStamped(msg.header, new_pose) def _place(self): self.state = "place" rospy.loginfo("Placing...") place_pose = self.int_markers['place_pose'] # It seems positions and orientations are randomly required to # be actual Point and Quaternion objects or lists/tuples. The # least coherent API ever seen. self.br.sendTransform(Point2list(place_pose.pose.position), Quaternion2list(place_pose.pose.orientation), rospy.Time.now(), "place_pose", self.robot.base) self.robot.place(place_pose) # For cubelets: place_pose.pose.position.z += 0.042 # For YCB: # place_pose.pose.position.z += 0.026 self.place_pose = place_pose def _pick(self): # State not modified until picking succeeds try: obj_to_get = int(self.character) except ValueError: rospy.logerr("Please provide a number in picking mode") return frame_name = "object_pose_{}".format(obj_to_get) rospy.loginfo("Picking object {}...".format(obj_to_get)) if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name): t = self.tl.getLatestCommonTime(self.robot.base, frame_name) position, quaternion = self.tl.lookupTransform(self.robot.base, frame_name, t) print("position", position) print("quaternion", quaternion) position = list(position) # Vertical orientation self.br.sendTransform(position, [1, 0, 0, 0], rospy.Time.now(), "pick_pose", self.robot.base) # Ignore orientation from perception pose = Pose(Point(*position), Quaternion(1, 0, 0, 0)) h = Header() h.stamp = t h.frame_id = self.robot.base stamped_pose = PoseStamped(h, pose) self.robot.pick(stamped_pose) self.state = 'postpick' def _level(self): self.robot.level = int(self.character) self.state = 'level'
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 MarkerTele(object): DISTANCE_MAGNITUDE = 0.5 ANGULAR_MAGNITUDE = math.pi / 6 def __init__(self): self._server = InteractiveMarkerServer("simple_marker") self._base = robot_api.Base() def click_forward(self, input): if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK): self._base.go_forward(self.DISTANCE_MAGNITUDE) def click_backward(self, input): if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK): self._base.go_forward(self.DISTANCE_MAGNITUDE * -1) def click_left(self, input): if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK): self._base.turn(self.ANGULAR_MAGNITUDE) def click_right(self, input): if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK): self._base.turn(self.ANGULAR_MAGNITUDE * -1) def callback(self, msg): forward_int_marker = InteractiveMarker() forward_int_marker.header.frame_id = "base_link" forward_int_marker.name = "forward_marker" forward_int_marker.description = "Forward" forward_int_marker.pose.position.x = 0.5 forward_int_marker.pose.position.z = 0.75 forward_int_marker.pose.orientation.w = 0 forward_marker = Marker() forward_marker.ns = "front_marker" forward_marker.type = Marker.CUBE forward_marker.pose.orientation.w = 0 forward_marker.scale.x = 0.45 forward_marker.scale.y = 0.45 forward_marker.scale.z = 0.45 forward_marker.color.r = 0.0 forward_marker.color.g = 0.5 forward_marker.color.b = 0.5 forward_marker.color.a = 1.0 backward_int_marker = InteractiveMarker() backward_int_marker.header.frame_id = "base_link" backward_int_marker.name = "backward_marker" backward_int_marker.description = "Backward" backward_int_marker.pose.position.x = -0.5 backward_int_marker.pose.position.z = 0.75 backward_int_marker.pose.orientation.w = 0 backward_marker = Marker() backward_marker.ns = "back_marker" backward_marker.type = Marker.CUBE backward_marker.pose.orientation.w = 0 backward_marker.scale.x = 0.45 backward_marker.scale.y = 0.45 backward_marker.scale.z = 0.45 backward_marker.color.r = 0.0 backward_marker.color.g = 0.5 backward_marker.color.b = 0.5 backward_marker.color.a = 1.0 left_int_marker = InteractiveMarker() left_int_marker.header.frame_id = "base_link" left_int_marker.name = "left_marker" left_int_marker.description = "left" left_int_marker.pose.position.y = 0.5 left_int_marker.pose.position.z = 0.75 left_int_marker.pose.orientation.w = 0 left_marker = Marker() left_marker.ns = "left_marker" left_marker.type = Marker.CUBE left_marker.pose.orientation.w = 0 left_marker.scale.x = 0.45 left_marker.scale.y = 0.45 left_marker.scale.z = 0.45 left_marker.color.r = 0.0 left_marker.color.g = 0.5 left_marker.color.b = 0.5 left_marker.color.a = 1.0 right_int_marker = InteractiveMarker() right_int_marker.header.frame_id = "base_link" right_int_marker.name = "right_marker" right_int_marker.description = "right" right_int_marker.pose.position.y = -0.5 right_int_marker.pose.position.z = 0.75 right_int_marker.pose.orientation.w = 0 right_marker = Marker() right_marker.ns = "right_marker" right_marker.type = Marker.CUBE right_marker.pose.orientation.w = 0 right_marker.scale.x = 0.45 right_marker.scale.y = 0.45 right_marker.scale.z = 0.45 right_marker.color.r = 0.0 right_marker.color.g = 0.5 right_marker.color.b = 0.5 right_marker.color.a = 1.0 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True button_control.markers.append(forward_marker) button_control.markers.append(backward_marker) button_control.markers.append(left_marker) button_control.markers.append(right_marker) forward_int_marker.controls.append(button_control) backward_int_marker.controls.append(button_control) left_int_marker.controls.append(button_control) right_int_marker.controls.append(button_control) self._server.insert(forward_int_marker, self.click_forward) self._server.insert(backward_int_marker, self.click_backward) self._server.insert(left_int_marker, self.click_left) self._server.insert(right_int_marker, self.click_right) self._server.applyChanges()
class FakeMarkerServer(): 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') #Service: rosservice call /fake_object_markers/print_templates #Returns a string of template names #ex) list: ['test_template1','test_template2','test_template3'] def get_templates(self, req): temp_list = [] if self.templates.keys() is None: self.publish_feedback('No templates') return for obj in self.templates.keys(): temp_list.append(str(obj)) #print temp_list return PrintTemplatesResponse(temp_list) def store_grasp(self, msg): self.last_grasp = msg.result.grasp # Given a mesh_id creates a name with format 'object + mesh_id' # ex.)Given '1234', creates 'object_1234' name def create_name(self, mesh_id): return 'object_' + str(mesh_id) # Creates a mesh of the given object with the given pose to be visualized by template maker def create_mesh(self, mesh_id, pose): response = self.get_mesh(mesh_id) mesh = response.mesh # build the mesh marker marker = Marker() marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.66 marker.frame_locked = False marker.type = Marker.TRIANGLE_LIST # add the mesh for j in range(len(mesh.triangles)): marker.points.append( mesh.vertices[mesh.triangles[j].vertex_indices[0]]) marker.points.append( mesh.vertices[mesh.triangles[j].vertex_indices[1]]) marker.points.append( mesh.vertices[mesh.triangles[j].vertex_indices[2]]) # create the interactive marker name = self.create_name(mesh_id) self.server.insert(self.create_im(marker, pose, name), self.process_feedback) self.server.setCallback(name, self.release, InteractiveMarkerFeedback.MOUSE_UP) self.server.applyChanges() # Creates an interactive marker # - at the given location and pose # - with a given name # - for given marker object def create_im(self, marker, pose, name): # create the new interactive marker int_marker = InteractiveMarker() int_marker.pose = copy.deepcopy(pose) int_marker.header.frame_id = 'base_link' int_marker.name = name # move freely on the X-Y plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE control.markers.append(marker) control.always_visible = True int_marker.controls.append(control) return int_marker def process_feedback(self, feedback): self.last_feedback = feedback # Returns true if given (x,y) coordinates are within "Graspable/Placeable(?)" range def check_pose(self, x, y): return x >= OFFSET + DEPTH_START and x <= OFFSET + DEPTH_END and y >= WIDTH_START and y <= WIDTH_END # Checks position of hallucinated interactive markers # Changes color and sets position when user releases mouse click (MOUSE_UP) on object def release(self, feedback): im = self.server.get(feedback.marker_name) # copy the mesh information marker = copy.deepcopy(im.controls[0].markers[0]) # determine color based on pose if self.check_pose(feedback.pose.position.x, feedback.pose.position.y): marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 0.66 else: marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.66 # create the new interactive marker self.server.insert( self.create_im(marker, feedback.pose, feedback.marker_name), self.process_feedback) self.server.setCallback(feedback.marker_name, self.release, InteractiveMarkerFeedback.MOUSE_UP) self.server.applyChanges() # updates server def update(self): self.server.applyChanges() # **Run by save_template service** # Saves given template information to file location def save(self, req): # go through each object and check if they are in the valid range to_save = [] for obj_id in self.objects: im = self.server.get(self.create_name(obj_id)) pose = im.pose if (self.check_pose(pose.position.x, pose.position.y)): to_save.append(copy.deepcopy(im)) # check if we have anything to save if len(to_save) > 0: self.templates[req.name] = to_save # PICKLE IT! pickle.dump(self.templates, open(SAVE_FILE, 'wb')) self.play('/home/rctoris/wav/t3_affirmative.wav') self.reset_objects() return SaveTemplateResponse(True) else: return SaveTemplateResponse(False) # Publishes feedback of current tasks def publish_feedback(self, msg): rospy.loginfo(msg) self.load_server.publish_feedback(LoadFeedback(msg)) # Publishes final result of action def publish_result(self, msg): rospy.loginfo(msg) self.load_server.set_succeeded(LoadResult(msg)) # Returns how many objects were recognized def proc_grasp_list(self, msg): objects = [] # start by going through each size = len(msg.graspable_objects) for i in range(size): obj = msg.graspable_objects[i] # only take recognized objects if len(obj.potential_models) is not 0: objects.append(copy.deepcopy(obj)) rospy.loginfo('Found ' + str(len(objects)) + ' object(s).') self.recognition = objects # Drives to and aligns with counter # Segments objects def look_for_objects(self): self.publish_feedback('Driving robot to counter') # drive the robot nav_goal = navigateGoal('Snack Nav', True) self.nav.send_goal_and_wait(nav_goal) res = self.nav.get_result() if not res.success: self.publish_feedback('Counter alignment failed.') return False self.publish_feedback('Aligned robot to counter') self.publish_feedback('Looking for objects') self.play('/home/rctoris/wav/GLaDOS_10_part1_entry-2.wav') self.recognition = None # Segment the table self.segclient.send_goal(UserCommandGoal(request=1, interactive=False)) self.segclient.wait_for_result() while self.recognition is None: time.sleep(1) # Recognize objects self.recognition = None self.segclient.send_goal(UserCommandGoal(request=2, interactive=False)) self.segclient.wait_for_result() while self.recognition is None: time.sleep(1) return True # **Run by load_template service** # Identifies remaining objects needed in template # Moves to and aligns with counter # Scans and recognizes objects on counter that match template # Picks up one object # Backs up from counter # Drives to table # Places object in given template location # Repeats def load(self, goal): name = goal.name self.publish_feedback('Loading template ' + name) # if requested template does not exist... if name not in self.templates.keys(): self.publish_result(name + ' template does not exist') return template = copy.deepcopy(self.templates[name]) self.publish_feedback('Loaded template ' + name) self.play('/home/rctoris/wav/help.wav') # look for any objects we need while len(template) is not 0: pickup_arm = None # if it does not see any objects/could not drive to counter if not self.look_for_objects(): self.publish_result('Object looking failed.') return # for each object in template array... for template_im in template: # for each recognized object for rec_obj in self.recognition: if template_im.name == self.create_name( rec_obj.potential_models[0].model_id): # create the object info for it obj_info = self.create_object_info(rec_obj) # pick it up pickup_arm = self.pickup(rec_obj) # if neither arm can could pick up object... if pickup_arm is None: self.publish_result('Pickup failed.') return # make sure we have a grasp self.publish_feedback('Waiting for grasp') while self.last_grasp is None: rospy.sleep(1) # store the grasp obj_info.grasp = self.last_grasp self.last_grasp = None self.publish_feedback('Grasp found') # good job robot, place that object to_place = Pose() # location of object in template on table to_place.position.z = TABLE_HEIGHT - PLACE_HEIGHT_OFFSET to_place.position.x = template_im.pose.position.x to_place.position.y = template_im.pose.position.y placed = self.place_object(obj_info, pickup_arm, to_place) # if the object could not be placed if not placed: self.publish_result('Place failed.') return self.publish_feedback('Placed the object!') if len(template) is not 1: self.play('/home/rctoris/wav/ill-be-back.wav') # removes object from list of objects to pick up from template template.remove(template_im) # if no objects are found... if pickup_arm is None: # No objects found :( self.publish_result('No objects found that we need :(') return # We completed the task! self.play('/home/rctoris/wav/down.wav') self.publish_result('Great success!') # resets collision map of world and rescan def reset_collision_map(self): self.publish_feedback('Reseting collision map') goal = IMGUIGoal() goal.command.command = 3 goal.options.reset_choice = 4 self.imgui.send_goal(goal) self.imgui.wait_for_result() self.publish_feedback('Collision map reset') # reset hallucinated interactive marker objects' positions on visualized table def reset_objects(self): pose = Pose() pose.position.z = TABLE_HEIGHT pose.position.x = OFFSET * 3 pose.position.y = -0.25 for obj_id in self.objects: self.create_mesh(obj_id, pose) pose.position.y = pose.position.y + 0.25 # Picks up object that matches obj def pickup(self, obj): # start by picking up the object options = IMGUIOptions() options.collision_checked = True options.grasp_selection = 1 options.adv_options.lift_steps = 10 options.adv_options.retreat_steps = 10 options.adv_options.reactive_force = False options.adv_options.reactive_grasping = False options.adv_options.reactive_place = False options.adv_options.lift_direction_choice = 0 # check which arm is closer if obj.potential_models[0].pose.pose.position.y > 0: options.arm_selection = 1 else: options.arm_selection = 0 goal = IMGUIGoal() goal.options = options goal.options.grasp_selection = 1 goal.options.selected_object = obj goal.command.command = goal.command.PICKUP # send it to IMGUI self.publish_feedback('Attempting to pick up') self.reset_collision_map() self.imgui.send_goal(goal) self.play('/home/rctoris/wav/humnbehv.wav') self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: # try the other arm if options.arm_selection is 0: options.arm_selection = 1 else: options.arm_selection = 0 self.publish_feedback('Initial pickup failed, trying other arm') self.reset_collision_map() self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: return None else: # now check if feedback to see if we actually got it if options.arm_selection is 0: arm = 'right' else: arm = 'left' self.publish_feedback('Checking if object was grasped') resp = self.grasp_check(arm) if resp.isGrasping is True: self.publish_feedback('Object was grasped') # attempt to back up backup_goal = BackupGoal() self.backup.send_goal_and_wait(backup_goal) res = self.backup.get_result() # if robot could not back up if not res.success: self.publish_feedback('Backup failed.') return None return options.arm_selection else: self.move_arm_to_side(options.arm_selection) return None # moves arm to sides def move_arm_to_side(self, arm_selection): goal = IMGUIGoal() goal.command.command = 4 goal.options.arm_selection = arm_selection goal.options.arm_action_choice = 0 goal.options.arm_planner_choice = 1 self.publish_feedback('Moving arm to the side using planner') self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: # try open loop self.publish_feedback('Planned arm move failed, trying open loop') goal.options.arm_planner_choice = 0 self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: self.publish_feedback('Arm move failed.') return False else: self.publish_feedback('Arm move was successful') return True # place object in given arm to given pose def place_object(self, obj_info_orig, arm_selection, pose): #drive to the table self.publish_feedback('Driving robot to table') nav_goal = navigateGoal('Dining Table Nav', True) self.play('/home/rctoris/wav/run.wav') self.nav.send_goal_and_wait(nav_goal) res = self.nav.get_result() if not res.success: self.publish_feedback('Table alignment failed.') return False self.publish_feedback('Aligned robot to table') self.reset_collision_map() self.publish_feedback('Attempting to place the object') # make a copy obj_info = copy.deepcopy(obj_info_orig) obj = obj_info.obj goal = PlaceGoal() # set the arm if arm_selection is 0: goal.arm_name = 'right_arm' prefix = 'r' else: goal.arm_name = 'left_arm' prefix = 'l' # rotate and "gu-chunk" orig_z = pose.position.z pose.orientation.x = 0 pose.orientation.y = 0 goal.place_locations = [] #iterating through possible x-locations to place object for x in range(0, 10): pose.position.x = pose.position.x + ((x - 5) * 0.0025) #iterating through possible y-locations to place object for y in range(0, 10): pose.position.y = pose.position.y + ((y - 5) * 0.0025) # 'i' is for some rotations for i in range(0, 10): rads = (pi * (i / 10.0)) pose.orientation.z = sin(-rads / 2.0) pose.orientation.w = cos(-rads / 2.0) # 'j' is for the 'z' height for j in range(0, 6): pose.position.z = orig_z + (j * 0.0025) pose_mat = pose_to_mat(pose) to_base_link_mat = pose_mat * obj_info.obj_origin_to_bounding_box grasp_mat = pose_to_mat(obj_info.grasp.grasp_pose.pose) gripper_mat = to_base_link_mat * grasp_mat gripper_pose = stamp_pose(mat_to_pose(gripper_mat), 'base_link') goal.place_locations.append(gripper_pose) # use the identity as the grasp obj_info.grasp.grasp_pose.pose = Pose() obj_info.grasp.grasp_pose.pose.orientation.w = 1 goal.grasp = obj_info.grasp goal.desired_retreat_distance = 0.1 goal.min_retreat_distance = 0.05 # set the approach goal.approach = GripperTranslation() goal.approach.desired_distance = .1 goal.approach.min_distance = 0.05 goal.approach.direction = create_vector3_stamped([0., 0., -1.], 'base_link') # set the collision info goal.collision_object_name = obj.collision_name goal.collision_support_surface_name = 'table' goal.place_padding = 0.0 goal.additional_link_padding = self.create_gripper_link_padding(prefix) goal.collision_support_surface_name = 'all' goal.allow_gripper_support_collision = True goal.use_reactive_place = False # send the goal self.place.send_goal(goal) # wait for result finished_within_time = self.place.wait_for_result(rospy.Duration(240)) if not finished_within_time: self.place.cancel_goal() return False # check the result res = self.place.get_result() if res.manipulation_result.value == -6 or res.manipulation_result.value == 1: # attempt to back up backup_goal = BackupGoal() self.backup.send_goal_and_wait(backup_goal) backup_res = self.backup.get_result() # if robot could not back up if not backup_res.success: self.publish_feedback('Backup failed.') return False self.move_arm_to_side(arm_selection) return True else: return False def create_gripper_link_padding(self, prefix): link_name_list = [ '_gripper_palm_link', '_gripper_r_finger_tip_link', '_gripper_l_finger_tip_link', '_gripper_l_finger_link', '_gripper_r_finger_link', '_wrist_roll_link' ] pad = 0. arm_link_names = [prefix + link_name for link_name in link_name_list] link_padding_list = [ LinkPadding(link_name, pad) for link_name in arm_link_names ] return link_padding_list def create_object_info(self, obj): # get the pose pose_stamped = obj.potential_models[0].pose # change the frame obj_frame_pose_stamped = change_pose_stamped_frame( self.tf_listener, pose_stamped, obj.reference_frame_id) return ObjectInfo(obj, obj_frame_pose_stamped, self.tf_listener)
def main(): rospy.init_node('base_demo') wait_for_time() server = InteractiveMarkerServer("drive_marker") int_f_marker = InteractiveMarker() int_f_marker.header.frame_id = "base_link" int_f_marker.name = "foward_marker" int_f_marker.description = "Forward Control" int_f_marker.pose.position.x = 2 int_f_marker.pose.orientation.w = 1 f_marker = Marker() f_marker.type = Marker.CUBE f_marker.pose.orientation.w = 2 f_marker.scale.x = 0.45 f_marker.scale.y = 0.45 f_marker.scale.z = 0.45 f_marker.color.r = 0.0 f_marker.color.g = 0.5 f_marker.color.b = 0.5 f_marker.color.a = 1.0 int_cw_marker = InteractiveMarker() int_cw_marker.header.frame_id = "base_link" int_cw_marker.name = "cw_marker" int_cw_marker.description = "Clockwise Turn Control" int_cw_marker.pose.position.y = -2 int_cw_marker.pose.orientation.w = 1 f_button_control = InteractiveMarkerControl() f_button_control.interaction_mode = InteractiveMarkerControl.BUTTON f_button_control.always_visible = True f_button_control.markers.append(f_marker) int_f_marker.controls.append(f_button_control) cw_marker = Marker() cw_marker.type = Marker.SPHERE cw_marker.pose.orientation.w = 2 cw_marker.scale.x = 0.45 cw_marker.scale.y = 0.45 cw_marker.scale.z = 0.45 cw_marker.color.r = 0.0 cw_marker.color.g = 0.5 cw_marker.color.b = 0.5 cw_marker.color.a = 1.0 cw_button_control = InteractiveMarkerControl() cw_button_control.interaction_mode = InteractiveMarkerControl.BUTTON cw_button_control.always_visible = True cw_button_control.markers.append(cw_marker) int_cw_marker.controls.append(cw_button_control) int_ccw_marker = InteractiveMarker() int_ccw_marker.header.frame_id = "base_link" int_ccw_marker.name = "ccw_marker" int_ccw_marker.description = "Counter-Clockwise Turn Control" int_ccw_marker.pose.position.y = 2 int_ccw_marker.pose.orientation.w = 1 ccw_marker = Marker() ccw_marker.type = Marker.SPHERE ccw_marker.pose.orientation.w = 1 ccw_marker.scale.x = 0.45 ccw_marker.scale.y = 0.45 ccw_marker.scale.z = 0.45 ccw_marker.color.r = 0.5 ccw_marker.color.g = 0.0 ccw_marker.color.b = 0.5 ccw_marker.color.a = 1.0 ccw_button_control = InteractiveMarkerControl() ccw_button_control.interaction_mode = InteractiveMarkerControl.BUTTON ccw_button_control.always_visible = True ccw_button_control.markers.append(ccw_marker) int_ccw_marker.controls.append(ccw_button_control) server.insert(int_f_marker, handle_f_input) server.insert(int_cw_marker, handle_cw_input) server.insert(int_ccw_marker, handle_ccw_input) rospy.sleep(0.5) server.applyChanges() rospy.spin()
class World: '''Object recognition and localization related stuff''' tf_listener = None objects = [] 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 receive_ar_markers(self, data): '''Callback function to receive marker info''' self._lock.acquire() if len(data.markers) > 0: rospy.loginfo('Received non-empty marker list.') for i in range(len(data.markers)): marker = data.markers[i] self._add_new_object(marker.pose.pose, self.marker_dims, marker.id) self._lock.release() def _reset_objects(self): '''Function that removes all objects''' self._lock.acquire() for i in range(len(World.objects)): self._im_server.erase(World.objects[i].int_marker.name) self._im_server.applyChanges() if self.surface != None: self._remove_surface() self._im_server.clear() self._im_server.applyChanges() World.objects = [] self._lock.release() def get_tool_id(self): if (len(self.objects) == 0): rospy.warning('There are no fiducials, cannot get tool ID.') return None elif (len(self.objects) > 1): rospy.warning('There are more than one fiducials, returning the first as tool ID.') return World.objects[0].marker_id def get_surface(self): if (len(self.objects) < 4): rospy.warning('There are not enough fiducials to detect surface.') return None elif (len(self.objects) > 4): rospy.warning('There are more than four fiducials for surface, will use first four.') return points = [World.objects[0].position, World.objects[1].position, World.objects[2].position, World.objects[3].position] s = Surface(points) return s def receive_table_marker(self, marker): '''Callback function for markers to determine table''' if (marker.type == Marker.LINE_STRIP): if (len(marker.points) == 6): rospy.loginfo('Received a TABLE marker.') xmin = marker.points[0].x ymin = marker.points[0].y xmax = marker.points[2].x ymax = marker.points[2].y depth = xmax - xmin width = ymax - ymin pose = Pose(marker.pose.position, marker.pose.orientation) pose.position.x = pose.position.x + xmin + depth / 2 pose.position.y = pose.position.y + ymin + width / 2 dimensions = Vector3(depth, width, 0.01) self.surface = World._get_surface_marker(pose, dimensions) self._im_server.insert(self.surface, self.marker_feedback_cb) self._im_server.applyChanges() def receive_object_info(self, object_list): '''Callback function to receive object info''' self._lock.acquire() rospy.loginfo('Received recognized object list.') if (len(object_list.graspable_objects) > 0): for i in range(len(object_list.graspable_objects)): models = object_list.graspable_objects[i].potential_models if (len(models) > 0): object_pose = None best_confidence = 0.0 for j in range(len(models)): if (best_confidence < models[j].confidence): object_pose = models[j].pose.pose best_confidence = models[j].confidence if (object_pose != None): rospy.logwarn('Adding the recognized object ' + 'with most confident model.') self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), i, object_list.meshes[i]) else: rospy.logwarn('... this is not a recognition result, ' + 'it is probably just segmentation.') cluster = object_list.graspable_objects[i].cluster bbox = self._bb_service(cluster) cluster_pose = bbox.pose.pose if (cluster_pose != None): rospy.loginfo('Adding unrecognized object with pose:' + World.pose_to_string(cluster_pose) + '\n' + 'In ref frame' + str(bbox.pose.header.frame_id)) self._add_new_object(cluster_pose, bbox.box_dims, i) else: rospy.logwarn('... but the list was empty.') self._lock.release() @staticmethod def get_pose_from_transform(transform): '''Returns pose for transformation matrix''' 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 get_matrix_from_pose(pose): '''Returns the transformation matrix for given 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_absolute_pose(arm_state): '''Returns absolute pose of an end effector state''' if (arm_state.refFrame == ArmState.OBJECT): arm_state_copy = ArmState(arm_state.refFrame, Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation), arm_state.joint_pose[:], arm_state.refFrameObject) World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE) return arm_state_copy.ee_pose else: return arm_state.ee_pose @staticmethod def _get_mesh_marker(marker, mesh): '''Function that generated a marker from a mesh''' marker.type = Marker.TRIANGLE_LIST index = 0 marker.scale = Vector3(1.0, 1.0, 1.0) while (index + 2 < len(mesh.triangles)): if ((mesh.triangles[index] < len(mesh.vertices)) and (mesh.triangles[index + 1] < len(mesh.vertices)) and (mesh.triangles[index + 2] < len(mesh.vertices))): marker.points.append(mesh.vertices[mesh.triangles[index]]) marker.points.append(mesh.vertices[mesh.triangles[index + 1]]) marker.points.append(mesh.vertices[mesh.triangles[index + 2]]) index += 3 else: rospy.logerr('Mesh contains invalid triangle!') break return marker def _add_new_object(self, pose, dimensions, id=None, mesh=None): '''Function to add new objects''' dist_threshold = 0.02 to_remove = None for i in range(len(World.objects)): if (World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold): rospy.loginfo('Previously detected object at the same' + 'location, will not add this object.') return False n_objects = len(World.objects) World.objects.append(WorldObject(pose, n_objects, dimensions, id)) int_marker = self._get_object_marker(len(World.objects) - 1) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True def _remove_object(self, to_remove): '''Function to remove object by index''' obj = World.objects.pop(to_remove) rospy.loginfo('Removing object ' + obj.int_marker.name) self._im_server.erase(obj.int_marker.name) self._im_server.applyChanges() def _remove_surface(self): '''Function to request removing surface''' rospy.loginfo('Removing surface') self._im_server.erase('surface') self._im_server.applyChanges() self.surface = None def _get_object_marker(self, index, mesh=None): '''Generate a marker for world objects''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.2, 0.8, 0.0, 0.6), pose=World.objects[index].object.pose) if (mesh != None): object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = (World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06) button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) return int_marker @staticmethod def _get_surface_marker(pose, dimensions): ''' Function that generates a surface marker''' int_marker = InteractiveMarker() int_marker.name = 'surface' int_marker.header.frame_id = 'base_link' int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=2000, lifetime=rospy.Duration(2), scale=dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.8, 0.0, 0.4, 0.4), pose=pose) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1))) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker @staticmethod def get_frame_list(): '''Function that returns the list of ref. frames''' objects = [] for i in range(len(World.objects)): objects.append(World.objects[i].object) return objects @staticmethod def has_objects(): '''Function that checks if there are any objects''' return len(World.objects) > 0 @staticmethod def get_ref_from_name(ref_name): '''Ref. frame type from ref. frame name''' if ref_name == 'base_link': return ArmState.ROBOT_BASE else: return ArmState.OBJECT @staticmethod def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()): '''Transforms an arm frame to a new ref. frame''' if ref_frame == ArmState.ROBOT_BASE: if (arm_frame.refFrame == ArmState.ROBOT_BASE): pass #rospy.logwarn('No reference frame transformations ' + #'needed (both absolute).') elif (arm_frame.refFrame == ArmState.OBJECT): abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, 'base_link') arm_frame.ee_pose = abs_ee_pose arm_frame.refFrame = ArmState.ROBOT_BASE arm_frame.refFrameObject = Object() else: rospy.logerr('Unhandled reference frame conversion:' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) elif ref_frame == ArmState.OBJECT: if (arm_frame.refFrame == ArmState.ROBOT_BASE): rel_ee_pose = World.transform(arm_frame.ee_pose, 'base_link', ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj elif (arm_frame.refFrame == ArmState.OBJECT): if (arm_frame.refFrameObject.name == ref_frame_obj.name): pass #rospy.logwarn('No reference frame transformations ' + #'needed (same object).') else: rel_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj else: rospy.logerr('Unhandled reference frame conversion:' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) return arm_frame @staticmethod def has_object(object_name): '''Checks if the world contains the object''' for obj in World.objects: if obj.object.name == object_name: return True return False @staticmethod def is_frame_valid(object_name): '''Checks if the frame is valid for transforms''' return (object_name == 'base_link') or World.has_object(object_name) @staticmethod def transform(pose, from_frame, to_frame): ''' Function to transform a pose between two ref. frames if there is a TF exception or object does not exist it will return the pose back without any transforms''' if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame): pose_stamped = PoseStamped() try: common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame) pose_stamped.header.stamp = common_time pose_stamped.header.frame_id = from_frame pose_stamped.pose = pose rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped) return rel_ee_pose.pose except tf.Exception: rospy.logerr('TF exception during transform.') return pose except rospy.ServiceException: rospy.logerr('Exception during transform.') return pose else: rospy.logwarn('One of the frame objects might not exist: ' + from_frame + ' or ' + to_frame) return pose @staticmethod def pose_to_string(pose): '''For printing a pose to stdout''' return ('Position: ' + str(pose.position.x) + ", " + str(pose.position.y) + ', ' + str(pose.position.z) + '\n' + 'Orientation: ' + str(pose.orientation.x) + ", " + str(pose.orientation.y) + ', ' + str(pose.orientation.z) + ', ' + str(pose.orientation.w) + '\n') def _publish_tf_pose(self, pose, name, parent): ''' Publishes a TF for object pose''' if (pose != None): pos = (pose.position.x, pose.position.y, pose.position.z) rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent) def update_object_pose(self): ''' Function to externally update an object pose''' Response.perform_gaze_action(GazeGoal.LOOK_DOWN) while (Response.gaze_client.get_state() == GoalStatus.PENDING or Response.gaze_client.get_state() == GoalStatus.ACTIVE): time.sleep(0.1) if (Response.gaze_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not look down to take table snapshot') return False rospy.loginfo('Looking at table now.') goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Object recognition has been reset.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) self._reset_objects() if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not reset recognition.') return False # Do segmentation goal = UserCommandGoal(UserCommandGoal.SEGMENT, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Table segmentation is complete.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED): rospy.logerr('Could not segment.') return False # Do recognition goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Objects on the table have been recognized.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) # Record the result if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED): wait_time = 0 total_wait_time = 5 while (not World.has_objects() and wait_time < total_wait_time): time.sleep(0.1) wait_time += 0.1 if (not World.has_objects()): rospy.logerr('Timeout waiting for a recognition result.') return False else: rospy.loginfo('Got the object list.') return True else: rospy.logerr('Could not recognize.') return False def clear_all_objects(self): '''Removes all objects from the world''' goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while (self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING): time.sleep(0.1) rospy.loginfo('Object recognition has been reset.') rospy.loginfo('STATUS: ' + self._object_action_client.get_goal_status_text()) if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('Successfully reset object localization pipeline.') self._reset_objects() self._remove_surface() def get_nearest_object(self, arm_pose): '''Gives a pointed to the nearest object''' dist_threshold = 0.4 def chObj(cur, obj): dist = World.pose_distance(obj.object.pose, arm_pose) return (dist, obj.object) if (dist < cur[0]) else cur return reduce(chObj, World.objects, (dist_threshold, None))[1] @staticmethod def pose_distance(pose1, pose2, is_on_table=True): '''Distance between two world poses''' if pose1 == [] or pose2 == []: return 0.0 else: if (is_on_table): arr1 = array([pose1.position.x, pose1.position.y]) arr2 = array([pose2.position.x, pose2.position.y]) else: arr1 = array([pose1.position.x, pose1.position.y, pose1.position.z]) arr2 = array([pose2.position.x, pose2.position.y, pose2.position.z]) dist = norm(arr1 - arr2) if dist < 0.0001: dist = 0 return dist def marker_feedback_cb(self, feedback): '''Callback for when feedback from a marker is received''' if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Clicked on object ' + str(feedback.marker_name)) rospy.loginfo('Number of objects ' + str(len(World.objects))) else: rospy.loginfo('Unknown event' + str(feedback.event_type)) def update(self): '''Update function called in a loop''' # Visualize the detected object is_world_changed = False self._lock.acquire() if (World.has_objects()): to_remove = None for i in range(len(World.objects)): self._publish_tf_pose(World.objects[i].object.pose, World.objects[i].get_name(), 'base_link') if (World.objects[i].is_removed): to_remove = i if to_remove != None: self._remove_object(to_remove) is_world_changed = True self._lock.release() return is_world_changed
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() # 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 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.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] 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.') joint_positions = self.ik.get_ik_for_ee(self.ee_pose) print 'joint: ' + str(joint_positions) if(joint_positions != None): self.toggle_arm(self.side_prefix, 'Freeze', False) #joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309] print 'returned from IK: ' + str(joint_positions) #print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]) #joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124] self.move_to_joints(self.side_prefix, list(joint_positions), 5.0) print 'done' def toggle_arm(self, side, toggle, button): controller_name = side + '_arm_controller' print 'toggle' start_controllers = [] stop_controllers = [] if (toggle == 'Relax'): stop_controllers.append(controller_name) else: start_controllers.append(controller_name) self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=rospy.Duration(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) 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])) 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 mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6) ik_solution = self.ik.get_ik_for_ee(self.ee_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) 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 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 InteractiveMarkerInterfaceServer: def __init__(self): rospy.init_node('interactive_marker_node') self._server = InteractiveMarkerServer("interactive_marker_interface") # create the interative markers self._forward_marker = InteractiveMarker() self._forward_marker.header.frame_id = 'base_link' self._forward_marker.name = "forward_marker" self._forward_marker.description = "Click to go forward 0.5m" self._forward_marker.pose.position = Point(-0.5,0,-0.5) self._forward_marker.pose.orientation.w = 1 self._cw_marker = InteractiveMarker() self._cw_marker.header.frame_id = 'base_link' self._cw_marker.name = "cw_marker" self._cw_marker.description = "Click to turn 30 degrees clockwise" self._cw_marker.pose.position = Point(-0.5,1,-0.5) self._cw_marker.pose.orientation.w = 1 self._ccw_marker = InteractiveMarker() self._ccw_marker.header.frame_id = 'base_link' self._ccw_marker.name = "ccw_marker" self._ccw_marker.description = "Click to turn 30 degrees counterclockwise" self._ccw_marker.pose.position = Point(-0.5,-1,-0.5) self._ccw_marker.pose.orientation.w = 1 # create a cube for the interactive marker self._forward_box_marker = Marker( type=Marker.CUBE, id=1, scale=Vector3(0.45, 0.45, 0.45), pose=Pose(Point(-0.5,0,0),Quaternion(0,0,0,1)), header=Header(frame_id='base_link', stamp = rospy.Time.now()), color=ColorRGBA(0, 0.5, 0.5, 1.0)) self._cw_box_marker = Marker( type=Marker.CUBE, id=2, scale=Vector3(0.45, 0.45, 0.45), pose=Pose(Point(-0.5,1,0),Quaternion(0,0,0,1)), header=Header(frame_id='base_link', stamp = rospy.Time.now()), color=ColorRGBA(0, 0.5, 0.5, 1.0)) self._ccw_box_marker = Marker( type=Marker.CUBE, id=3, scale=Vector3(0.45, 0.45, 0.45), pose=Pose(Point(-0.5,-1,0),Quaternion(0,0,0,1)), header=Header(frame_id='base_link', stamp = rospy.Time.now()), color=ColorRGBA(0, 0.5, 0.5, 1.0)) # create a control for the interactive marker self._forward_control = InteractiveMarkerControl() self._forward_control.interaction_mode = InteractiveMarkerControl.BUTTON self._forward_control.always_visible = True self._forward_control.markers.append(self._forward_box_marker) self._forward_marker.controls.append(self._forward_control) self._cw_control = InteractiveMarkerControl() self._cw_control.interaction_mode = InteractiveMarkerControl.BUTTON self._cw_control.always_visible = True self._cw_control.markers.append(self._cw_box_marker) self._cw_marker.controls.append(self._cw_control) self._ccw_control = InteractiveMarkerControl() self._ccw_control.interaction_mode = InteractiveMarkerControl.BUTTON self._ccw_control.always_visible = True self._ccw_control.markers.append(self._ccw_box_marker) self._ccw_marker.controls.append(self._ccw_control) self._server.insert(self._forward_marker, self.handle_input) self._server.insert(self._cw_marker, self.handle_input) self._server.insert(self._ccw_marker, self.handle_input) self._server.applyChanges() self._base = robot_api.Base() def handle_input(self, input): if input.event_type == InteractiveMarkerFeedback.MOUSE_UP and input.marker_name == "forward_marker": self._base.go_forward(0.5, speed=1) elif input.event_type == InteractiveMarkerFeedback.MOUSE_UP and input.marker_name == "cw_marker": self._base.turn(-30.0 * math.pi / 180.0, speed=1) elif input.event_type == InteractiveMarkerFeedback.MOUSE_UP and input.marker_name == "ccw_marker": self._base.turn(30.0 * math.pi / 180.0, speed=1) else: # rospy.loginfo('Cannot handle this InteractiveMarker event') pass
class TopologyVisualiser(object): """Visualise topology graph in rviz (and edit it)""" 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 save(self): with open(self.network_file, 'w') as file_obj: node_dict = [node.to_dict() for node in self.nodes.values()] yaml.dump({'nodes': node_dict}, file_obj, default_flow_style=False) yaml.dump({'connections': self.connections}, file_obj, default_flow_style=False) def command_cb(self, msg): command = msg.data if 'add connection' in command or 'del connection' in command: command_list = command.split() if len(command_list) != 4: self.print_command_warning() return try: conn1 = int(command_list[2]) conn2 = int(command_list[3]) except Exception as e: self.print_command_warning() return if conn1 not in self.nodes: rospy.logwarn(command_list[2] + ' not in nodes list') return if conn1 not in self.nodes: rospy.logwarn(command_list[2] + ' not in nodes list') return if command_list[0] == 'add': self.connections.append([conn1, conn2]) rospy.loginfo('Added connection') else: connection = [conn1, conn2] if connection in self.connections: self.connections.remove(connection) elif connection[::-1] in self.connections: self.connections.remove(connection[::-1]) else: rospy.logwarn(str(connection) + ' not in connections list') return rospy.loginfo('Deleted connection') self._topology_pub.publish(self.get_marker_array()) rospy.sleep(0.5) else: self.print_command_warning() def print_command_warning(self): rospy.logwarn('Invalid command format.') print('Valid commands are:') for i in ["add connection", "del connection"]: print('-', i) print('Examples:') print('- "add connection 123 234"') print('- "del connection 123 234"') def clicked_point_cb(self, msg): rospy.loginfo(msg) self.node_id_counter += 1 node_id = self.node_id_counter self.nodes[node_id] = Node(node_id, msg.point.x, msg.point.y, "BRSU_area", "area") self._topology_pub.publish(self.get_marker_array()) self._add_interactive_marker(msg.point.x, msg.point.y, node_id) rospy.loginfo('Added node successfully') rospy.sleep(0.5) def get_marker_array(self): """Return a MarkerArray object representing the graph formed by topology nodes and connections :returns: visualization_msgs.MarkerArray """ marker_array = MarkerArray() for i, node in enumerate(self.nodes.values()): marker_msg_text = Marker(type=Marker.TEXT_VIEW_FACING, id=node.id) marker_msg_text.text = str(node.id) + " (" + str( node.area_type) + ")" marker_msg_text.scale.z = 0.5 marker_msg_text.color.a = 1.0 marker_msg_text.pose.position.x = node.x marker_msg_text.pose.position.y = node.y marker_msg_text.header.stamp = rospy.Time.now() marker_msg_text.header.frame_id = self.frame marker_array.markers.append(marker_msg_text) for i, conn in enumerate(self.connections): start_node = self.nodes[conn[0]] end_node = self.nodes[conn[1]] marker = Marker(type=Marker.LINE_STRIP, id=10000 + i) marker.points.append(Point(x=start_node.x, y=start_node.y)) marker.points.append(Point(x=end_node.x, y=end_node.y)) marker.header.stamp = rospy.Time.now() marker.header.frame_id = self.frame marker.color.b = marker.color.a = 1.0 marker.scale.x = 0.05 marker_array.markers.append(marker) return marker_array def _add_interactive_marker(self, x, y, node_id): """adds a interactive marker at the position. :x: float :y: float :node_id: int or None :returns: None """ marker = Utils.get_2_dof_interactive_marker(str(node_id), self.frame, x, y) self.interactive_marker_server.insert(marker, self.interactive_marker_cb) self.interactive_marker_server.applyChanges() def interactive_marker_cb(self, feedback): """Updates the control point when interactive marker is moved. :feedback: InteractiveMarkerFeedback :returns: None """ if not feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: return p = feedback.pose.position node_id = int(feedback.marker_name) if node_id in self.nodes: self.nodes[node_id].x = round(p.x, 2) self.nodes[node_id].y = round(p.y, 2) else: rospy.logerr("Incorrect interactive marker id " + feedback.marker_name) # should never come here self._topology_pub.publish(self.get_marker_array()) rospy.sleep(0.5)
class InteractiveMarkerManager(object): 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 _process_feedback(self, feedback): s = "Feedback from marker '" + feedback.marker_name s += "' / control '" + feedback.control_name + "'" mp = "" if feedback.mouse_point_valid: mp = " at " + str(feedback.mouse_point.x) mp += ", " + str(feedback.mouse_point.y) mp += ", " + str(feedback.mouse_point.z) mp += " in frame " + feedback.header.frame_id if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.logdebug(s + ": button click" + mp + ".") pass elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") self._process_menu_select(feedback) elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback rospy.logdebug(s + ": pose changed") self._update_waypoint_position(feedback.marker_name, feedback.pose) elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: rospy.logdebug(s + ": mouse down" + mp + ".") pass elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.logdebug(s + ": mouse up" + mp + ".") pass self.server.applyChanges() def _process_menu_select(self, feedback): menu_entry_id = feedback.menu_entry_id if menu_entry_id == self._menu_cmds['del_wp']: self._delete_waypoint(feedback) elif menu_entry_id == self._menu_cmds['ins_wp_before']: self._insert_waypoint(feedback, before=True) elif menu_entry_id == self._menu_cmds['ins_wp_after']: self._insert_waypoint(feedback, before=False) elif menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']: self._process_menu_time_cmds(feedback) else: pass def _delete_waypoint(self, feedback): marker = self.name_to_marker_dict[feedback.marker_name] waypoint = self.name_to_waypoint_dict[marker.name] self.parent_instance_handle.delete_waypoint(waypoint) # Update dictionaries del self.name_to_marker_dict[marker.name] del self.waypoint_to_marker_dict[waypoint] del self.name_to_waypoint_dict[marker.name] del self._menu_handlers[marker.name] del self._menu_cmds[marker.name] # Erase marker from server self.server.erase(feedback.marker_name) self.server.applyChanges() self.marker_cnt -=1 self._int_marker_name_list.remove(marker.name) self._update_wp_marker_descriptions() self.server.applyChanges() def _insert_waypoint(self, feedback, before=True): ref_marker = self.name_to_marker_dict[feedback.marker_name] ref_waypoint = self.name_to_waypoint_dict[ref_marker.name] self.parent_instance_handle.insert_waypoint(ref_waypoint, before=before) self._update_wp_marker_descriptions() self.server.applyChanges() def _process_menu_time_cmds(self, feedback): menu_entry_handle = feedback.menu_entry_id menu_handler = self._menu_handlers[feedback.marker_name] check_state = menu_handler.getCheckState(menu_entry_handle) if check_state == MenuHandler.UNCHECKED: menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED) # Uncheck all other menu entries for menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']: if menu_entry_id != menu_entry_handle: menu_handler.setCheckState(menu_entry_id, MenuHandler.UNCHECKED) # Update waypoints waypoint = self.name_to_waypoint_dict[feedback.marker_name] waypoint.time = self._menu_cmds[feedback.marker_name]['menu_time_cmds'][menu_entry_handle] # Apply marker changes menu_handler.reApply(self.server) self.server.applyChanges() def _update_waypoint_pose(self): # TODO maybe handle pose changes differently? pass def _update_waypoint_position(self, marker_name, marker_pose): waypoint = self.name_to_waypoint_dict[marker_name] eff_pos = marker_pose.position eff_orient = marker_pose.orientation target_jt_angles = self.ik_solver.get_ik(waypoint.joint_state.positions, eff_pos.x, eff_pos.y, eff_pos.z, eff_orient.w, eff_orient.x, eff_orient.y, eff_orient.z, self.ik_position_xyz_bounds[0], self.ik_position_xyz_bounds[1], self.ik_position_xyz_bounds[2], self.ik_position_rpy_bounds[0], self.ik_position_rpy_bounds[1], self.ik_position_rpy_bounds[2]) if target_jt_angles is not None: waypoint.end_effector_pose = copy.deepcopy(marker_pose) waypoint.joint_state.positions = target_jt_angles self._change_wp_marker_color(marker_name, "white", 0.9) else: self._change_wp_marker_color(marker_name, "red", 0.9) def _change_wp_marker_color(self, wp_marker_name, color, opacity=1.0): assert 0.0 <= opacity <= 1.0 # set color int_marker = self.server.get(wp_marker_name) marker = int_marker.controls[0].markers[0] replacement_int_marker = copy.deepcopy(int_marker) replacement_marker = replacement_int_marker.controls[0].markers[0] rgb = None if color == "white": rgb = (1.0, 1.0, 1.0) elif color == "red": rgb = (1.0, 0.0, 0.0) # TODO: Additional colors here changed = False if rgb is not None: if marker.color.r != rgb[0]: replacement_marker.color.r = rgb[0] changed = True print("change to ", rgb) if marker.color.g != rgb[1]: replacement_marker.color.g = rgb[1] changed = True if marker.color.b != rgb[2]: replacement_marker.color.b = rgb[2] changed = True if marker.color.a != opacity: replacement_marker.color.a = opacity changed = True # update wp marker if changed: self._replace_wp_marker(int_marker, replacement_int_marker) def _update_wp_marker_descriptions(self): for index, marker_name in enumerate(self._int_marker_name_list): marker = self.name_to_marker_dict[marker_name] marker_description = int(marker.description) if marker_description != index+1: marker.description = str(index+1) replacement_marker = copy.deepcopy(self.server.get(marker.name)) replacement_marker.description = marker.description self._replace_wp_marker(marker, replacement_marker) def _replace_wp_marker(self, old_int_marker, replacement_int_marker): # Update dictionaries self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker self.waypoint_to_marker_dict[self.name_to_waypoint_dict[old_int_marker.name]] = replacement_int_marker # Erase marker from server self.server.erase(old_int_marker.name) self.server.insert(replacement_int_marker, self._process_feedback) self.server.applyChanges() def add_waypoint_marker(self, waypoint, description=None): self.markers_created_cnt += 1 self.marker_cnt += 1 int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.pose.position = waypoint.end_effector_pose.position int_marker.scale = 0.1 int_marker.name = str(self.markers_created_cnt) if description is None: int_marker.description = int_marker.name else: int_marker.description = description self.name_to_marker_dict[int_marker.name] = int_marker self.waypoint_to_marker_dict[waypoint] = int_marker self.name_to_waypoint_dict[int_marker.name] = waypoint # insert a box # TODO: may change geometry / eff mesh make_box_control_marker(int_marker) int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) self.server.insert(int_marker, self._process_feedback) # add menus menu_handler = MenuHandler() del_sub_menu_handle = menu_handler.insert("Delete Waypoint") self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback) menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback) ins_sub_menu_handle = menu_handler.insert("Insert Waypoint") self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback) self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback) menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback) # Set time time_sub_menu_handle = menu_handler.insert("Set Waypoint Time") menu_time_cmds = {} time = self.name_to_waypoint_dict[int_marker.name].time time_list = [float(num) for num in range((int(m.ceil(time))))] time_list.append(time) time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)]) self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds} for t in time_list: time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback) menu_time_cmds[time_opt_handle] = t if t == time: menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED) else: menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED) menu_handler.apply(self.server, int_marker.name) self._menu_handlers[int_marker.name] = menu_handler self.server.applyChanges() if int(int_marker.description)-1 < len(self._int_marker_name_list): self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name) else: self._int_marker_name_list.append(int_marker.name) def clear_waypoint_markers(self): self.server.clear() self.server.applyChanges() self._menu_handlers = {} self._menu_cmds = {} 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 = {}
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 Projection: 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 setup_marker(self, frame="velodyne", name = "capture vehicle", translation=True): int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.name = name int_marker.description = name int_marker.scale = 3 marker_control = InteractiveMarkerControl() marker_control.always_visible = True marker_control.markers.append(self.marker) int_marker.controls.append(marker_control) control = InteractiveMarkerControl() control.name = "rotate_x" control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "rotate_z" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "rotate_y" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) if not translation : #int_marker.pose.position = Point(0,0,0) return int_marker control = InteractiveMarkerControl() control.name = "move_x" control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "move_z" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "move_y" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) return int_marker def add_bbox(self): inputName = '/image_raw' rospy.Subscriber(inputName, Image, self.handle_img_msg, queue_size=1) def handle_img_msg(self, img_msg): now = rospy.get_rostime() img = None bridge = cv_bridge.CvBridge() try: img = bridge.imgmsg_to_cv2(img_msg, 'bgr8') except cv_bridge.CvBridgeError as e: rospy.logerr( 'image message to cv conversion failed :' ) rospy.logerr( e ) print( e ) return self.frame_index = self.timestamp_map[img_msg.header.stamp.to_nsec()] f = self.frame_map[self.frame_index][0] obs_centroid = np.array(f.trans) + np.array(self.offset) R = tf.transformations.quaternion_matrix(self.rotation_offset) rotated_centroid = R.dot(list(obs_centroid)+[1]) obs_centroid = rotated_centroid[:3] #self.orient = list(f.rotq) self.marker.header.stamp = now self.marker.pose.position = Point(*list(obs_centroid)) self.marker.pose.orientation = Quaternion(*self.orient) self.obs_marker.pose.position = Point(*list(obs_centroid)) self.obs_marker.pose.orientation = Quaternion(*self.orient) self.add_bbox_lidar() md = self.metadata dims = np.array([md['l'], md['w'], md['h']]) outputName = '/image_bbox' if obs_centroid is None: rospy.loginfo("Couldn't find obstacle centroid") imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) return # print centroid info rospy.loginfo(str(obs_centroid)) # case when obstacle is not in camera frame if obs_centroid[0]<2.5 : self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) return # get bbox R = tf.transformations.quaternion_matrix(self.orient) corners = [0.5*np.array([i,j,k])*dims for i in [-1,1] for j in [-1,1] for k in [-1,1]] corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners] projected_pts = [] cameraModel = PinholeCameraModel() cam_info = load_cam_info(self.calib_file) cameraModel.fromCameraInfo(cam_info) projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners] projected_pts = np.array(projected_pts) center = np.mean(projected_pts, axis=0) out_img = drawBbox(img, projected_pts) self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8')) def processFeedback(self, feedback ): p = feedback.pose.orientation self.rotation_offset = [p.x, p.y, p.z, p.w] p = feedback.pose.position self.offset = [p.x, p.y, p.z] self.server.applyChanges() def obs_processFeedback(self, feedback ): p = feedback.pose.orientation self.orient = (p.x, p.y, p.z, p.w) self.marker.pose.orientation = Quaternion(*self.orient) self.server.applyChanges() def add_bbox_lidar(self): #if obs_centroid is None : # return now = rospy.get_rostime() self.velodyne_marker.header.stamp = now self.obs_marker.header.stamp = now # tell the server to call processFeedback() when feedback arrives for it self.server.insert(self.velodyne_marker, self.processFeedback) self.server.applyChanges() self.server.insert(self.obs_marker, self.obs_processFeedback) self.server.applyChanges()
class InteractiveMarkerGoal(object): def __init__(self, root_link, tip_links): # tf self.tfBuffer = Buffer(rospy.Duration(1)) self.tf_listener = TransformListener(self.tfBuffer) # giskard goal client # self.client = SimpleActionClient('move', ControllerListAction) self.client = SimpleActionClient('qp_controller/command', ControllerListAction) self.client.wait_for_server() # marker server self.server = InteractiveMarkerServer("eef_control") self.menu_handler = MenuHandler() for tip_link in tip_links: int_marker = self.make6DofMarker( InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link) self.server.insert( int_marker, self.process_feedback(self.server, self.client, root_link, tip_link)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges() def transformPose(self, target_frame, pose, time=None): transform = self.tfBuffer.lookup_transform( target_frame, pose.header.frame_id, pose.header.stamp if time is not None else rospy.Time(0), rospy.Duration(1.0)) new_pose = do_transform_pose(pose, transform) return new_pose def makeBox(self, msg): marker = Marker() marker.type = Marker.SPHERE marker.scale.x = msg.scale * 0.2 marker.scale.y = msg.scale * 0.2 marker.scale.z = msg.scale * 0.2 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 0.5 return marker def makeBoxControl(self, msg): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self.makeBox(msg)) msg.controls.append(control) return control def make6DofMarker(self, interaction_mode, root_link, tip_link): def normed_q(x, y, z, w): return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w]) int_marker = InteractiveMarker() p = PoseStamped() p.header.frame_id = tip_link p.pose.orientation.w = 1 int_marker.header.frame_id = tip_link int_marker.pose.orientation.w = 1 int_marker.pose = self.transformPose(root_link, p).pose int_marker.header.frame_id = root_link int_marker.scale = .2 int_marker.name = "eef_{}_to_{}".format(root_link, tip_link) # insert a box self.makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = interaction_mode if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D: "MOVE_3D", InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D", InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D" } int_marker.name += "_" + control_modes_dict[interaction_mode] control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 1, 0, 1)) control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 1, 0, 1)) control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 0, 1, 1)) control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 0, 1, 1)) control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) return int_marker class process_feedback(object): def __init__(self, i_server, client, root_link, tip_link): self.i_server = i_server self.client = client self.tip_link = tip_link self.root_link = root_link def __call__(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: print('interactive goal update') goal = ControllerListGoal() goal.type = ControllerListGoal.STANDARD_CONTROLLER # asd = 'asd' # translation controller = Controller() controller.type = Controller.TRANSLATION_3D controller.tip_link = self.tip_link controller.root_link = self.root_link controller.goal_pose.header = feedback.header controller.goal_pose.pose = feedback.pose controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.05 controller.weight = 1.0 goal.controllers.append(controller) # rotation controller = Controller() controller.type = Controller.ROTATION_3D controller.tip_link = self.tip_link controller.root_link = self.root_link controller.goal_pose.header = feedback.header controller.goal_pose.pose = feedback.pose controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.2 controller.weight = 1.0 goal.controllers.append(controller) self.client.send_goal(goal) self.i_server.applyChanges()
class TrajectoryAnalyzer(): 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) def get_poses_persecond(self): average_poses = 0 for uuid in self._traj: traj = self._traj[uuid] inner_counter = 1 outer_counter = 1 prev_sec = traj.secs[0] for i, sec in enumerate(traj.secs[1:]): if prev_sec == sec: inner_counter += 1 else: prev_sec = sec outer_counter += 1 average_poses += round(inner_counter/outer_counter) return round(average_poses/len(self._traj)) def _retrieve_logs(self): logs = self._client.message_store.people_perception.find() for log in logs: for i, uuid in enumerate(log['uuids']): if uuid not in self._traj: t = Trajectory(uuid) t.append_pose(log['people'][i], log['header']['stamp']['secs'], log['header']['stamp']['nsecs']) self._traj[uuid] = t else: t = self._traj[uuid] t.append_pose(log['people'][i], log['header']['stamp']['secs'], log['header']['stamp']['nsecs']) def visualize_trajectories(self, mode="all", average_length=0, longest_length=0): counter = 0 for uuid in self._traj: if len(self._traj[uuid].pose) > 1: if mode == "average": if abs(self._traj[uuid].length - average_length) \ < (average_length / 10): self.visualize_trajectory(self._traj[uuid]) counter += 1 elif mode == "longest": if abs(self._traj[uuid].length - longest_length) \ < (longest_length / 10): self.visualize_trajectory(self._traj[uuid]) counter += 1 elif mode == "shortest": if self._traj[uuid].length < 1: self.visualize_trajectory(self._traj[uuid]) counter += 1 else: self.visualize_trajectory(self._traj[uuid]) counter += 1 rospy.loginfo("Total Trajectories: " + str(len(self._traj))) rospy.loginfo("Printed trajectories: " + str(counter)) self.delete_trajectory(self._traj[uuid]) def _update_cb(self, feedback): return def visualize_trajectory(self, traj): int_marker = self.create_trajectory_marker(traj) self._server.insert(int_marker, self._update_cb) self._server.applyChanges() def delete_trajectory(self, traj): self._server.erase(traj.uuid) self._server.applyChanges() def create_trajectory_marker(self, traj): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = traj.uuid # int_marker.description = traj.uuid pose = Pose() pose.position.x = traj.pose[0]['position']['x'] pose.position.y = traj.pose[0]['position']['y'] int_marker.pose = pose line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.05 # random.seed(traj.uuid) # val = random.random() # line_marker.color.r = r_func(val) # line_marker.color.g = g_func(val) # line_marker.color.b = b_func(val) # line_marker.color.a = 1.0 line_marker.points = [] MOD = 3 for i, point in enumerate(traj.pose): if i % MOD == 0: x = point['position']['x'] y = point['position']['y'] p = Point() p.x = x - int_marker.pose.position.x p.y = y - int_marker.pose.position.y line_marker.points.append(p) line_marker.colors = [] for i, vel in enumerate(traj.vel): if i % MOD == 0: color = ColorRGBA() if traj.max_vel == 0: val = vel / 0.01 else: val = vel / traj.max_vel color.r = r_func(val) color.g = g_func(val) color.b = b_func(val) color.a = 1.0 line_marker.colors.append(color) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.markers.append(line_marker) int_marker.controls.append(control) return int_marker def trajectory_visualization(self, mode): average_length = 0 longest_length = -1 short_trajectories = 0 average_max_vel = 0 highest_max_vel = -1 minimal_frame = self.get_poses_persecond() * 5 for k, v in self._traj.items(): v.sort_pose() v.calc_stats() # Delete non-moving objects if (v.max_vel < 0.1 or v.length < 0.1) and k in self._traj: del self._traj[k] # Delete trajectories that appear less than 5 seconds if len(v.pose) < minimal_frame and k in self._traj: del self._traj[k] for k, v in self._traj.iteritems(): average_length += v.length average_max_vel += v.max_vel if v.length < 1: short_trajectories += 1 if longest_length < v.length: longest_length = v.length if highest_max_vel < v.max_vel: highest_max_vel = v.max_vel average_length /= len(self._traj) average_max_vel /= len(self._traj) rospy.loginfo("Average length of tracks is " + str(average_length)) rospy.loginfo("Longest length of tracks is " + str(longest_length)) rospy.loginfo("Short trajectories are " + str(short_trajectories)) rospy.loginfo("Average maximum velocity of tracks is " + str(average_max_vel)) rospy.loginfo("Highest maximum velocity of tracks is " + str(highest_max_vel)) self.visualize_trajectories(mode, average_length, longest_length)
class InteractiveMarkerPoseStampedPublisher(): def __init__(self, topic, initial_x, initial_y, initial_z, initial_roll, initial_pitch, initial_yaw, frame_id): self.server = InteractiveMarkerServer("posestamped_im") self.menu_handler = MenuHandler() self.pub = rospy.Publisher(topic, PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.resolved_name)) pose = Pose() pose.position.x = initial_x pose.position.y = initial_y pose.position.z = initial_z quat = quaternion_from_euler(initial_roll, initial_pitch, initial_yaw) pose.orientation = Quaternion(*quat) self.initial_pose = pose self.frame_id = frame_id # self.makeMenuMarker(pose) self.makeGraspIM(pose) self.server.applyChanges() def processFeedback(self, feedback): """ :type feedback: InteractiveMarkerFeedback """ s = "Feedback from marker '" + feedback.marker_name s += "' / control '" + feedback.control_name + "'" mp = "" if feedback.mouse_point_valid: mp = " at " + str(feedback.mouse_point.x) mp += ", " + str(feedback.mouse_point.y) mp += ", " + str(feedback.mouse_point.z) mp += " in frame " + feedback.header.frame_id if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo(s + ": button click" + mp + ".") elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: rospy.loginfo(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") # When clicking this event triggers! elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: rospy.loginfo(s + ": pose changed") ps = PoseStamped() ps.header.frame_id = self.frame_id ps.pose = feedback.pose self.pub.publish(ps) # TODO # << "\nposition = " # << feedback.pose.position.x # << ", " << feedback.pose.position.y # << ", " << feedback.pose.position.z # << "\norientation = " # << feedback.pose.orientation.w # << ", " << feedback.pose.orientation.x # << ", " << feedback.pose.orientation.y # << ", " << feedback.pose.orientation.z # << "\nframe: " << feedback.header.frame_id # << " time: " << feedback.header.stamp.sec << "sec, " # << feedback.header.stamp.nsec << " nsec" ) elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: rospy.loginfo(s + ": mouse down" + mp + ".") elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.loginfo(s + ": mouse up" + mp + ".") self.server.applyChanges() def makeArrow(self, msg): marker = Marker() marker.type = Marker.ARROW marker.scale.x = 0.15 # msg.scale * 0.3 marker.scale.y = 0.08 # msg.scale * 0.1 marker.scale.z = 0.03 # msg.scale * 0.03 marker.color.r = 0.3 marker.color.g = 0.3 marker.color.b = 0.5 marker.color.a = 1.0 return marker def makeBoxControl(self, msg): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self.makeArrow(msg)) msg.controls.append(control) return control def makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.frame_id int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "" # "EEF 6DOF control" # insert a box, well, an arrow self.makeBoxControl(int_marker) int_marker.controls[ 0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D int_marker.controls.append(control) # self.menu_handler.insert("Do stuff", callback=self.processFeedback) # This makes the floating text appear # make one control using default visuals # control = InteractiveMarkerControl() # control.interaction_mode = InteractiveMarkerControl.MENU # control.description="Menu" # control.name = "menu_only_control" # int_marker.controls.append(copy.deepcopy(control)) # marker = self.makeArrow( int_marker ) # control.markers.append( marker ) # control.always_visible = False # int_marker.controls.append(control) self.server.insert(int_marker, self.processFeedback)
class FakeMarkerServer(): 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') #Service: rosservice call /fake_object_markers/print_templates #Returns a string of template names #ex) list: ['test_template1','test_template2','test_template3'] def get_templates(self, req): temp_list = [] if self.templates.keys() is None: self.publish_feedback('No templates') return for obj in self.templates.keys(): temp_list.append(str(obj)) #print temp_list return PrintTemplatesResponse(temp_list) def store_grasp(self, msg): self.last_grasp = msg.result.grasp # Given a mesh_id creates a name with format 'object + mesh_id' # ex.)Given '1234', creates 'object_1234' name def create_name(self, mesh_id): return 'object_' + str(mesh_id) # Creates a mesh of the given object with the given pose to be visualized by template maker def create_mesh(self, mesh_id, pose): response = self.get_mesh(mesh_id) mesh = response.mesh # build the mesh marker marker = Marker() marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.66 marker.frame_locked = False marker.type = Marker.TRIANGLE_LIST # add the mesh for j in range(len(mesh.triangles)): marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[0]]) marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[1]]) marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[2]]) # create the interactive marker name = self.create_name(mesh_id) self.server.insert(self.create_im(marker, pose, name), self.process_feedback) self.server.setCallback(name, self.release, InteractiveMarkerFeedback.MOUSE_UP) self.server.applyChanges() # Creates an interactive marker # - at the given location and pose # - with a given name # - for given marker object def create_im(self, marker, pose, name): # create the new interactive marker int_marker = InteractiveMarker() int_marker.pose = copy.deepcopy(pose) int_marker.header.frame_id = 'base_link' int_marker.name = name # move freely on the X-Y plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE control.markers.append(marker) control.always_visible = True int_marker.controls.append(control) return int_marker def process_feedback(self, feedback): self.last_feedback = feedback # Returns true if given (x,y) coordinates are within "Graspable/Placeable(?)" range def check_pose(self, x, y): return x >= OFFSET + DEPTH_START and x <= OFFSET + DEPTH_END and y >= WIDTH_START and y <= WIDTH_END # Checks position of hallucinated interactive markers # Changes color and sets position when user releases mouse click (MOUSE_UP) on object def release(self, feedback): im = self.server.get(feedback.marker_name) # copy the mesh information marker = copy.deepcopy(im.controls[0].markers[0]) # determine color based on pose if self.check_pose(feedback.pose.position.x, feedback.pose.position.y): marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 0.66 else: marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.66 # create the new interactive marker self.server.insert(self.create_im(marker, feedback.pose, feedback.marker_name), self.process_feedback) self.server.setCallback(feedback.marker_name, self.release, InteractiveMarkerFeedback.MOUSE_UP) self.server.applyChanges() # updates server def update(self): self.server.applyChanges() # **Run by save_template service** # Saves given template information to file location def save(self, req): # go through each object and check if they are in the valid range to_save = [] for obj_id in self.objects: im = self.server.get(self.create_name(obj_id)) pose = im.pose if (self.check_pose(pose.position.x, pose.position.y)): to_save.append(copy.deepcopy(im)) # check if we have anything to save if len(to_save) > 0: self.templates[req.name] = to_save # PICKLE IT! pickle.dump(self.templates, open(SAVE_FILE, 'wb')) self.play('/home/rctoris/wav/t3_affirmative.wav') self.reset_objects() return SaveTemplateResponse(True) else: return SaveTemplateResponse(False) # Publishes feedback of current tasks def publish_feedback(self, msg): rospy.loginfo(msg) self.load_server.publish_feedback(LoadFeedback(msg)) # Publishes final result of action def publish_result(self, msg): rospy.loginfo(msg) self.load_server.set_succeeded(LoadResult(msg)) # Returns how many objects were recognized def proc_grasp_list(self, msg): objects = [] # start by going through each size = len(msg.graspable_objects) for i in range(size): obj = msg.graspable_objects[i] # only take recognized objects if len(obj.potential_models) is not 0: objects.append(copy.deepcopy(obj)) rospy.loginfo('Found ' + str(len(objects)) + ' object(s).') self.recognition = objects # Drives to and aligns with counter # Segments objects def look_for_objects(self): self.publish_feedback('Driving robot to counter') # drive the robot nav_goal = navigateGoal('Snack Nav', True) self.nav.send_goal_and_wait(nav_goal) res = self.nav.get_result() if not res.success: self.publish_feedback('Counter alignment failed.') return False self.publish_feedback('Aligned robot to counter') self.publish_feedback('Looking for objects') self.play('/home/rctoris/wav/GLaDOS_10_part1_entry-2.wav') self.recognition = None # Segment the table self.segclient.send_goal(UserCommandGoal(request=1,interactive=False)) self.segclient.wait_for_result() while self.recognition is None: time.sleep(1) # Recognize objects self.recognition = None self.segclient.send_goal(UserCommandGoal(request=2,interactive=False)) self.segclient.wait_for_result() while self.recognition is None: time.sleep(1) return True # **Run by load_template service** # Identifies remaining objects needed in template # Moves to and aligns with counter # Scans and recognizes objects on counter that match template # Picks up one object # Backs up from counter # Drives to table # Places object in given template location # Repeats def load(self, goal): name = goal.name self.publish_feedback('Loading template ' + name) # if requested template does not exist... if name not in self.templates.keys(): self.publish_result(name + ' template does not exist') return template = copy.deepcopy(self.templates[name]) self.publish_feedback('Loaded template ' + name) self.play('/home/rctoris/wav/help.wav') # look for any objects we need while len(template) is not 0: pickup_arm = None # if it does not see any objects/could not drive to counter if not self.look_for_objects(): self.publish_result('Object looking failed.') return # for each object in template array... for template_im in template: # for each recognized object for rec_obj in self.recognition: if template_im.name == self.create_name(rec_obj.potential_models[0].model_id): # create the object info for it obj_info = self.create_object_info(rec_obj) # pick it up pickup_arm = self.pickup(rec_obj) # if neither arm can could pick up object... if pickup_arm is None: self.publish_result('Pickup failed.') return # make sure we have a grasp self.publish_feedback('Waiting for grasp') while self.last_grasp is None: rospy.sleep(1) # store the grasp obj_info.grasp = self.last_grasp self.last_grasp = None self.publish_feedback('Grasp found') # good job robot, place that object to_place = Pose() # location of object in template on table to_place.position.z = TABLE_HEIGHT - PLACE_HEIGHT_OFFSET to_place.position.x = template_im.pose.position.x to_place.position.y = template_im.pose.position.y placed = self.place_object(obj_info, pickup_arm, to_place) # if the object could not be placed if not placed: self.publish_result('Place failed.') return self.publish_feedback('Placed the object!') if len(template) is not 1: self.play('/home/rctoris/wav/ill-be-back.wav') # removes object from list of objects to pick up from template template.remove(template_im) # if no objects are found... if pickup_arm is None: # No objects found :( self.publish_result('No objects found that we need :(') return # We completed the task! self.play('/home/rctoris/wav/down.wav') self.publish_result('Great success!') # resets collision map of world and rescan def reset_collision_map(self): self.publish_feedback('Reseting collision map') goal = IMGUIGoal() goal.command.command = 3 goal.options.reset_choice = 4 self.imgui.send_goal(goal) self.imgui.wait_for_result() self.publish_feedback('Collision map reset') # reset hallucinated interactive marker objects' positions on visualized table def reset_objects(self): pose = Pose() pose.position.z = TABLE_HEIGHT pose.position.x = OFFSET * 3 pose.position.y = -0.25 for obj_id in self.objects: self.create_mesh(obj_id, pose) pose.position.y = pose.position.y + 0.25 # Picks up object that matches obj def pickup(self, obj): # start by picking up the object options = IMGUIOptions() options.collision_checked = True options.grasp_selection = 1 options.adv_options.lift_steps = 10 options.adv_options.retreat_steps = 10 options.adv_options.reactive_force = False options.adv_options.reactive_grasping = False options.adv_options.reactive_place = False options.adv_options.lift_direction_choice = 0 # check which arm is closer if obj.potential_models[0].pose.pose.position.y > 0: options.arm_selection = 1 else: options.arm_selection = 0 goal = IMGUIGoal() goal.options = options goal.options.grasp_selection = 1 goal.options.selected_object = obj goal.command.command = goal.command.PICKUP # send it to IMGUI self.publish_feedback('Attempting to pick up') self.reset_collision_map() self.imgui.send_goal(goal) self.play('/home/rctoris/wav/humnbehv.wav') self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: # try the other arm if options.arm_selection is 0: options.arm_selection = 1 else: options.arm_selection = 0 self.publish_feedback('Initial pickup failed, trying other arm') self.reset_collision_map() self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: return None else: # now check if feedback to see if we actually got it if options.arm_selection is 0: arm = 'right' else: arm = 'left' self.publish_feedback('Checking if object was grasped') resp = self.grasp_check(arm) if resp.isGrasping is True: self.publish_feedback('Object was grasped') # attempt to back up backup_goal = BackupGoal() self.backup.send_goal_and_wait(backup_goal) res = self.backup.get_result() # if robot could not back up if not res.success: self.publish_feedback('Backup failed.') return None return options.arm_selection else: self.move_arm_to_side(options.arm_selection) return None # moves arm to sides def move_arm_to_side(self, arm_selection): goal = IMGUIGoal() goal.command.command = 4 goal.options.arm_selection = arm_selection goal.options.arm_action_choice = 0 goal.options.arm_planner_choice = 1 self.publish_feedback('Moving arm to the side using planner') self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: # try open loop self.publish_feedback('Planned arm move failed, trying open loop') goal.options.arm_planner_choice = 0 self.imgui.send_goal(goal) self.imgui.wait_for_result() # check the result res = self.imgui.get_result() if res.result.value is not 1: self.publish_feedback('Arm move failed.') return False else: self.publish_feedback('Arm move was successful') return True # place object in given arm to given pose def place_object(self, obj_info_orig, arm_selection, pose): #drive to the table self.publish_feedback('Driving robot to table') nav_goal = navigateGoal('Dining Table Nav', True) self.play('/home/rctoris/wav/run.wav') self.nav.send_goal_and_wait(nav_goal) res = self.nav.get_result() if not res.success: self.publish_feedback('Table alignment failed.') return False self.publish_feedback('Aligned robot to table') self.reset_collision_map() self.publish_feedback('Attempting to place the object') # make a copy obj_info = copy.deepcopy(obj_info_orig) obj = obj_info.obj goal = PlaceGoal() # set the arm if arm_selection is 0: goal.arm_name = 'right_arm' prefix = 'r' else: goal.arm_name = 'left_arm' prefix = 'l' # rotate and "gu-chunk" orig_z = pose.position.z pose.orientation.x = 0 pose.orientation.y = 0 goal.place_locations = [] #iterating through possible x-locations to place object for x in range(0, 10): pose.position.x = pose.position.x + ((x - 5) * 0.0025) #iterating through possible y-locations to place object for y in range(0, 10): pose.position.y = pose.position.y + ((y - 5) * 0.0025) # 'i' is for some rotations for i in range(0, 10): rads = (pi * (i/10.0)) pose.orientation.z = sin(-rads/2.0) pose.orientation.w = cos(-rads/2.0); # 'j' is for the 'z' height for j in range (0, 6): pose.position.z = orig_z + (j * 0.0025) pose_mat = pose_to_mat(pose) to_base_link_mat = pose_mat * obj_info.obj_origin_to_bounding_box grasp_mat = pose_to_mat(obj_info.grasp.grasp_pose.pose) gripper_mat = to_base_link_mat * grasp_mat gripper_pose = stamp_pose(mat_to_pose(gripper_mat), 'base_link') goal.place_locations.append(gripper_pose) # use the identity as the grasp obj_info.grasp.grasp_pose.pose = Pose() obj_info.grasp.grasp_pose.pose.orientation.w = 1 goal.grasp = obj_info.grasp goal.desired_retreat_distance = 0.1 goal.min_retreat_distance = 0.05 # set the approach goal.approach = GripperTranslation() goal.approach.desired_distance = .1 goal.approach.min_distance = 0.05 goal.approach.direction = create_vector3_stamped([0. , 0. , -1.], 'base_link') # set the collision info goal.collision_object_name = obj.collision_name goal.collision_support_surface_name = 'table' goal.place_padding = 0.0 goal.additional_link_padding = self.create_gripper_link_padding(prefix) goal.collision_support_surface_name = 'all' goal.allow_gripper_support_collision = True goal.use_reactive_place = False # send the goal self.place.send_goal(goal) # wait for result finished_within_time = self.place.wait_for_result(rospy.Duration(240)) if not finished_within_time: self.place.cancel_goal() return False # check the result res = self.place.get_result() if res.manipulation_result.value == -6 or res.manipulation_result.value == 1: # attempt to back up backup_goal = BackupGoal() self.backup.send_goal_and_wait(backup_goal) backup_res = self.backup.get_result() # if robot could not back up if not backup_res.success: self.publish_feedback('Backup failed.') return False self.move_arm_to_side(arm_selection) return True else: return False def create_gripper_link_padding(self, prefix): link_name_list = ['_gripper_palm_link', '_gripper_r_finger_tip_link', '_gripper_l_finger_tip_link', '_gripper_l_finger_link', '_gripper_r_finger_link', '_wrist_roll_link'] pad = 0. arm_link_names = [prefix + link_name for link_name in link_name_list] link_padding_list = [LinkPadding(link_name, pad) for link_name in arm_link_names] return link_padding_list def create_object_info(self, obj): # get the pose pose_stamped = obj.potential_models[0].pose # change the frame obj_frame_pose_stamped = change_pose_stamped_frame(self.tf_listener, pose_stamped, obj.reference_frame_id) return ObjectInfo(obj, obj_frame_pose_stamped, self.tf_listener)
class InteractiveMarkerPoseStampedPublisher(): 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 processFeedback(self, feedback): """ :type feedback: InteractiveMarkerFeedback """ s = "Feedback from marker '" + feedback.marker_name s += "' / control '" + feedback.control_name + "'" mp = "" if feedback.mouse_point_valid: mp = " at " + str(feedback.mouse_point.x) mp += ", " + str(feedback.mouse_point.y) mp += ", " + str(feedback.mouse_point.z) mp += " in frame " + feedback.header.frame_id if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.logdebug(s + ": button click" + mp + ".") elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") if feedback.menu_entry_id == 1: rospy.logdebug("Start publishing transform...") if self.tf_br is None: self.tf_br = tf.TransformBroadcaster() elif feedback.menu_entry_id == 2: rospy.logdebug("Stop publishing transform...") self.tf_br = None # When clicking this event triggers! elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: rospy.logdebug(s + ": pose changed") elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: rospy.logdebug(s + ": mouse down" + mp + ".") elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.logdebug(s + ": mouse up" + mp + ".") # TODO: Print here the commands # tf static transform self.print_commands(feedback.pose) self.last_pose = deepcopy(feedback.pose) self.server.applyChanges() def print_commands(self, pose, decimals=4): p = pose.position o = pose.orientation print "\n---------------------------" print "Static transform publisher command (with roll pitch yaw):" common_part = "rosrun tf static_transform_publisher " pos_part = str(round(p.x, decimals)) + " " + str(round( p.y, decimals)) + " " + str(round(p.z, decimals)) r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) ori_part = str(round(r, decimals)) + " " + str(round( p, decimals)) + " " + str(round(y, decimals)) static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" print " " + static_tf_cmd print print "Static transform publisher command (with quaternion):" ori_q = str(round(o.x, decimals)) + " " + str(round( o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str( round(o.w, decimals)) static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50" print " " + static_tf_cmd print print "Roslaunch line of static transform publisher (rpy):" node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf" roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\ pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />' print roslaunch_part print print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" /> print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />' print "\n---------------------------" def makeArrow(self, msg): marker = Marker() # An arrow that's squashed to easier view the orientation on roll marker.type = Marker.ARROW marker.scale.x = 0.08 marker.scale.y = 0.08 marker.scale.z = 0.08 marker.color.r = 0.3 marker.color.g = 0.3 marker.color.b = 0.5 marker.color.a = 1.0 return marker def makeBoxControl(self, msg): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self.makeArrow(msg)) msg.controls.append(control) return control def makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.from_frame int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame # insert a box, well, an arrow self.makeBoxControl(int_marker) int_marker.controls[ 0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.menu_handler.insert("Publish transform", callback=self.processFeedback) self.menu_handler.insert("Stop publishing transform", callback=self.processFeedback) self.server.insert(int_marker, self.processFeedback) self.menu_handler.apply(self.server, int_marker.name) def run(self): r = pi / 2.0 p = pi y = pi / 2.0 o = quaternion_from_euler(r, p, y) r = rospy.Rate(20) while not rospy.is_shutdown(): if self.tf_br is not None: pos = self.last_pose.position ori = self.last_pose.orientation self.tf_br.sendTransform( (pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w), rospy.Time.now(), self.to_frame, self.from_frame) ps = PoseStamped() ps.pose = self.last_pose ps.header.frame_id = self.from_frame ps.header.stamp = rospy.Time.now() self.pub.publish(ps) r.sleep()
class Projection: 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 setup_marker(self, frame="velodyne", name = "capture vehicle", translation=True): int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.name = name int_marker.description = name int_marker.scale = 3 marker_control = InteractiveMarkerControl() marker_control.always_visible = True marker_control.markers.append(self.marker) int_marker.controls.append(marker_control) control = InteractiveMarkerControl() control.name = "rotate_x" control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "rotate_z" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "rotate_y" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) if not translation : #int_marker.pose.position = Point(0,0,0) return int_marker control = InteractiveMarkerControl() control.name = "move_x" control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "move_z" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.name = "move_y" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) return int_marker def reset(self): self.last_cap_r = None self.last_cap_f = None self.last_cap_yaw = None self.obs_centroid = None def track_obstacle(self) : obj_topics = { 'cap_r': '/objects/capture_vehicle/rear/gps/rtkfix', 'cap_f': '/objects/capture_vehicle/front/gps/rtkfix', 'obs_r': '/objects/obs1/rear/gps/rtkfix' } for obj in obj_topics: rospy.Subscriber(obj_topics[obj], Odometry, self.handle_msg, obj) def handle_msg(self, msg, who): assert isinstance(msg, Odometry) now = rospy.get_rostime() if now < self.current_time : self.reset() self.current_time = now if who == 'cap_r': self.last_cap_r = rtk_position_to_numpy(msg) elif who == 'cap_f' and self.last_cap_r is not None: cap_f = rtk_position_to_numpy(msg) cap_r = self.last_cap_r self.last_cap_f = cap_f self.last_cap_yaw = get_yaw(cap_f, cap_r) elif who == 'obs_r' and self.last_cap_f is not None and self.last_cap_yaw is not None: md = self.metadata # find obstacle rear RTK to centroid vector lrg_to_gps = [md['rear_gps_l'], -md['rear_gps_w'], md['rear_gps_h']] lrg_to_centroid = [md['l'] / 2., -md['w'] / 2., md['h'] / 2.] obs_r_to_centroid = np.subtract(lrg_to_centroid, lrg_to_gps) # in the fixed GPS frame cap_f = self.last_cap_f obs_r = rtk_position_to_numpy(msg) # in the capture vehicle velodyne frame cap_to_obs = np.dot(rotMatZ(-self.last_cap_yaw), obs_r - cap_f) cap_to_obs_centroid = cap_to_obs + obs_r_to_centroid velo_to_front = np.array([-1.0922, 0, -0.0508]) cap_to_obs_centroid += velo_to_front self.obs_centroid = cap_to_obs_centroid + np.array(self.offset) R = tf.transformations.quaternion_matrix(self.rotation_offset) rotated_centroid = R.dot(list(self.obs_centroid)+[1]) self.obs_centroid = rotated_centroid[:3] #br = tf.TransformBroadcaster() now = rospy.get_rostime() self.br.sendTransform(tuple(self.obs_centroid), (0,0,0,1), now, 'obs_centroid', 'velodyne') self.obs_marker.header.frame_id = 'obs_centroid' self.obs_marker.pose.position = Point(0,0,0) self.obs_marker.pose.orientation = Quaternion(*self.orient) self.add_bbox_lidar() def add_bbox(self): inputName = '/image_raw' rospy.Subscriber(inputName, Image, self.handle_img_msg, queue_size=1) def handle_img_msg(self, img_msg): img = None bridge = cv_bridge.CvBridge() try: img = bridge.imgmsg_to_cv2(img_msg, 'bgr8') except cv_bridge.CvBridgeError as e: rospy.logerr( 'image message to cv conversion failed :' ) rospy.logerr( e ) print( e ) return #tx, ty, tz, yaw, pitch, roll = [0.00749025, -0.40459941, -0.51372948, # -1.66780896, -1.59875352, -3.05415572] #translation = [tx, ty, tz, 1] #rotationMatrix = tf.transformations.euler_matrix(roll, pitch, yaw) #rotationMatrix[:, 3] = translation md = self.metadata dims = np.array([md['l'], md['w'], md['h']]) outputName = '/image_bbox' imgOutput = rospy.Publisher(outputName, Image, queue_size=1) obs_centroid = self.obs_centroid if self.obs_centroid is None: rospy.loginfo("Couldn't find obstacle centroid") imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) return # print centroid info rospy.loginfo(str(obs_centroid)) # case when obstacle is not in camera frame if obs_centroid[0]<2.5 : imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) return # get bbox R = tf.transformations.quaternion_matrix(self.orient) corners = [0.5*np.array([i,j,k])*dims for i in [-1,1] for j in [-1,1] for k in [-1,1]] corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners] projected_pts = [] cameraModel = PinholeCameraModel() cam_info = load_cam_info(self.calib_file) cameraModel.fromCameraInfo(cam_info) projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners] #for pt in corners: # rotated_pt = rotationMatrix.dot(list(pt)+[1]) # projected_pts.append(cameraModel.project3dToPixel(rotated_pt)) projected_pts = np.array(projected_pts) center = np.mean(projected_pts, axis=0) out_img = drawBbox(img, projected_pts) imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8')) def processFeedback(self, feedback ): p = feedback.pose.orientation self.rotation_offset = [p.x, p.y, p.z, p.w] p = feedback.pose.position self.offset = [p.x, p.y, p.z] self.server.applyChanges() def obs_processFeedback(self, feedback ): p = feedback.pose.orientation self.orient = (p.x, p.y, p.z, p.w) now = rospy.get_rostime() self.br.sendTransform(tuple(self.obs_centroid), (0,0,0,1), now, 'obs_centroid', 'velodyne') self.marker.pose.orientation = Quaternion(*self.orient) self.server.applyChanges() def add_bbox_lidar(self): if self.obs_centroid is None : return now = rospy.get_rostime() self.velodyne_marker.header.stamp = now #rospy.get_rostime() self.obs_marker.header.stamp = now #rospy.get_rostime() # tell the server to call processFeedback() when feedback arrives for it self.server.insert(self.velodyne_marker, self.processFeedback) self.server.applyChanges() self.server.insert(self.obs_marker, self.obs_processFeedback) #self.menu_handler.apply(self.server, self.int_marker.name) # 'commit' changes and send to all clients self.server.applyChanges()
class GripperMarkers: _offset = 0.09 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 get_ee_pose(self): from_frame = 'base_link' to_frame = self.side_prefix + '_wrist_roll_link' try: if self._tf_listener.frameExists(from_frame) and self._tf_listener.frameExists(to_frame): t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) # t = rospy.Time.now() (pos, rot) = self._tf_listener.lookupTransform(to_frame, from_frame, t) # Note argument order :( else: rospy.logerr('TF frames do not exist; could not get end effector pose.') except Exception as err: rospy.logerr('Could not get end effector pose through TF.') rospy.logerr(err) 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, feedback): rospy.loginfo('You pressed the `Move arm here` button from the menu.') '''Moves the arm to the desired joints''' print feedback time_to_joint = 2.0 positions = self.ik.get_ik_for_ee(feedback.pose) velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (self.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) pass 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_' + self.side_prefix 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])) 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 print self ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution is 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
class LookAtMarkerServer: 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 # self._head = robot_api.Head(tf_listener=self._tf_listener) def makeBox(self, msg): marker = Marker() marker.type = Marker.CUBE marker.scale.x = msg.scale * 0.45 marker.scale.y = msg.scale * 0.45 marker.scale.z = msg.scale * 0.45 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 1.0 return marker def makeBoxControl(self, msg): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self.makeBox(msg)) msg.controls.append(control) return control def makeQuadrocopterMarker(self, position): int_marker = InteractiveMarker() int_marker.header.frame_id = "odom" int_marker.pose.position = position int_marker.scale = 1 int_marker.name = "look_at_point" int_marker.description = "Point to look at" self.makeBoxControl(int_marker) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.always_visible = True control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE int_marker.controls.append(copy.deepcopy(control)) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) self._server.insert(int_marker, self.handleInput) self._server.applyChanges() def makeStaticMarker(self, position): int_marker = InteractiveMarker() int_marker.header.frame_id = "odom" int_marker.pose.position = position int_marker.scale = 1 int_marker.name = "movement_controller" int_marker.description = "Click to make Kuri move" box_marker = self.makeBox(int_marker) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.always_visible = True control.interaction_mode = InteractiveMarkerControl.BUTTON control.markers.append(box_marker) int_marker.controls.append(copy.deepcopy(control)) self._server.insert(int_marker, self.handleInput) self._server.applyChanges() def handleInput(self, input): if input.event_type == InteractiveMarkerFeedback.MOUSE_UP: if input.marker_name == "look_at_point": ps = PointStamped(header=input.header, point=input.pose.position) self._look_at_target = ps self._head.look_at(self._look_at_target) pass elif input.marker_name == "movement_controller": if self._look_at_target == None: rospy.loginfo("position of target unknown") else: self._head.move_to(self._look_at_target) pass else: pass
control_rotate_y = makeInteractiveMarkerControl( interactive_marker, InteractiveMarkerControl.ROTATE_AXIS) control_rotate_z = makeInteractiveMarkerControl( interactive_marker, InteractiveMarkerControl.ROTATE_AXIS) control_sphere = makeInteractiveMarkerControl( interactive_marker, InteractiveMarkerControl.MOVE_ROTATE_3D) marker = Marker() marker.color.r = 0.2 marker.color.g = 0.3 marker.color.b = 0.7 marker.color.a = 0.5 marker.type = Marker.SPHERE marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 control_sphere.markers.append(marker) setOrientation(1,1,0,0, control_slide_x) setOrientation(1,0,1,0, control_slide_y) setOrientation(1,0,0,1, control_slide_z) setOrientation(1,1,0,0, control_rotate_x) setOrientation(1,0,1,0, control_rotate_y) setOrientation(1,0,0,1, control_rotate_z) setOrientation(1,1,0,0, control_sphere) server.insert(interactive_marker, feedback) server.applyChanges() rospy.spin()
class GripperMarkers: _offset = -0.09 _tf_listener = None 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 get_ee_pose(self): from_frame = '/base_link' to_frame = '/' + self.side_prefix + '_wrist_roll_link' try: GripperMarkers._tf_listener.waitForTransform(from_frame, to_frame, rospy.Time(0), rospy.Duration(5)) t = GripperMarkers._tf_listener.getLatestCommonTime(from_frame, to_frame) (pos, rot) = GripperMarkers._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.marker_pose, GripperMarkers._offset) ik_solution = self.ik.get_ik_for_ee(target_pose) if (ik_solution == None): rospy.logwarn('No IK solutions for this pose, cannot move.') else: self.ik.move_to_joints(ik_solution, 2.0) pass def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.marker_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.marker_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) text = self.side_prefix + '_arm' menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.05), 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 offset = -GripperMarkers._offset transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - 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 = -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 - 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, x_offset): transform = GripperMarkers.get_matrix_from_pose(pose) offset_array = [x_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.marker_pose, GripperMarkers._offset) 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) 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 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 ParkingSpotServer(Node): def __init__(self): super(ParkingSpotServer, self).__init__('parking_spot_server') self.declare_parameter('map_yaml') self.markers = {} self.poses = {} self.marker_server = InteractiveMarkerServer(self, 'markers') map_param = self.get_parameter('map_yaml').value self.map_path = Path(map_param) self.load_map() self.name_counter = 0 # Create a timeout to throttle saving data on feedback # We reset it on marker feedback so that once the user stops editing, save is triggered self.save_timer = self.create_timer(0.1, self.save_map) self.save_timer.cancel() self.add_srv = self.create_service(AddParkingSpot, 'add_parking_spot', self.add_spot) self.del_srv = self.create_service(DeleteParkingSpot, 'delete_parking_spot', self.delete_spot) self.get_srv = self.create_service(GetParkingSpot, 'get_parking_spot', self.get_spot) self.rename_srv = self.create_service(RenameParkingSpot, 'rename_parking_spot', self.rename_spot) def save_map(self): self.save_timer.cancel() dump = {} for name, marker in self.markers.items(): pose = marker.pose euler = euler_from_quaternion([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) yaw = euler[2] dump[name] = [ pose.position.x, pose.position.y, yaw, ] self.map_data['parking'] = dump with self.map_path.open('w') as map_yaml: yaml.dump(self.map_data, map_yaml) def load_map(self): with self.map_path.open('r') as map_yaml: self.map_data = yaml.safe_load(map_yaml) self.poses = { name: Pose2D(x=pose[0], y=pose[1], theta=pose[2]) for name, pose in self.map_data.get('parking', {}).items() } self.markers = { name: self.add_marker(name, Pose2D(x=pose[0], y=pose[1], theta=pose[2])) for name, pose in self.map_data.get('parking', {}).items() } def marker_feedback(self, event: InteractiveMarkerFeedback) -> None: self.save_timer.reset() def add_marker(self, name: str, pose: Pose2D) -> InteractiveMarker: marker = InteractiveMarker(name=name, pose=Pose( position=Point(x=pose.x, y=pose.y, z=0.), orientation=quaternion_from_euler( pose.theta, 0., 0.), )) marker.header.frame_id = 'map' sc = 0.2 z = 0.1 name_marker = Marker( type=Marker.TEXT_VIEW_FACING, scale=Vector3(x=sc, y=sc, z=sc), color=ColorRGBA(r=1., g=1., b=1., a=1.), text=name, ) name_marker.pose.position.x = sc * -0.1 name_marker.pose.position.z = z + sc * -0.1 marker.controls = [ InteractiveMarkerControl( name='name', orientation_mode=InteractiveMarkerControl.VIEW_FACING, interaction_mode=InteractiveMarkerControl.NONE, independent_marker_orientation=False, always_visible=True, markers=[name_marker], ), InteractiveMarkerControl( name='xaxis', orientation_mode=InteractiveMarkerControl.FIXED, orientation=Quaternion(x=0., y=0., z=0.7068252, w=0.7068252), interaction_mode=InteractiveMarkerControl.MOVE_AXIS, ), InteractiveMarkerControl( name='yaxis', orientation_mode=InteractiveMarkerControl.FIXED, interaction_mode=InteractiveMarkerControl.MOVE_AXIS, ), InteractiveMarkerControl( name='turn', interaction_mode=InteractiveMarkerControl.ROTATE_AXIS, orientation=Quaternion(x=0., y=0.7068252, z=0., w=0.7068252), ) ] self.marker_server.insert(marker, feedback_callback=self.marker_feedback) self.marker_server.applyChanges() return marker def add_spot(self, request, response): print(request) name = 'park{:02}'.format(self.name_counter) while name in self.poses: self.name_counter += 1 name = 'park{:02}'.format(self.name_counter) marker = self.add_marker(name, request.pose) self.poses[name] = request.pose self.markers[name] = marker response.success = True self.save_map() return response def delete_spot(self, request, response): try: del self.poses[request.name] self.marker_server.erase(request.name) self.marker_server.applyChanges() del self.markers[request.name] response.success = True self.save_map() except KeyError: response.success = False return response def get_spot(self, request, response): try: response.pose = self.poses[request.name] response.success = True except KeyError: response.success = False return response def rename_spot(self, request, response): if request.name not in self.poses: response.success = False response.msg = 'Spot does not exist' elif request.new_name in self.poses: response.success = False response.msg = 'New name already taken' else: response.success = True name = request.name pose = self.poses[name] del self.markers[name] del self.poses[name] self.poses[request.new_name] = pose self.marker_server.erase(name) marker = self.add_marker(request.new_name, pose) self.markers[request.new_name] = marker self.save_map() return response
class World: """Object recognition and localization related stuff""" tf_listener = None objects = [] 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 _reset_objects(self): """Function that removes all objects""" self._lock.acquire() for i in range(len(World.objects)): self._im_server.erase(World.objects[i].int_marker.name) self._im_server.applyChanges() if self.surface != None: self._remove_surface() self._im_server.clear() self._im_server.applyChanges() World.objects = [] self._lock.release() def receieve_table_marker(self, marker): """Callback function for markers to determine table""" if marker.type == Marker.LINE_STRIP: if len(marker.points) == 6: rospy.loginfo("Received a TABLE marker.") xmin = marker.points[0].x ymin = marker.points[0].y xmax = marker.points[2].x ymax = marker.points[2].y depth = xmax - xmin width = ymax - ymin pose = Pose(marker.pose.position, marker.pose.orientation) pose.position.x = pose.position.x + xmin + depth / 2 pose.position.y = pose.position.y + ymin + width / 2 dimensions = Vector3(depth, width, 0.01) self.surface = World._get_surface_marker(pose, dimensions) self._im_server.insert(self.surface, self.marker_feedback_cb) self._im_server.applyChanges() def receieve_object_info(self, object_list): """Callback function to receive object info""" self._lock.acquire() rospy.loginfo("Received recognized object list.") if len(object_list.graspable_objects) > 0: for i in range(len(object_list.graspable_objects)): models = object_list.graspable_objects[i].potential_models if len(models) > 0: object_pose = None best_confidence = 0.0 for j in range(len(models)): if best_confidence < models[j].confidence: object_pose = models[j].pose.pose best_confidence = models[j].confidence if object_pose != None: rospy.logwarn("Adding the recognized object " + "with most confident model.") self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), True, object_list.meshes[i]) else: rospy.logwarn("... this is not a recognition result, " + "it is probably just segmentation.") cluster = object_list.graspable_objects[i].cluster bbox = self._bb_service(cluster) cluster_pose = bbox.pose.pose if cluster_pose != None: rospy.loginfo( "Adding unrecognized object with pose:" + World.pose_to_string(cluster_pose) + "\n" + "In ref frame" + str(bbox.pose.header.frame_id) ) self._add_new_object(cluster_pose, bbox.box_dims, False) else: rospy.logwarn("... but the list was empty.") self._lock.release() @staticmethod def get_pose_from_transform(transform): """Returns pose for transformation matrix""" 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 get_matrix_from_pose(pose): """Returns the transformation matrix for given 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_absolute_pose(arm_state): """Returns absolute pose of an end effector state""" if arm_state.refFrame == ArmState.OBJECT: arm_state_copy = ArmState( arm_state.refFrame, Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation), arm_state.joint_pose[:], arm_state.refFrameObject, ) World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE) return arm_state_copy.ee_pose else: return arm_state.ee_pose @staticmethod def get_most_similar_obj(ref_object, ref_frame_list): """Finds the most similar object in the world""" best_dist = 10000 chosen_obj_index = -1 for i in range(len(ref_frame_list)): dist = World.object_dissimilarity(ref_frame_list[i], ref_object) if dist < best_dist: best_dist = dist chosen_obj_index = i if chosen_obj_index == -1: rospy.logwarn("Did not find a similar object..") return None else: print "Object dissimilarity is --- ", best_dist if best_dist > 0.075: rospy.logwarn("Found some objects, but not similar enough.") return None else: rospy.loginfo("Most similar to new object " + str(chosen_obj_index)) return ref_frame_list[chosen_obj_index] @staticmethod def _get_mesh_marker(marker, mesh): """Function that generated a marker from a mesh""" marker.type = Marker.TRIANGLE_LIST index = 0 marker.scale = Vector3(1.0, 1.0, 1.0) while index + 2 < len(mesh.triangles): if ( (mesh.triangles[index] < len(mesh.vertices)) and (mesh.triangles[index + 1] < len(mesh.vertices)) and (mesh.triangles[index + 2] < len(mesh.vertices)) ): marker.points.append(mesh.vertices[mesh.triangles[index]]) marker.points.append(mesh.vertices[mesh.triangles[index + 1]]) marker.points.append(mesh.vertices[mesh.triangles[index + 2]]) index += 3 else: rospy.logerr("Mesh contains invalid triangle!") break return marker def _add_new_object(self, pose, dimensions, is_recognized, mesh=None): """Function to add new objects""" dist_threshold = 0.02 to_remove = None if is_recognized: # Temporary HACK for testing. # Will remove all recognition completely if this works. return False # Check if there is already an object for i in range(len(World.objects)): distance = World.pose_distance(World.objects[i].object.pose, pose) if distance < dist_threshold: if World.objects[i].is_recognized: rospy.loginfo( "Previously recognized object at " + "the same location, will not add this object." ) return False else: rospy.loginfo( "Previously unrecognized object at " + "the same location, will replace it with the " + "recognized object." ) to_remove = i break if to_remove != None: self._remove_object(to_remove) n_objects = len(World.objects) World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized)) int_marker = self._get_object_marker(len(World.objects) - 1, mesh) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True else: for i in range(len(World.objects)): if World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold: rospy.loginfo("Previously detected object at the same" + "location, will not add this object.") return False n_objects = len(World.objects) World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized)) int_marker = self._get_object_marker(len(World.objects) - 1) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True def _remove_object(self, to_remove): """Function to remove object by index""" obj = World.objects.pop(to_remove) rospy.loginfo("Removing object " + obj.int_marker.name) self._im_server.erase(obj.int_marker.name) self._im_server.applyChanges() # if (obj.is_recognized): # for i in range(len(World.objects)): # if ((World.objects[i].is_recognized) # and World.objects[i].index>obj.index): # World.objects[i].decrease_index() # self.n_recognized -= 1 # else: # for i in range(len(World.objects)): # if ((not World.objects[i].is_recognized) and # World.objects[i].index>obj.index): # World.objects[i].decrease_index() # self.n_unrecognized -= 1 def _remove_surface(self): """Function to request removing surface""" rospy.loginfo("Removing surface") self._im_server.erase("surface") self._im_server.applyChanges() self.surface = None def _get_object_marker(self, index, mesh=None): """Generate a marker for world objects""" int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = "base_link" int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id="base_link"), color=ColorRGBA(0.2, 0.8, 0.0, 0.6), pose=World.objects[index].object.pose, ) if mesh != None: object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = World.objects[index].object.pose.position.x text_pos.y = World.objects[index].object.pose.position.y text_pos.z = World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06 button_control.markers.append( Marker( type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id="base_link"), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)), ) ) int_marker.controls.append(button_control) return int_marker @staticmethod def _get_surface_marker(pose, dimensions): """ Function that generates a surface marker""" int_marker = InteractiveMarker() int_marker.name = "surface" int_marker.header.frame_id = "base_link" int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker( type=Marker.CUBE, id=2000, lifetime=rospy.Duration(2), scale=dimensions, header=Header(frame_id="base_link"), color=ColorRGBA(0.8, 0.0, 0.4, 0.4), pose=pose, ) button_control.markers.append(object_marker) text_pos = Point() position = pose.position dimensions = dimensions text_pos.x = position.x + dimensions.x / 2 - 0.06 text_pos.y = position.y - dimensions.y / 2 + 0.06 text_pos.z = position.z + dimensions.z / 2 + 0.06 text_marker = Marker( type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0, 0, 0.03), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id="base_link"), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)), ) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker @staticmethod def get_frame_list(): """Function that returns the list of ref. frames""" objects = [] for i in range(len(World.objects)): objects.append(World.objects[i].object) return objects @staticmethod def has_objects(): """Function that checks if there are any objects""" return len(World.objects) > 0 @staticmethod def object_dissimilarity(obj1, obj2): """Distance between two objects""" dims1 = obj1.dimensions dims2 = obj2.dimensions return norm(array([dims1.x, dims1.y, dims1.z]) - array([dims2.x, dims2.y, dims2.z])) @staticmethod def get_ref_from_name(ref_name): """Ref. frame type from ref. frame name""" if ref_name == "base_link": return ArmState.ROBOT_BASE else: return ArmState.OBJECT @staticmethod def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()): """Transforms an arm frame to a new ref. frame""" if ref_frame == ArmState.ROBOT_BASE: if arm_frame.refFrame == ArmState.ROBOT_BASE: pass # rospy.logwarn('No reference frame transformations ' + #'needed (both absolute).') elif arm_frame.refFrame == ArmState.OBJECT: abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, "base_link") arm_frame.ee_pose = abs_ee_pose arm_frame.refFrame = ArmState.ROBOT_BASE arm_frame.refFrameObject = Object() else: rospy.logerr( "Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame) ) elif ref_frame == ArmState.OBJECT: if arm_frame.refFrame == ArmState.ROBOT_BASE: rel_ee_pose = World.transform(arm_frame.ee_pose, "base_link", ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj elif arm_frame.refFrame == ArmState.OBJECT: if arm_frame.refFrameObject.name == ref_frame_obj.name: pass # rospy.logwarn('No reference frame transformations ' + #'needed (same object).') else: rel_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj else: rospy.logerr( "Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame) ) return arm_frame @staticmethod def has_object(object_name): """Checks if the world contains the object""" for obj in World.objects: if obj.object.name == object_name: return True return False @staticmethod def is_frame_valid(object_name): """Checks if the frame is valid for transforms""" return (object_name == "base_link") or World.has_object(object_name) @staticmethod def transform(pose, from_frame, to_frame): """ Function to transform a pose between two ref. frames if there is a TF exception or object does not exist it will return the pose back without any transforms""" if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame): pose_stamped = PoseStamped() try: common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame) pose_stamped.header.stamp = common_time pose_stamped.header.frame_id = from_frame pose_stamped.pose = pose rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped) return rel_ee_pose.pose except tf.Exception: rospy.logerr("TF exception during transform.") return pose except rospy.ServiceException: rospy.logerr("Exception during transform.") return pose else: rospy.logwarn("One of the frame objects might not exist: " + from_frame + " or " + to_frame) return pose @staticmethod def pose_to_string(pose): """For printing a pose to stdout""" return ( "Position: " + str(pose.position.x) + ", " + str(pose.position.y) + ", " + str(pose.position.z) + "\n" + "Orientation: " + str(pose.orientation.x) + ", " + str(pose.orientation.y) + ", " + str(pose.orientation.z) + ", " + str(pose.orientation.w) + "\n" ) def _publish_tf_pose(self, pose, name, parent): """ Publishes a TF for object pose""" if pose != None: pos = (pose.position.x, pose.position.y, pose.position.z) rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent) def update_object_pose(self): """ Function to externally update an object pose""" Response.perform_gaze_action(GazeGoal.LOOK_DOWN) while ( Response.gaze_client.get_state() == GoalStatus.PENDING or Response.gaze_client.get_state() == GoalStatus.ACTIVE ): time.sleep(0.1) if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("Could not look down to take table snapshot") return False rospy.loginfo("Looking at table now.") goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while ( self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING ): time.sleep(0.1) rospy.loginfo("Object recognition has been reset.") rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text()) self._reset_objects() if self._object_action_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("Could not reset recognition.") return False # Do segmentation goal = UserCommandGoal(UserCommandGoal.SEGMENT, False) self._object_action_client.send_goal(goal) while ( self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING ): time.sleep(0.1) rospy.loginfo("Table segmentation is complete.") rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text()) if self._object_action_client.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("Could not segment.") return False # Do recognition goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False) self._object_action_client.send_goal(goal) while ( self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING ): time.sleep(0.1) rospy.loginfo("Objects on the table have been recognized.") rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text()) # Record the result if self._object_action_client.get_state() == GoalStatus.SUCCEEDED: wait_time = 0 total_wait_time = 5 while not World.has_objects() and wait_time < total_wait_time: time.sleep(0.1) wait_time += 0.1 if not World.has_objects(): rospy.logerr("Timeout waiting for a recognition result.") return False else: rospy.loginfo("Got the object list.") return True else: rospy.logerr("Could not recognize.") return False def clear_all_objects(self): """Removes all objects from the world""" goal = UserCommandGoal(UserCommandGoal.RESET, False) self._object_action_client.send_goal(goal) while ( self._object_action_client.get_state() == GoalStatus.ACTIVE or self._object_action_client.get_state() == GoalStatus.PENDING ): time.sleep(0.1) rospy.loginfo("Object recognition has been reset.") rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text()) if self._object_action_client.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo("Successfully reset object localization pipeline.") self._reset_objects() self._remove_surface() def get_nearest_object(self, arm_pose): """Gives a pointed to the nearest object""" dist_threshold = 0.4 def chObj(cur, obj): dist = World.pose_distance(obj.object.pose, arm_pose) return (dist, obj.object) if (dist < cur[0]) else cur return reduce(chObj, World.objects, (dist_threshold, None))[1] @staticmethod def pose_distance(pose1, pose2, is_on_table=True): """Distance between two world poses""" if pose1 == [] or pose2 == []: return 0.0 else: if is_on_table: arr1 = array([pose1.position.x, pose1.position.y]) arr2 = array([pose2.position.x, pose2.position.y]) else: arr1 = array([pose1.position.x, pose1.position.y, pose1.position.z]) arr2 = array([pose2.position.x, pose2.position.y, pose2.position.z]) dist = norm(arr1 - arr2) if dist < 0.0001: dist = 0 return dist def marker_feedback_cb(self, feedback): """Callback for when feedback from a marker is received""" if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo("Clicked on object " + str(feedback.marker_name)) rospy.loginfo("Number of objects " + str(len(World.objects))) else: rospy.loginfo("Unknown event" + str(feedback.event_type)) def update(self): """Update function called in a loop""" # Visualize the detected object is_world_changed = False self._lock.acquire() if World.has_objects(): to_remove = None for i in range(len(World.objects)): self._publish_tf_pose(World.objects[i].object.pose, World.objects[i].get_name(), "base_link") if World.objects[i].is_removed: to_remove = i if to_remove != None: self._remove_object(to_remove) is_world_changed = True self._lock.release() return is_world_changed
class ActuatorServer(object): def __init__(self): self.goto_pub = rospy.Publisher('/move_base_simple/goal', geometry_msgs.msg.PoseStamped) self.pose_pub = rospy.Publisher('/pose_names', order_system.msg.PoseNames, latch=True) rospy.Subscriber('/amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped, self._handle_pose_callback) self.names = {} try: with open( "/home/team3/catkin_ws/src/cse481c/order_service/LOL.pickle", "r") as f: self.names = pickle.load(f) except EOFError: self.names = {} self.pose = None self.server = InteractiveMarkerServer('simple_marker') self.name_markers = {} self.name_controls = {} def _create_marker(self, name): self.name_markers[name] = InteractiveMarker() self.name_markers[name].header.frame_id = "map" self.name_markers[name].name = name self.name_markers[name].description = name self.name_markers[ name].pose.position.x = self.pose.pose.pose.position.x self.name_markers[ name].pose.position.y = self.pose.pose.pose.position.y # TODO(emersonn): REMEMBER THAT THIS IS 1 IF IT IS CRAZY LOL self.name_markers[name].pose.position.z = 1 box_marker = create_box_marker() self.name_controls[name] = InteractiveMarkerControl() self.name_controls[ name].interaction_mode = InteractiveMarkerControl.MOVE_PLANE self.name_controls[name].always_visible = True self.name_controls[name].orientation.w = 1 self.name_controls[name].orientation.x = 0 self.name_controls[name].orientation.y = 1 self.name_controls[name].orientation.z = 0 new_controller_lol = InteractiveMarkerControl() new_controller_lol.orientation.w = 1 new_controller_lol.orientation.x = 0 new_controller_lol.orientation.y = 1 new_controller_lol.orientation.z = 0 new_controller_lol.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS new_controller_lol.orientation_mode = InteractiveMarkerControl.FIXED self.name_markers[name].controls.append(new_controller_lol) # self.name_controls[name].orientation.w = self.pose.pose.pose.orientation.w # self.name_controls[name].orientation.x = self.pose.pose.pose.orientation.x # self.name_controls[name].orientation.y = self.pose.pose.pose.orientation.y # self.name_controls[name].orientation.z = self.pose.pose.pose.orientation.z self.name_controls[name].markers.append(box_marker) self.name_markers[name].controls.append(self.name_controls[name]) self.server.insert(self.name_markers[name], self._callback) self.server.applyChanges() def _callback(self, dog): name = dog.marker_name pose = dog.pose rospy.logerr(str(self.names[name])) self.names[name].pose.pose.position.x = pose.position.x self.names[name].pose.pose.position.y = pose.position.y self.names[name].pose.pose.orientation = pose.orientation rospy.logerr(str(self.names[name])) def _delete_marker(self, name): self.server.erase(name) self.server.applyChanges() def handle_send_fetch(self, request): rospy.logerr("yo we got fam" + str(request)) if request.command == request.CREATE: self._handle_create(request.name) self._create_marker(request.name) elif request.command == request.DELETE: self._handle_delete(request.name) self._delete_marker(request.name) elif request.command == request.GOTO: self._handle_goto(request.name) elif request.command == request.LIST: rospy.loginfo('hey Karen') else: rospy.logerr('none of these work') pose_message = order_system.msg.PoseNames() pose_message.poses = list(self.names.keys()) self.pose_pub.publish(pose_message) fetch_response = SendFetchResponse() fetch_response.names = list(self.names.keys()) return fetch_response def _handle_create(self, name): rospy.loginfo(str(self.pose)) self.names[name] = self.pose def _handle_delete(self, name): if name not in self.names: rospy.loginfo('wtf bro') else: del self.names[name] def _handle_goto(self, name): new_goto = geometry_msgs.msg.PoseStamped() rospy.loginfo(str(new_goto)) new_goto.header = self.names[name].header new_goto.pose = self.names[name].pose.pose rospy.logerr("wtf" + str(self.names[name].pose.pose)) rospy.logerr("bro" + str(new_goto)) self.goto_pub.publish(new_goto) def _handle_pose_callback(self, data): self.pose = copy.deepcopy(data) def pickle_it_up_bro(self): with open("/home/team3/catkin_ws/src/cse481c/map_annotator/LOL.pickle", "w") as f: pickle.dump(self.names, f)
class World: '''Object recognition and localization related stuff''' tf_listener = None objects = [] world = None segmentation_service = rospy.get_param("/pr2_pbd_interaction/tabletop_segmentation_service") 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 @staticmethod def get_world(): if World.world is None: World.world = World() return World.world def _reset_objects(self): '''Function that removes all objects''' self._lock.acquire() for i in range(len(World.objects)): self._im_server.erase(World.objects[i].int_marker.name) self._im_server.applyChanges() if self.surface != None: self._remove_surface() #self._im_server.clear() self._im_server.applyChanges() World.objects = [] self._lock.release() def receieve_table_marker(self, marker): '''Callback function for markers to determine table''' if (marker.type == Marker.LINE_STRIP): if (len(marker.points) == 6): rospy.loginfo('Received a TABLE marker.') xmin = marker.points[0].x ymin = marker.points[0].y xmax = marker.points[2].x ymax = marker.points[2].y depth = xmax - xmin width = ymax - ymin pose = Pose(marker.pose.position, marker.pose.orientation) pose.position.x = pose.position.x + xmin + depth / 2 pose.position.y = pose.position.y + ymin + width / 2 dimensions = Vector3(depth, width, 0.01) self.surface = World._get_surface_marker(pose, dimensions) self._im_server.insert(self.surface, self.marker_feedback_cb) self._im_server.applyChanges() def receive_ar_markers(self, data): '''Callback function to receive marker info''' self._lock.acquire() if self.is_looking_for_markers: if len(data.markers) > 0: rospy.loginfo('Received non-empty marker list.') for i in range(len(data.markers)): marker = data.markers[i] self._add_new_object(marker.pose.pose, self.marker_dims, False, id=marker.id) self._lock.release() def receieve_object_info(self, object_list): '''Callback function to receive object info''' self._lock.acquire() rospy.loginfo('Received recognized object list.') if (len(object_list.graspable_objects) > 0): for i in range(len(object_list.graspable_objects)): models = object_list.graspable_objects[i].potential_models if (len(models) > 0): object_pose = None best_confidence = 0.0 for j in range(len(models)): if (best_confidence < models[j].confidence): object_pose = models[j].pose.pose best_confidence = models[j].confidence if (object_pose != None): rospy.logwarn('Adding the recognized object ' + 'with most confident model.') self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), True, object_list.meshes[i]) else: rospy.logwarn('... this is not a recognition result, ' + 'it is probably just segmentation.') cluster = object_list.graspable_objects[i].cluster bbox = self._bb_service(cluster) cluster_pose = bbox.pose.pose if (cluster_pose != None): rospy.loginfo('Adding unrecognized object with pose:' + World.pose_to_string(cluster_pose) + '\n' + 'In ref frame' + str(bbox.pose.header.frame_id)) self._add_new_object(cluster_pose, bbox.box_dims, False) else: rospy.logwarn('... but the list was empty.') self._lock.release() @staticmethod def get_pose_from_transform(transform): '''Returns pose for transformation matrix''' 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 get_matrix_from_pose(pose): '''Returns the transformation matrix for given 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_absolute_pose(arm_state): '''Returns absolute pose of an end effector state''' if (arm_state.refFrame == ArmState.OBJECT): arm_state_copy = ArmState(arm_state.refFrame, Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation), arm_state.joint_pose[:], arm_state.refFrameObject) World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE) return arm_state_copy.ee_pose else: return arm_state.ee_pose @staticmethod def get_most_similar_obj(ref_object, ref_frame_list, threshold=0.075): '''Finds the most similar object in the world''' best_dist = 10000 chosen_obj_index = -1 for i in range(len(ref_frame_list)): dist = World.object_dissimilarity(ref_frame_list[i], ref_object) if (dist < best_dist): best_dist = dist chosen_obj_index = i if chosen_obj_index == -1: rospy.logwarn('Did not find a similar object..') return None else: print 'Object dissimilarity is --- ', best_dist if best_dist > threshold: rospy.logwarn('Found some objects, but not similar enough.') return None else: rospy.loginfo('Most similar to object ' + ref_frame_list[chosen_obj_index].name) return ref_frame_list[chosen_obj_index] @staticmethod def get_map_of_most_similar_obj(object_list, ref_frame_list, threshold=0.075): if None in object_list: object_list.remove(None) if len(object_list) == 0 or len(ref_frame_list) == 0: return None markers_dict = {} marker_names = [] for obj in object_list: object_name = obj.name if object_name.startswith("marker"): marker_names.append(object_name) found_match = False for ref_frame in ref_frame_list: if ref_frame.name == object_name: markers_dict[object_name] = ref_frame found_match = True break if not found_match: return None ref_frame_list = [x for x in ref_frame_list if x.name not in marker_names] object_list = [x for x in object_list if x.name not in marker_names] if len(object_list) == 0 or len(ref_frame_list) == 0: return markers_dict if len(markers_dict) > 0 else None orderings = [] World.permute(object_list, orderings) costs = [] assignments = [] for ordering in orderings: cur_cost = 0 cur_ref_frame_list = ref_frame_list[:] cur_assignment = [] for object in ordering: most_similar_object = World.get_most_similar_obj(object, cur_ref_frame_list, threshold) if most_similar_object is None: return None cur_ref_frame_list.remove(most_similar_object) cur_assignment.append(most_similar_object) cur_cost += World.object_dissimilarity(object, most_similar_object) costs.append(cur_cost) assignments.append(cur_assignment) min_cost, min_idx = min((val, idx) for (idx, val) in enumerate(costs)) names = [x.name for x in orderings[min_idx]] object_dict = dict(zip(names, assignments[min_idx])) object_dict.update(markers_dict) return object_dict @staticmethod def permute(a, results): if len(a) == 1: results.insert(len(results), a) else: for i in range(0, len(a)): element = a[i] a_copy = [a[j] for j in range(0, len(a)) if j != i] subresults = [] World.permute(a_copy, subresults) for subresult in subresults: result = [element] + subresult results.insert(len(results), result) @staticmethod def _get_mesh_marker(marker, mesh): '''Function that generated a marker from a mesh''' marker.type = Marker.TRIANGLE_LIST index = 0 marker.scale = Vector3(1.0, 1.0, 1.0) while (index + 2 < len(mesh.triangles)): if ((mesh.triangles[index] < len(mesh.vertices)) and (mesh.triangles[index + 1] < len(mesh.vertices)) and (mesh.triangles[index + 2] < len(mesh.vertices))): marker.points.append(mesh.vertices[mesh.triangles[index]]) marker.points.append(mesh.vertices[mesh.triangles[index + 1]]) marker.points.append(mesh.vertices[mesh.triangles[index + 2]]) index += 3 else: rospy.logerr('Mesh contains invalid triangle!') break return marker def _add_new_object(self, pose, dimensions, is_recognized, mesh=None, id=None): '''Function to add new objects''' dist_threshold = 0.02 to_remove = None if (is_recognized): # Temporary HACK for testing. # Will remove all recognition completely if this works. return False # Check if there is already an object for i in range(len(World.objects)): distance = World.pose_distance(World.objects[i].object.pose, pose) if (distance < dist_threshold): if (World.objects[i].is_recognized): rospy.loginfo('Previously recognized object at ' + 'the same location, will not add this object.') return False else: rospy.loginfo('Previously unrecognized object at ' + 'the same location, will replace it with the ' + 'recognized object.') to_remove = i break if (to_remove != None): self._remove_object(to_remove) n_objects = len(World.objects) new_object = WorldObject(pose, n_objects, dimensions, is_recognized) if id is not None: new_object.assign_name("marker" + str(id)) new_object.is_marker = True World.objects.append(new_object) int_marker = self._get_object_marker(len(World.objects) - 1, mesh) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True else: for i in range(len(World.objects)): if (World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold): rospy.loginfo('Previously detected object at the same' + 'location, will not add this object.') return False if id is not None and World.objects[i].get_name() == "marker" + str(id): rospy.loginfo('Previously added marker with the same id, will not add this object.') return False n_objects = len(World.objects) new_object = WorldObject(pose, n_objects, dimensions, is_recognized) if id is not None: new_object.assign_name("marker" + str(id)) new_object.is_marker = True World.objects.append(new_object) int_marker = self._get_object_marker(len(World.objects) - 1) World.objects[-1].int_marker = int_marker self._im_server.insert(int_marker, self.marker_feedback_cb) self._im_server.applyChanges() World.objects[-1].menu_handler.apply(self._im_server, int_marker.name) self._im_server.applyChanges() return True def _add_new_marker(self, id, pose): '''Function to add new markers''' #dist_threshold = 0.01 #to_remove = None #for i in range(len(World.markers)): # if (World.pose_distance(World.markers[i].pose, pose) # < dist_threshold): # rospy.loginfo('Previously detected marker at the same' + # 'location, will not add this marker.') # return False #n_markers = len(World.markers) #World.markers.append(WorldMarker(id, pose)) #int_marker = self._get_object_marker(len(World.objects) - 1) #World.markers[-1].int_marker = int_marker #self._im_server.insert(int_marker, self.marker_feedback_cb) #self._im_server.applyChanges() #World.markers[-1].menu_handler.apply(self._im_server, # int_marker.name) #self._im_server.applyChanges() return True def _remove_object(self, to_remove): '''Function to remove object by index''' obj = World.objects.pop(to_remove) rospy.loginfo('Removing object ' + obj.int_marker.name) self._im_server.erase(obj.int_marker.name) self._im_server.applyChanges() # if (obj.is_recognized): # for i in range(len(World.objects)): # if ((World.objects[i].is_recognized) # and World.objects[i].index>obj.index): # World.objects[i].decrease_index() # self.n_recognized -= 1 # else: # for i in range(len(World.objects)): # if ((not World.objects[i].is_recognized) and # World.objects[i].index>obj.index): # World.objects[i].decrease_index() # self.n_unrecognized -= 1 def _remove_surface(self): '''Function to request removing surface''' rospy.loginfo('Removing surface') self._im_server.erase('surface') self._im_server.applyChanges() self.surface = None def _get_object_reachability_marker(self, world_object): radius = self.relative_frame_threshold pointsList = [] nx = 8 ny = 8 pointsList.append(Point(0, 0, radius)) pointsList.append(Point(0, 0, -radius)) for x in range(nx): theta = 2 * pi * (x * 1.0 / nx) for y in range(1, ny): phi = pi * (y * 1.0 / ny) destx = radius * cos(theta) * sin(phi) desty = radius * sin(theta) * sin(phi) destz = radius * cos(phi) pointsList.append(Point(destx, desty, destz)) id = abs(hash(world_object.get_name() + "_reachability")) % (10 ** 8) marker = Marker(type=Marker.SPHERE_LIST, id=id, lifetime=rospy.Duration(nsecs=10 ** 8), scale=Vector3(0.01, 0.01, 0.01), points=set(pointsList), header=Header(frame_id='base_link'), color=ColorRGBA(1, 1, 1, 0.5), pose=world_object.object.pose) return marker def _get_object_marker(self, index, mesh=None): '''Generate a marker for world objects''' int_marker = InteractiveMarker() int_marker.name = World.objects[index].get_name() int_marker.header.frame_id = 'base_link' int_marker.pose = World.objects[index].object.pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True color = WorldObject.default_color if not World.objects[index].is_fake else WorldObject.fake_color object_marker = Marker(type=Marker.CUBE, id=index, lifetime=rospy.Duration(2), scale=World.objects[index].object.dimensions, header=Header(frame_id='base_link'), color=color, pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))) if (mesh != None): object_marker = World._get_mesh_marker(object_marker, mesh) button_control.markers.append(object_marker) text_pos = Point() text_pos.x = 0 text_pos.y = 0 text_pos.z = World.objects[index].object.dimensions.z / 2 + 0.06 button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=index, scale=Vector3(0.05, 0.05, 0.05), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker.controls.append(button_control) return int_marker @staticmethod def _get_surface_marker(pose, dimensions): ''' Function that generates a surface marker''' int_marker = InteractiveMarker() int_marker.name = 'surface' int_marker.header.frame_id = 'base_link' int_marker.pose = pose int_marker.scale = 1 button_control = InteractiveMarkerControl() button_control.interaction_mode = InteractiveMarkerControl.BUTTON button_control.always_visible = True object_marker = Marker(type=Marker.CUBE, id=2000, lifetime=rospy.Duration(2), scale=dimensions, header=Header(frame_id='base_link'), color=ColorRGBA(0.8, 0.0, 0.4, 0.4), pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))) button_control.markers.append(object_marker) text_pos = Point() dimensions = dimensions text_pos.x = dimensions.x / 2 - 0.06 text_pos.y = - dimensions.y / 2 + 0.06 text_pos.z = dimensions.z / 2 + 0.06 text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001, scale=Vector3(0.05, 0.05, 0.05), text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id='base_link'), pose=Pose(text_pos, Quaternion(0, 0, 0, 1))) button_control.markers.append(text_marker) int_marker.controls.append(button_control) return int_marker def get_frame_list(self): '''Function that returns the list of ref. frames''' self._lock.acquire() objects = [] for i in range(len(World.objects)): objects.append(World.objects[i].object) self._lock.release() return objects @staticmethod def has_objects(): '''Function that checks if there are any objects''' return len(World.objects) > 0 @staticmethod def has_marker_objects(): '''Function that checks if there are any markers''' for o in World.objects: if o.is_marker: return True return False @staticmethod def has_non_marker_objects(): '''Function that checks if there are any objects''' for o in World.objects: if not o.is_marker: return True return False @staticmethod def object_dissimilarity(obj1, obj2): '''Distance between two objects''' dims1 = obj1.dimensions dims2 = obj2.dimensions return norm(array([dims1.x, dims1.y, dims1.z]) - array([dims2.x, dims2.y, dims2.z])) @staticmethod def get_ref_from_name(ref_name): '''Ref. frame type from ref. frame name''' if ref_name == 'base_link': return ArmState.ROBOT_BASE else: return ArmState.OBJECT @staticmethod def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()): '''Transforms an arm frame to a new ref. frame''' if ref_frame == ArmState.ROBOT_BASE: if (arm_frame.refFrame == ArmState.ROBOT_BASE): rospy.logwarn('No reference frame transformations ' + 'needed (both absolute).') elif (arm_frame.refFrame == ArmState.OBJECT): abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, 'base_link') arm_frame.ee_pose = abs_ee_pose arm_frame.refFrame = ArmState.ROBOT_BASE arm_frame.refFrameObject = Object() else: rospy.logerr('Unhandled reference frame conversion:' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) elif ref_frame == ArmState.OBJECT: if (arm_frame.refFrame == ArmState.ROBOT_BASE): rel_ee_pose = World.transform(arm_frame.ee_pose, 'base_link', ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj elif (arm_frame.refFrame == ArmState.OBJECT): if (arm_frame.refFrameObject.name == ref_frame_obj.name): rospy.logwarn('No reference frame transformations ' + 'needed (same object).') else: rel_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name) arm_frame.ee_pose = rel_ee_pose arm_frame.refFrame = ArmState.OBJECT arm_frame.refFrameObject = ref_frame_obj else: rospy.logerr('Unhandled reference frame conversion:' + str(arm_frame.refFrame) + ' to ' + str(ref_frame)) return arm_frame @staticmethod def has_object(object_name): '''Checks if the world contains the object''' for obj in World.objects: if obj.object.name == object_name: return True return False @staticmethod def is_frame_valid(object_name): '''Checks if the frame is valid for transforms''' return (object_name == 'base_link') or World.has_object(object_name) @staticmethod def transform(pose, from_frame, to_frame): ''' Function to transform a pose between two ref. frames if there is a TF exception or object does not exist it will return the pose back without any transforms''' if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame): pose_stamped = PoseStamped() try: common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame) pose_stamped.header.stamp = common_time pose_stamped.header.frame_id = from_frame pose_stamped.pose = pose rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped) return rel_ee_pose.pose except tf.Exception: rospy.logerr('TF exception during transform.') return pose except rospy.ServiceException: rospy.logerr('Exception during transform.') return pose else: # rospy.logwarn('One of the frame objects might not exist: ' + # from_frame + ' or ' + to_frame) return pose @staticmethod def pose_to_string(pose): '''For printing a pose to stdout''' return ('Position: ' + str(pose.position.x) + ", " + str(pose.position.y) + ', ' + str(pose.position.z) + '\n' + 'Orientation: ' + str(pose.orientation.x) + ", " + str(pose.orientation.y) + ', ' + str(pose.orientation.z) + ', ' + str(pose.orientation.w) + '\n') def _publish_tf_pose(self, pose, name, parent): ''' Publishes a TF for object pose''' if (pose != None): pos = (pose.position.x, pose.position.y, pose.position.z) rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent) def update_object_pose(self): ''' Function to externally update an object pose''' rospy.loginfo("waiting for segmentation service") rospy.wait_for_service(self.segmentation_service) try: get_segmentation = rospy.ServiceProxy(self.segmentation_service, TabletopSegmentation) resp = get_segmentation() rospy.loginfo("Adding landmarks") self._reset_objects() # add the table xmin = resp.table.x_min ymin = resp.table.y_min xmax = resp.table.x_max ymax = resp.table.y_max depth = xmax - xmin width = ymax - ymin pose = resp.table.pose.pose pose.position.x = pose.position.x + xmin + depth / 2 pose.position.y = pose.position.y + ymin + width / 2 dimensions = Vector3(depth, width, 0.01) self.surface = World._get_surface_marker(pose, dimensions) self._im_server.insert(self.surface, self.marker_feedback_cb) self._im_server.applyChanges() for cluster in resp.clusters: points = cluster.points if (len(points) == 0): return Point(0, 0, 0) [minX, maxX, minY, maxY, minZ, maxZ] = [ points[0].x, points[0].x, points[0].y, points[0].y, points[0].z, points[0].z] for pt in points: minX = min(minX, pt.x) minY = min(minY, pt.y) minZ = min(minZ, pt.z) maxX = max(maxX, pt.x) maxY = max(maxY, pt.y) maxZ = max(maxZ, pt.z) self._add_new_object(Pose(Point((minX + maxX) / 2, (minY + maxY) / 2, (minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)), Point(maxX - minX, maxY - minY, maxZ - minZ), False) return True except rospy.ServiceException, e: print "Call to segmentation service failed: %s" % e return False
class ToolCalib(object): def __init__(self): rospy.loginfo("Constructor got called") self.frame_id = rospy.get_param("~frame_id") self.mesh_resource = rospy.get_param("~mesh_resource") self.scale = rospy.get_param("~marker_scale") self.server = InteractiveMarkerServer("tool_calib") self.server.insert(self.create_6dof_marker(), self.marker_callback) self.server.applyChanges() self.tool_marker = Marker() self.tool_marker.header.frame_id = self.frame_id self.tool_marker.header.stamp = rospy.get_rostime() self.tool_marker.pose.orientation.w = 1 self.tool_marker.ns = "tool_marker" self.tool_marker.id = 1 self.tool_marker.action = Marker.ADD self.tool_marker.type = Marker.MESH_RESOURCE self.tool_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0) self.tool_marker.scale = Vector3(self.scale, self.scale, self.scale) self.tool_marker.frame_locked = True self.tool_marker.mesh_resource = self.mesh_resource self.tool_marker.mesh_use_embedded_materials = True self.marker_pub = rospy.Publisher("/visualization_marker", Marker, queue_size=1) def create_6dof_marker(self): imarker = InteractiveMarker() imarker.header.frame_id = self.frame_id imarker.pose.orientation.w = 1 imarker.name = "tool_calib" imarker.name = "Tool Calibration" imarker.scale = 0.2 control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS imarker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS imarker.controls.append(control) return imarker def marker_callback(self, data): if data.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.loginfo("Current Marker Pose for %s:", self.mesh_resource) self.tool_marker.header = data.header self.tool_marker.pose = data.pose self.marker_pub.publish(self.tool_marker) rospy.loginfo("\n%s", PoseStamped(data.header, data.pose)) self.server.applyChanges()
vehicleMarker.name = 'quadcopter' q = quaternion_from_euler(0, 0, 0) vehicleMarker.pose.orientation.x = q[0] vehicleMarker.pose.orientation.y = q[1] vehicleMarker.pose.orientation.z = q[2] vehicleMarker.pose.orientation.w = q[3] normalizeQuaternion(vehicleMarker.pose.orientation) vehicleMesh = Marker() vehicleMesh.type = Marker.MESH_RESOURCE vehicleMesh.mesh_resource = MARKER_RESOURCE vehicleMesh.scale.x, vehicleMesh.scale.y, vehicleMesh.scale.z = (tuple([vehicleMarker.scale*MARKER_SCALE]))*3 vehicleMesh.color.r, vehicleMesh.color.g, vehicleMesh.color.b = MARKER_COLOR vehicleMesh.color.a = 1.0 vehicleControl = InteractiveMarkerControl() vehicleControl.always_visible = True vehicleControl.markers.append(vehicleMesh) vehicleMarker.controls.append(vehicleControl) server.insert(vehicleMarker, processFeedback) server.applyChanges() #rospy.spin() while not rospy.core.is_shutdown(): print('*********************') rospy.rostime.wallsleep(0.5)
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 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)
class GripperMarkers: _offset = 0.09 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 get_ee_pose(self): from_frame = 'base_link' to_frame = self.side_prefix + '_wrist_roll_link' try: if self._tf_listener.frameExists( from_frame) and self._tf_listener.frameExists(to_frame): t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) # t = rospy.Time.now() (pos, rot) = self._tf_listener.lookupTransform( to_frame, from_frame, t) # Note argument order :( else: rospy.logerr( 'TF frames do not exist; could not get end effector pose.') except Exception as err: rospy.logerr('Could not get end effector pose through TF.') rospy.logerr(err) 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, feedback): rospy.loginfo('You pressed the `Move arm here` button from the menu.') '''Moves the arm to the desired joints''' print feedback time_to_joint = 2.0 positions = self.ik.get_ik_for_ee(feedback.pose) velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (self.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) pass 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_' + self.side_prefix 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])) 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 print self ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution is 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
class InteractiveMarkerPoseStampedPublisher(): 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 processFeedback(self, feedback): """ :type feedback: InteractiveMarkerFeedback """ s = "Feedback from marker '" + feedback.marker_name s += "' / control '" + feedback.control_name + "'" mp = "" if feedback.mouse_point_valid: mp = " at " + str(feedback.mouse_point.x) mp += ", " + str(feedback.mouse_point.y) mp += ", " + str(feedback.mouse_point.z) mp += " in frame " + feedback.header.frame_id if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.logdebug(s + ": button click" + mp + ".") elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".") if feedback.menu_entry_id == 1: rospy.logdebug("Start publishing transform...") if self.tf_br is None: self.tf_br = tf.TransformBroadcaster() elif feedback.menu_entry_id == 2: rospy.logdebug("Stop publishing transform...") self.tf_br = None # When clicking this event triggers! elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: rospy.logdebug(s + ": pose changed") elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: rospy.logdebug(s + ": mouse down" + mp + ".") elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.logdebug(s + ": mouse up" + mp + ".") # TODO: Print here the commands # tf static transform self.print_commands(feedback.pose) self.last_pose = deepcopy(feedback.pose) self.server.applyChanges() def print_commands(self, pose, decimals=4): p = pose.position o = pose.orientation print "\n---------------------------" print "Static transform publisher command (with roll pitch yaw):" common_part = "rosrun tf static_transform_publisher " pos_part = str(round(p.x, decimals)) + " " + str(round(p.y, decimals)) + " "+ str(round(p.z, decimals)) r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) ori_part = str(round(r, decimals)) + " " + str(round(p, decimals)) + " " + str(round(y, decimals)) static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" print " " + static_tf_cmd print print "Static transform publisher command (with quaternion):" ori_q = str(round(o.x, decimals)) + " " + str(round(o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(round(o.w, decimals)) static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50" print " " + static_tf_cmd print print "Roslaunch line of static transform publisher (rpy):" node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf" roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\ pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />' print roslaunch_part print print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" /> print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />' print "\n---------------------------" def makeArrow(self, msg): marker = Marker() # An arrow that's squashed to easier view the orientation on roll marker.type = Marker.ARROW marker.scale.x = 0.08 marker.scale.y = 0.08 marker.scale.z = 0.08 marker.color.r = 0.3 marker.color.g = 0.3 marker.color.b = 0.5 marker.color.a = 1.0 return marker def makeBoxControl(self, msg): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self.makeArrow(msg)) msg.controls.append(control) return control def makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.from_frame int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame # insert a box, well, an arrow self.makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.menu_handler.insert("Publish transform", callback=self.processFeedback) self.menu_handler.insert("Stop publishing transform", callback=self.processFeedback) self.server.insert(int_marker, self.processFeedback) self.menu_handler.apply(self.server, int_marker.name) def run(self): r = pi/2.0 p = pi y = pi/2.0 o = quaternion_from_euler(r, p, y) opt_frame = self.to_frame + "_rgb_optical_frame" r = rospy.Rate(20) while not rospy.is_shutdown(): if self.tf_br is not None: pos = self.last_pose.position ori = self.last_pose.orientation self.tf_br.sendTransform( (pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w), rospy.Time.now(), self.to_frame + "_link", self.from_frame ) ps = PoseStamped() ps.pose = self.last_pose ps.header.frame_id = self.from_frame ps.header.stamp = rospy.Time.now() self.pub.publish(ps) br = tf.TransformBroadcaster() br.sendTransform((0, 0, 0), o, rospy.Time.now(), opt_frame, self.to_frame + "_link") r.sleep()
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 RvizVisualizer(object): '''Cute tool for drawing both depth and height-from-bottom in RVIZ ''' def __init__(self): rospy.init_node('revisualizer') self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2) self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2) self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2) self.kill_server = InteractiveMarkerServer("interactive_kill") # text marker # TODO: Clean this up, there should be a way to set all of this inline self.surface_marker = visualization_msgs.Marker() self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING self.surface_marker.color = ColorRGBA(1, 1, 1, 1) self.surface_marker.scale.z = 0.1 self.depth_marker = visualization_msgs.Marker() self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0) self.depth_marker.scale.z = 0.1 # create marker for displaying current battery voltage self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0) self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5) self.voltage_marker = visualization_msgs.Marker() self.voltage_marker.header.frame_id = "base_link" self.voltage_marker.lifetime = rospy.Duration(5) self.voltage_marker.ns = 'sub' self.voltage_marker.id = 22 self.voltage_marker.pose.position.x = -2.0 self.voltage_marker.scale.z = 0.2 self.voltage_marker.color.a = 1 self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING # create an interactive marker to display kill status and change it self.need_kill_update = True self.kill_marker = InteractiveMarker() self.kill_marker.header.frame_id = "base_link" self.kill_marker.pose.position.x = -2.3 self.kill_marker.name = "kill button" kill_status_marker = Marker() kill_status_marker.type = Marker.TEXT_VIEW_FACING kill_status_marker.text = "UNKILLED" kill_status_marker.id = 55 kill_status_marker.scale.z = 0.2 kill_status_marker.color.a = 1.0 kill_button_control = InteractiveMarkerControl() kill_button_control.name = "kill button" kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON kill_button_control.markers.append(kill_status_marker) self.kill_server.insert(self.kill_marker, self.kill_buttton_callback) self.kill_marker.controls.append(kill_button_control) self.killed = False # connect kill marker to kill alarm self.kill_listener = AlarmListener("kill") self.kill_listener.add_callback(self.kill_alarm_callback) self.kill_alarm = AlarmBroadcaster("kill") # distance to bottom self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback) # distance to surface self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback) # battery voltage self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback) def update_kill_button(self): if self.killed: self.kill_marker.controls[0].markers[0].text = "KILLED" self.kill_marker.controls[0].markers[0].color.r = 1 self.kill_marker.controls[0].markers[0].color.g = 0 else: self.kill_marker.controls[0].markers[0].text = "UNKILLED" self.kill_marker.controls[0].markers[0].color.r = 0 self.kill_marker.controls[0].markers[0].color.g = 1 self.kill_server.insert(self.kill_marker) self.kill_server.applyChanges() def kill_alarm_callback(self, alarm): self.need_kill_update = False self.killed = alarm.raised self.update_kill_button() def kill_buttton_callback(self, feedback): if not feedback.event_type == 3: return if self.need_kill_update: return self.need_kill_update = True if self.killed: self.kill_alarm.clear_alarm() else: self.kill_alarm.raise_alarm() def voltage_callback(self, voltage): self.voltage_marker.text = str(round(voltage.data, 2)) + ' volts' self.voltage_marker.header.stamp = rospy.Time() if voltage.data < self.low_battery_threshold: self.voltage_marker.color.r = 1 self.voltage_marker.color.g = 0 elif voltage.data < self.warn_battery_threshold: self.voltage_marker.color.r = 1 self.voltage_marker.color.g = 1 else: self.voltage_marker.color.r = 0 self.voltage_marker.color.g = 1 self.rviz_pub_utils.publish(self.voltage_marker) def depth_callback(self, msg): '''Handle depth data sent from depth sensor''' frame = '/depth' distance = msg.depth marker = self.make_cylinder_marker( np.array([0.0, 0.0, 0.0]), # place at origin length=distance, color=(0.0, 1.0, 0.2, 0.7), # green, frame=frame, id=0 # Keep these guys from overwriting eachother ) self.surface_marker.ns = 'sub' self.surface_marker.header = mil_ros_tools.make_header(frame='/depth') self.surface_marker.pose = marker.pose self.surface_marker.text = str(round(distance, 3)) + 'm' self.surface_marker.id = 0 self.rviz_pub.publish(marker) self.rviz_pub_t.publish(self.depth_marker) def range_callback(self, msg): '''Handle range data grabbed from dvl''' # future: should be /base_link/dvl, no? frame = '/dvl' distance = msg.range # Color a sharper red if we're in danger if distance < 1.0: color = (1.0, 0.1, 0.0, 0.9) else: color = (0.2, 0.8, 0.0, 0.7) marker = self.make_cylinder_marker( np.array([0.0, 0.0, 0.0]), # place at origin length=distance, color=color, # red, frame=frame, up_vector=np.array([0.0, 0.0, -1.0]), # up is down in range world id=1 # Keep these guys from overwriting eachother ) self.depth_marker.ns = 'sub' self.depth_marker.header = mil_ros_tools.make_header(frame='/dvl') self.depth_marker.pose = marker.pose self.depth_marker.text = str(round(distance, 3)) + 'm' self.depth_marker.id = 1 self.rviz_pub_t.publish(self.depth_marker) self.rviz_pub.publish(marker) def make_cylinder_marker(self, base, length, color, frame='/base_link', up_vector=np.array([0.0, 0.0, 1.0]), **kwargs): '''Handle the frustration that Rviz cylinders are designated by their center, not base''' center = base + (up_vector * (length / 2)) pose = Pose( position=mil_ros_tools.numpy_to_point(center), orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0]) ) marker = visualization_msgs.Marker( ns='sub', header=mil_ros_tools.make_header(frame=frame), type=visualization_msgs.Marker.CYLINDER, action=visualization_msgs.Marker.ADD, pose=pose, color=ColorRGBA(*color), scale=Vector3(0.2, 0.2, length), lifetime=rospy.Duration(), **kwargs ) return marker
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 InteractivePose2DTF(Node): """ A node that provides an interactive draggable marker, which publishes a tf """ pub_marker = Publisher('/marker', MarkerArray, queue_size=1) world_frame = Param(str, default='map') target_frame = Param(str, default='base_link') 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 _set_marker(self, color): self.interact.clear() marker = InteractiveMarker( header=Header( frame_id=self.world_frame ), scale=1, name="Robot", controls=[ InteractiveMarkerControl( interaction_mode=InteractiveMarkerControl.MOVE_ROTATE, orientation=Quaternion(*transformations.quaternion_from_euler(0, np.pi/2, 0)), markers=[ Marker( type=Marker.ARROW, scale=Vector3(0.5, 0.05, 0.05), color=color, pose=Pose( orientation=Quaternion(0, 0, 0, 1) ) ) ] ) ], pose=self.marker_pose ) self.interact.insert(marker, self.processFeedback) self.interact.applyChanges() def processFeedback(self, feedback): if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE: return self.marker_pose = feedback.pose @Timer(rospy.Duration(0.1)) def timer_tf_pub(self, event): t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = self.world_frame t.child_frame_id = self.target_frame t.transform.translation = self.marker_pose.position t.transform.rotation = self.marker_pose.orientation self.pub_tf.sendTransform(t) @Subscriber('~color', ColorRGBA) def sub_color(self, color): self._set_marker(color) @Subscriber('/initialpose', PoseWithCovarianceStamped) def sub_initialpose(self, pose): """ The default topic used by rviz """ self.marker_pose = pose.pose.pose self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))
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() # 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 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.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] 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.') joint_positions = self.ik.get_ik_for_ee(self.ee_pose) print 'joint: ' + str(joint_positions) if (joint_positions != None): self.toggle_arm(self.side_prefix, 'Freeze', False) #joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309] print 'returned from IK: ' + str(joint_positions) #print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]) #joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124] self.move_to_joints(self.side_prefix, list(joint_positions), 5.0) print 'done' def toggle_arm(self, side, toggle, button): controller_name = side + '_arm_controller' print 'toggle' start_controllers = [] stop_controllers = [] if (toggle == 'Relax'): stop_controllers.append(controller_name) else: start_controllers.append(controller_name) self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=positions, velocities=velocities, time_from_start=rospy.Duration(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) 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])) 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 mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6) ik_solution = self.ik.get_ik_for_ee(self.ee_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) 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 IMServer(object): """ Spawns interactive Marker which send cart goals to action server. Does not interact with god map. """ def __init__(self, root_tips, suffix=u'', giskard_name=u'giskardpy/command'): """ :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(giskard_name) if len(root_tips) > 0: self.roots, self.tips = zip(*root_tips) else: self.roots = [] self.tips = [] self.suffix = suffix self.giskard_name = giskard_name 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 make_sphere(self, msg): """ :param msg: :return: :rtype: Marker """ marker = Marker() marker.type = Marker.SPHERE marker.scale.x = msg.scale * MARKER_SCALE * 2 marker.scale.y = msg.scale * MARKER_SCALE * 2 marker.scale.z = msg.scale * MARKER_SCALE * 2 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 0.5 return marker def make_sphere_control(self, msg): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self.make_sphere(msg)) msg.controls.append(control) return control def make_6dof_marker(self, interaction_mode, root_link, tip_link): def normed_q(x, y, z, w): return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w]) int_marker = InteractiveMarker() int_marker.header.frame_id = tip_link int_marker.pose.orientation.w = 1 int_marker.scale = MARKER_SCALE int_marker.name = u'eef_{}_to_{}'.format(root_link, tip_link) # insert a box self.make_sphere_control(int_marker) int_marker.controls[0].interaction_mode = interaction_mode if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D: u'MOVE_3D', InteractiveMarkerControl.ROTATE_3D: u'ROTATE_3D', InteractiveMarkerControl.MOVE_ROTATE_3D: u'MOVE_ROTATE_3D' } int_marker.name += u'_' + control_modes_dict[interaction_mode] control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = u'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = u'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 1, 0, 1)) control.name = u'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 1, 0, 1)) control.name = u'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 0, 1, 1)) control.name = u'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 0, 1, 1)) control.name = u'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) self.markers[int_marker.name] = int_marker return int_marker class process_feedback(object): def __init__(self, i_server, marker_name, root_link, tip_link, giskard, enable_self_collision): """ :param i_server: :type i_server: InteractiveMarkerServer :param marker_name: :type marker_name: str :param client: :type client: SimpleActionClient :param root_link: :type root_link: str :param tip_link: :type tip_link: str :param giskard: :type giskard: GiskardWrapper :param enable_self_collision: :type enable_self_collision: bool """ self.i_server = i_server self.marker_name = marker_name self.tip_link = tip_link self.root_link = root_link self.giskard = giskard self.enable_self_collision = enable_self_collision self.marker_pub = rospy.Publisher(u'visualization_marker_array', MarkerArray, queue_size=10) def __call__(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: logging.loginfo(u'got interactive goal update') p = PoseStamped() p.header.frame_id = feedback.header.frame_id p.pose = feedback.pose self.giskard.set_cart_goal(self.root_link, self.tip_link, p) if not self.enable_self_collision: self.giskard.allow_self_collision() self.giskard.plan_and_execute(wait=False) self.pub_goal_marker(feedback.header, feedback.pose) self.i_server.setPose(self.marker_name, Pose()) self.i_server.applyChanges() def pub_goal_marker(self, header, pose): """ :param header: :type header: std_msgs.msg._Header.Header :param pose: :type pose: Pose """ ma = MarkerArray() m = Marker() m.action = Marker.ADD m.type = Marker.CYLINDER m.header = header old_q = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] # x m.pose = deepcopy(pose) m.scale.x = 0.05 * MARKER_SCALE m.scale.y = 0.05 * MARKER_SCALE m.scale.z = MARKER_SCALE muh = qv_mult(old_q, [m.scale.z / 2, 0, 0]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.pose.orientation = Quaternion(*quaternion_multiply( old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0]))) m.color.r = 1 m.color.g = 0 m.color.b = 0 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 0 ma.markers.append(m) # y m = deepcopy(m) m.pose = deepcopy(pose) muh = qv_mult(old_q, [0, m.scale.z / 2, 0]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.pose.orientation = Quaternion(*quaternion_multiply( old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0]))) m.color.r = 0 m.color.g = 1 m.color.b = 0 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 1 ma.markers.append(m) # z m = deepcopy(m) m.pose = deepcopy(pose) muh = qv_mult(old_q, [0, 0, m.scale.z / 2]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.color.r = 0 m.color.g = 0 m.color.b = 1 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 2 ma.markers.append(m) self.marker_pub.publish(ma)
int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) server.insert(int_marker, processFeedback) server.applyChanges() rospy.spin()
class TrajectoryAnalyzer(): 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 _retrieve_logs(self, marker_name): #logs = self._client.message_store.people_perception_marathon_uob.find() logs = self._client.message_store.people_perception.find() for log in logs: #print "logs: " + repr(log) #print "log keys: " + repr(log.keys()) for i, uuid in enumerate(log['uuids']): #if uuid not in ['21c75fa0-2ed9-5359-b4db-250142fe0f5d', '89c29b5f-e568-56ea-bca2-f3e59ddff3f7', '0824a8d9-cf9c-5aca-89fc-03e08c14275f']: # continue if uuid not in self._traj: t = Trajectory(uuid) t.append_pose(log['people'][i], log['header']['stamp']['secs'], log['header']['stamp']['nsecs']) self._traj[uuid] = t else: t = self._traj[uuid] t.append_pose(log['people'][i], log['header']['stamp']['secs'], log['header']['stamp']['nsecs']) #print "pose x,y: " + repr(t.uuid) + repr(t.pose[0]['position'][u'x']) + ", " + repr( t.pose[0]['position']['y']) #print "" #sys.exit(1) def visualize_trajectories(self, mode="all", average_length=0, longest_length=0): counter = 0 for uuid in self._traj: if len(self._traj[uuid].pose) > 1: if mode == "average": if abs(self._traj[uuid].length - average_length) \ < (average_length / 10): self.visualize_trajectory(self._traj[uuid]) counter += 1 elif mode == "longest": if abs(self._traj[uuid].length - longest_length) \ < (longest_length / 10): self.visualize_trajectory(self._traj[uuid]) counter += 1 elif mode == "shortest": if self._traj[uuid].length < 1: self.visualize_trajectory(self._traj[uuid]) counter += 1 else: self.visualize_trajectory(self._traj[uuid]) #print "uuid: " + repr(uuid) #raw_input("Press 'Enter' for the next trajectory.") #self.delete_trajectory(self._traj[uuid]) counter += 1 rospy.loginfo("Total Trajectories: " + str(len(self._traj))) rospy.loginfo("Printed trajectories: " + str(counter)) self.delete_trajectory(self._traj[uuid]) def _update_cb(self, feedback): return def visualize_trajectory(self, traj): int_marker = self.create_trajectory_marker(traj) self._server.insert(int_marker, self._update_cb) self._server.applyChanges() def delete_trajectory(self, traj): self._server.erase(traj.uuid) self._server.applyChanges() def create_trajectory_marker(self, traj): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = traj.uuid # int_marker.description = traj.uuid pose = Pose() pose.position.x = traj.pose[0]['position']['x'] pose.position.y = traj.pose[0]['position']['y'] int_marker.pose = pose # for i in range(len(traj.pose)): # print "Velocity: ", traj.vel[i] # print "X,Y: ", traj.pose[i]['position']['x'],\ # traj.pose[i]['position']['y'] # print "Time: ", str(traj.secs[i]) + "." + str(traj.nsecs[i]) # print traj.max_vel, traj.length line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.05 # random.seed(traj.uuid) # val = random.random() # line_marker.color.r = r_func(val) # line_marker.color.g = g_func(val) # line_marker.color.b = b_func(val) # line_marker.color.a = 1.0 line_marker.points = [] MOD = 1 for i, point in enumerate(traj.pose): if i % MOD == 0: x = point['position']['x'] y = point['position']['y'] p = Point() p.x = x - int_marker.pose.position.x p.y = y - int_marker.pose.position.y line_marker.points.append(p) line_marker.colors = [] for i, vel in enumerate(traj.vel): if i % MOD == 0: color = ColorRGBA() if traj.max_vel == 0: val = vel / 0.01 else: val = vel / traj.max_vel color.r = r_func(val) color.g = g_func(val) color.b = b_func(val) color.a = 1.0 line_marker.colors.append(color) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.markers.append(line_marker) int_marker.controls.append(control) return int_marker