moveit2
The MoveIt Motion Planning Framework for ROS 2.
conversions.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2012, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 #
33 # Author: Ioan Sucan
34 
35 try:
36  # Try Python 2.7 behaviour first
37  from StringIO import StringIO
38 except ImportError:
39  # Use Python 3.x behaviour as fallback and choose the non-unicode version
40  from io import BytesIO as StringIO
41 
42 from .exception import MoveItCommanderException
43 from geometry_msgs.msg import Pose, PoseStamped, Transform
44 import rospy
45 import tf
46 
47 
48 def msg_to_string(msg):
49  buf = StringIO()
50  msg.serialize(buf)
51  return buf.getvalue()
52 
53 
54 def msg_from_string(msg, data):
55  msg.deserialize(data)
56 
57 
58 def pose_to_list(pose_msg):
59  pose = []
60  pose.append(pose_msg.position.x)
61  pose.append(pose_msg.position.y)
62  pose.append(pose_msg.position.z)
63  pose.append(pose_msg.orientation.x)
64  pose.append(pose_msg.orientation.y)
65  pose.append(pose_msg.orientation.z)
66  pose.append(pose_msg.orientation.w)
67  return pose
68 
69 
70 def list_to_pose(pose_list):
71  pose_msg = Pose()
72  if len(pose_list) == 7:
73  pose_msg.position.x = pose_list[0]
74  pose_msg.position.y = pose_list[1]
75  pose_msg.position.z = pose_list[2]
76  pose_msg.orientation.x = pose_list[3]
77  pose_msg.orientation.y = pose_list[4]
78  pose_msg.orientation.z = pose_list[5]
79  pose_msg.orientation.w = pose_list[6]
80  elif len(pose_list) == 6:
81  pose_msg.position.x = pose_list[0]
82  pose_msg.position.y = pose_list[1]
83  pose_msg.position.z = pose_list[2]
84  q = tf.transformations.quaternion_from_euler(
85  pose_list[3], pose_list[4], pose_list[5]
86  )
87  pose_msg.orientation.x = q[0]
88  pose_msg.orientation.y = q[1]
89  pose_msg.orientation.z = q[2]
90  pose_msg.orientation.w = q[3]
91  else:
93  "Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)"
94  )
95  return pose_msg
96 
97 
98 def list_to_pose_stamped(pose_list, target_frame):
99  pose_msg = PoseStamped()
100  pose_msg.pose = list_to_pose(pose_list)
101  pose_msg.header.frame_id = target_frame
102  pose_msg.header.stamp = rospy.Time.now()
103  return pose_msg
104 
105 
106 def transform_to_list(trf_msg):
107  trf = []
108  trf.append(trf_msg.translation.x)
109  trf.append(trf_msg.translation.y)
110  trf.append(trf_msg.translation.z)
111  trf.append(trf_msg.rotation.x)
112  trf.append(trf_msg.rotation.y)
113  trf.append(trf_msg.rotation.z)
114  trf.append(trf_msg.rotation.w)
115  return trf
116 
117 
118 def list_to_transform(trf_list):
119  trf_msg = Transform()
120  trf_msg.translation.x = trf_list[0]
121  trf_msg.translation.y = trf_list[1]
122  trf_msg.translation.z = trf_list[2]
123  trf_msg.rotation.x = trf_list[3]
124  trf_msg.rotation.y = trf_list[4]
125  trf_msg.rotation.z = trf_list[5]
126  trf_msg.rotation.w = trf_list[6]
127  return trf_msg
def list_to_pose(pose_list)
Definition: conversions.py:70
def list_to_transform(trf_list)
Definition: conversions.py:118
def msg_from_string(msg, data)
Definition: conversions.py:54
def list_to_pose_stamped(pose_list, target_frame)
Definition: conversions.py:98