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 import StringIO
36 from moveit_commander import MoveItCommanderException
37 from geometry_msgs.msg import Pose, PoseStamped, Transform
38 import rospy
39 import tf
40 
41 def msg_to_string(msg):
42  buf = StringIO.StringIO()
43  msg.serialize(buf)
44  return buf.getvalue()
45 
46 def msg_from_string(msg, data):
47  msg.deserialize(data)
48 
49 def pose_to_list(pose_msg):
50  pose = []
51  pose.append(pose_msg.position.x)
52  pose.append(pose_msg.position.y)
53  pose.append(pose_msg.position.z)
54  pose.append(pose_msg.orientation.x)
55  pose.append(pose_msg.orientation.y)
56  pose.append(pose_msg.orientation.z)
57  pose.append(pose_msg.orientation.w)
58  return pose
59 
60 def list_to_pose(pose_list):
61  pose_msg = Pose()
62  if len(pose_list) == 7:
63  pose_msg.position.x = pose_list[0]
64  pose_msg.position.y = pose_list[1]
65  pose_msg.position.z = pose_list[2]
66  pose_msg.orientation.x = pose_list[3]
67  pose_msg.orientation.y = pose_list[4]
68  pose_msg.orientation.z = pose_list[5]
69  pose_msg.orientation.w = pose_list[6]
70  elif len(pose_list) == 6:
71  pose_msg.position.x = pose_list[0]
72  pose_msg.position.y = pose_list[1]
73  pose_msg.position.z = pose_list[2]
74  q = tf.transformations.quaternion_from_euler(pose_list[3], pose_list[4], pose_list[5])
75  pose_msg.orientation.x = q[0]
76  pose_msg.orientation.y = q[1]
77  pose_msg.orientation.z = q[2]
78  pose_msg.orientation.w = q[3]
79  else:
80  raise MoveItCommanderException("Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)")
81  return pose_msg
82 
83 def list_to_pose_stamped(pose_list, target_frame):
84  pose_msg = PoseStamped()
85  pose_msg.pose = list_to_pose(pose_list)
86  pose_msg.header.frame_id = target_frame
87  pose_msg.header.stamp = rospy.Time.now()
88  return pose_msg
89 
90 def transform_to_list(trf_msg):
91  trf = []
92  trf.append(trf_msg.translation.x)
93  trf.append(trf_msg.translation.y)
94  trf.append(trf_msg.translation.z)
95  trf.append(trf_msg.rotation.x)
96  trf.append(trf_msg.rotation.y)
97  trf.append(trf_msg.rotation.z)
98  trf.append(trf_msg.rotation.w)
99  return trf
100 
101 def list_to_transform(trf_list):
102  trf_msg = Transform()
103  trf_msg.translation.x = trf_list[0]
104  trf_msg.translation.y = trf_list[1]
105  trf_msg.translation.z = trf_list[2]
106  trf_msg.rotation.x = trf_list[3]
107  trf_msg.rotation.y = trf_list[4]
108  trf_msg.rotation.z = trf_list[5]
109  trf_msg.rotation.w = trf_list[6]
110  return trf_msg
def list_to_transform(trf_list)
Definition: conversions.py:101
def list_to_pose_stamped(pose_list, target_frame)
Definition: conversions.py:83
def transform_to_list(trf_msg)
Definition: conversions.py:90
def list_to_pose(pose_list)
Definition: conversions.py:60
def msg_from_string(msg, data)
Definition: conversions.py:46


moveit_commander
Author(s): Ioan Sucan
autogenerated on Wed Jul 10 2019 04:04:15