Пример #1
0
 def draw_axes(self, pose_array, rgba = (0,1,0,1), arrow_scale = .1, ns = "default_ns", duration=0):
     marker_array = MarkerArray()
     for pose in pose_array.poses:
         pose_trans, pose_rot = conversions.pose_to_trans_rot(pose)
         x_arrow_pose = conversions.trans_rot_to_pose(pose_trans, (0,0,0,1))
         x_arrow = self.make_arrow_marker(pose_array.header, x_arrow_pose, rgba, arrow_scale, ns)
         marker_array.markers.append(x_arrow)
         y_arrow_pose = conversions.trans_rot_to_pose(pose_trans, (0,0,-2**0.5/2,2**0.5/2))
         y_arrow = self.make_arrow_marker(pose_array.header, y_arrow_pose, rgba, arrow_scale, ns)
         marker_array.markers.append(y_arrow)
         z_arrow_pose = conversions.trans_rot_to_pose(pose_trans, (0,-2**0.5/2,0,2**0.5/2))
         z_arrow = self.make_arrow_marker(pose_array.header, z_arrow_pose, rgba, arrow_scale, ns)
         marker_array.markers.append(z_arrow)
     self.array_pub.publish(marker_array)
     return MarkerListHandle([MarkerHandle(marker, self.pub) for marker in marker_array.markers])
Пример #2
0
 def run(self):
     while not rospy.is_shutdown() and not self.wants_exit:
         rot = quaternion_from_euler(*self.transform.R)
         trans = self.transform.T
         self.br.sendTransform(trans, rot, rospy.Time.now(), "/camera_link", PARENT_FRAME)
         ps = gm.PoseStamped()
         ps.pose = trans_rot_to_pose(trans, rot)
         ps.header.frame_id = "/camera_link"
         ps.header.stamp = rospy.Time.now()
         self.pose_pub.publish(ps)
         rospy.sleep(0.01)
Пример #3
0
 def draw(self):
     origin = [self.cx, self.cy, self.cz]
     quat = conversions.yaw_to_quat(self.theta)
     pose = conversions.trans_rot_to_pose(origin, quat)
     ps = gm.PoseStamped()
     ps.pose = pose
     ps.header.frame_id = "/base_link"
     ps.header.stamp = rospy.Time(0)
     print "(%.3f, %.3f, %.3f), (%.3f, %.3f, %.3f, %.3f), (%.3f, %.3f, %.3f)"%(
         self.cx, self.cy, self.cz, quat[0], quat[1], quat[2], quat[3], self.sx/2., self.sy/2., self.sz/2.)
     #rviz.draw_marker(ps, 0, type=Marker.CUBE, rgba=(0,1,0,.25), scale=(self.sx,self.sy,self.sz))
     self.marker_handle = rviz.place_kin_tree_from_link(ps, "base_link")
Пример #4
0
 def run(self):
     while not rospy.is_shutdown() and not self.wants_exit:
         rot = quaternion_from_euler(*self.transform.R)
         trans = self.transform.T
         self.br.sendTransform(trans, rot, rospy.Time.now(),
                               "/camera2_link", PARENT_FRAME)
         ps = gm.PoseStamped()
         ps.pose = trans_rot_to_pose(trans, rot)
         ps.header.frame_id = PARENT_FRAME
         ps.header.stamp = rospy.Time.now()
         self.pose_pub.publish(ps)
         rospy.sleep(.01)
Пример #5
0
    def goto_pose(self, xya, frame_id):
        trans,rot = conv.xya_to_trans_rot(xya)
        pose = conv.trans_rot_to_pose(trans, rot)
        ps = gm.PoseStamped()
        ps.pose = pose
        ps.header.frame_id = frame_id
        ps.header.stamp = rospy.Time(0)

        goal = mbm.MoveBaseGoal()
        goal.target_pose = ps
        goal.header.frame_id = frame_id
        rospy.loginfo('Sending move base goal')
        finished = self.action_client.send_goal_and_wait(goal)
        rospy.loginfo('Move base action returned %d.' % finished)
        return finished