conversions.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Author: Ioan Sucan
00034 
00035 import StringIO
00036 from moveit_commander import MoveItCommanderException
00037 from geometry_msgs.msg import Pose, PoseStamped, Transform
00038 import rospy
00039 import tf
00040 
00041 def msg_to_string(msg):
00042     buf = StringIO.StringIO()
00043     msg.serialize(buf)
00044     return buf.getvalue()
00045 
00046 def msg_from_string(msg, data):
00047     msg.deserialize(data)
00048 
00049 def pose_to_list(pose_msg):
00050     pose = []
00051     pose.append(pose_msg.position.x)
00052     pose.append(pose_msg.position.y)
00053     pose.append(pose_msg.position.z) 
00054     pose.append(pose_msg.orientation.x)
00055     pose.append(pose_msg.orientation.y)
00056     pose.append(pose_msg.orientation.z)
00057     pose.append(pose_msg.orientation.w)
00058     return pose
00059 
00060 def list_to_pose(pose_list):
00061     pose_msg = Pose()
00062     if len(pose_list) == 7:
00063         pose_msg.position.x = pose_list[0]
00064         pose_msg.position.y = pose_list[1]
00065         pose_msg.position.z = pose_list[2]
00066         pose_msg.orientation.x = pose_list[3]
00067         pose_msg.orientation.y = pose_list[4]
00068         pose_msg.orientation.z = pose_list[5]
00069         pose_msg.orientation.w = pose_list[6]
00070     elif len(pose_list) == 6: 
00071         pose_msg.position.x = pose_list[0]
00072         pose_msg.position.y = pose_list[1]
00073         pose_msg.position.z = pose_list[2]
00074         q = tf.transformations.quaternion_from_euler(pose_list[3], pose_list[4], pose_list[5])
00075         pose_msg.orientation.x = q[0]
00076         pose_msg.orientation.y = q[1]
00077         pose_msg.orientation.z = q[2]
00078         pose_msg.orientation.w = q[3]
00079     else:
00080         raise MoveItCommanderException("Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)")
00081     return pose_msg
00082 
00083 def list_to_pose_stamped(pose_list, target_frame):
00084     pose_msg = PoseStamped()
00085     pose_msg.pose = list_to_pose(pose_list)
00086     pose_msg.header.frame_id = target_frame
00087     pose_msg.header.stamp = rospy.Time.now()
00088     return pose_msg
00089 
00090 def transform_to_list(trf_msg):
00091     trf = []
00092     trf.append(trf_msg.translation.x)
00093     trf.append(trf_msg.translation.y)
00094     trf.append(trf_msg.translation.z) 
00095     trf.append(trf_msg.rotation.x)
00096     trf.append(trf_msg.rotation.y)
00097     trf.append(trf_msg.rotation.z)
00098     trf.append(trf_msg.rotation.w)
00099     return trf
00100 
00101 def list_to_transform(trf_list):
00102     trf_msg = Transform()
00103     trf_msg.translation.x = trf_list[0]
00104     trf_msg.translation.y = trf_list[1]
00105     trf_msg.translation.z = trf_list[2]
00106     trf_msg.rotation.x = trf_list[3]
00107     trf_msg.rotation.y = trf_list[4]
00108     trf_msg.rotation.z = trf_list[5]
00109     trf_msg.rotation.w = trf_list[6]
00110     return trf_msg


moveit_commander
Author(s): Ioan Sucan
autogenerated on Wed Aug 26 2015 12:38:52