37 from StringIO
import StringIO
40 from io
import BytesIO
as StringIO
42 from moveit_commander
import MoveItCommanderException
43 from geometry_msgs.msg
import Pose, PoseStamped, Transform
57 pose.append(pose_msg.position.x)
58 pose.append(pose_msg.position.y)
59 pose.append(pose_msg.position.z)
60 pose.append(pose_msg.orientation.x)
61 pose.append(pose_msg.orientation.y)
62 pose.append(pose_msg.orientation.z)
63 pose.append(pose_msg.orientation.w)
68 if len(pose_list) == 7:
69 pose_msg.position.x = pose_list[0]
70 pose_msg.position.y = pose_list[1]
71 pose_msg.position.z = pose_list[2]
72 pose_msg.orientation.x = pose_list[3]
73 pose_msg.orientation.y = pose_list[4]
74 pose_msg.orientation.z = pose_list[5]
75 pose_msg.orientation.w = pose_list[6]
76 elif len(pose_list) == 6:
77 pose_msg.position.x = pose_list[0]
78 pose_msg.position.y = pose_list[1]
79 pose_msg.position.z = pose_list[2]
80 q = tf.transformations.quaternion_from_euler(pose_list[3], pose_list[4], pose_list[5])
81 pose_msg.orientation.x = q[0]
82 pose_msg.orientation.y = q[1]
83 pose_msg.orientation.z = q[2]
84 pose_msg.orientation.w = q[3]
86 raise MoveItCommanderException(
"Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)")
90 pose_msg = PoseStamped()
92 pose_msg.header.frame_id = target_frame
93 pose_msg.header.stamp = rospy.Time.now()
98 trf.append(trf_msg.translation.x)
99 trf.append(trf_msg.translation.y)
100 trf.append(trf_msg.translation.z)
101 trf.append(trf_msg.rotation.x)
102 trf.append(trf_msg.rotation.y)
103 trf.append(trf_msg.rotation.z)
104 trf.append(trf_msg.rotation.w)
108 trf_msg = Transform()
109 trf_msg.translation.x = trf_list[0]
110 trf_msg.translation.y = trf_list[1]
111 trf_msg.translation.z = trf_list[2]
112 trf_msg.rotation.x = trf_list[3]
113 trf_msg.rotation.y = trf_list[4]
114 trf_msg.rotation.z = trf_list[5]
115 trf_msg.rotation.w = trf_list[6]
def pose_to_list(pose_msg)
def list_to_transform(trf_list)
def list_to_pose_stamped(pose_list, target_frame)
def transform_to_list(trf_msg)
def list_to_pose(pose_list)
def msg_from_string(msg, data)