Esempio n. 1
0
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()
Esempio n. 2
0
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()
Esempio n. 3
0
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()
Esempio n. 9
0
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()
Esempio n. 10
0
    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()
Esempio n. 11
0
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)
Esempio n. 12
0
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'
Esempio n. 13
0
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))
Esempio n. 14
0
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()
Esempio n. 15
0
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)
Esempio n. 16
0
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()
Esempio n. 17
0
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
Esempio n. 19
0
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
Esempio n. 20
0
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
Esempio n. 21
0
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)
Esempio n. 22
0
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 = {}
Esempio n. 23
0
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
Esempio n. 24
0
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()
Esempio n. 25
0
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()
Esempio n. 26
0
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()
Esempio n. 30
0
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()
Esempio n. 31
0
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
Esempio n. 32
0
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()
Esempio n. 34
0
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
Esempio n. 35
0
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()
Esempio n. 36
0
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
Esempio n. 37
0
    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()
Esempio n. 38
0
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
Esempio n. 39
0
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)
Esempio n. 40
0
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
Esempio n. 41
0
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()
Esempio n. 42
0
    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)
Esempio n. 44
0
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)
Esempio n. 45
0
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()
Esempio n. 47
0
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)
Esempio n. 48
0
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)
Esempio n. 50
0
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))
Esempio n. 51
0
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
Esempio n. 52
0
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)
Esempio n. 53
0
    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