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])
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)
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")
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)
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