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 moveit_commander import MoveItCommanderException
43 from geometry_msgs.msg import Pose, PoseStamped, Transform
44 import rospy
45 import tf
46 
47 def msg_to_string(msg):
48  buf = StringIO()
49  msg.serialize(buf)
50  return buf.getvalue()
51 
52 def msg_from_string(msg, data):
53  msg.deserialize(data)
54 
55 def pose_to_list(pose_msg):
56  pose = []
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)
64  return pose
65 
66 def list_to_pose(pose_list):
67  pose_msg = Pose()
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]
85  else:
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)")
87  return pose_msg
88 
89 def list_to_pose_stamped(pose_list, target_frame):
90  pose_msg = PoseStamped()
91  pose_msg.pose = list_to_pose(pose_list)
92  pose_msg.header.frame_id = target_frame
93  pose_msg.header.stamp = rospy.Time.now()
94  return pose_msg
95 
96 def transform_to_list(trf_msg):
97  trf = []
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)
105  return trf
106 
107 def list_to_transform(trf_list):
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]
116  return trf_msg
def list_to_transform(trf_list)
Definition: conversions.py:107
def list_to_pose_stamped(pose_list, target_frame)
Definition: conversions.py:89
def transform_to_list(trf_msg)
Definition: conversions.py:96
def list_to_pose(pose_list)
Definition: conversions.py:66
def msg_from_string(msg, data)
Definition: conversions.py:52


moveit_commander
Author(s): Ioan Sucan
autogenerated on Mon Nov 23 2020 03:54:19