Only released in EOL distros:
Package Summary
Contains pose_converter, a set of Python scripts which automatically detect the type of the input position/orientation pose and allows you to convert to any of the other types in a single call. Also contains a copy of transformations.py
- Author: Kelsey Hawkins / kphawkins@gatech.edu, Advisor: Prof. Charlie Kemp (Healthcare Robotics Lab at Georgia Tech)
- License: BSD
- Source: git https://code.google.com/p/gt-ros-pkg.hrl-kdl/ (branch: master)
Package Summary
Contains pose_converter, a set of Python scripts which automatically detect the type of the input position/orientation pose and allows you to convert to any of the other types in a single call. Also contains a copy of transformations.py
- Maintainer: Kelsey Hawkins <kphawkins AT gatech DOT edu>
- Author: Kelsey Hawkins <kphawkins AT gatech DOT edu>
- License: BSD
- Source: git https://github.com/gt-ros-pkg/hrl-kdl.git (branch: hydro)
Contents
PoseConv Usage
Each of the methods in PoseConv converts a pose-like Python tuple, numpy matrix or tuple, or ROS message into another. Each of the methods is called using PoseConv.to_%pose_type% where %pose_type% is in PoseConv.POSE_TYPES. Notes on various types:
*_msg - refers to a ROS message of similar name.
homo_mat - homogeneous matrix 4x4 numpy.mat.
pos_rot - 3x1 numpy.mat position, 3x3 numpy.mat
pos_quat - list of length 3, list of length 4
pos_euler - list of length 3, list of length 3
The input parsing is fairly forgiving on the exact type and shape of the Python/numpy arguments.
The first argument can optionally be a string, representing the designed header frame_id if the requested type is a stamped message. The stamp is always set as the current time and seq is 0. After the optional header string, you can pass the pose. If the input type is a tuple, you can pass either the complete tuple as one argument or split it into two arguments.
1 import roslib; roslib.load_manifest("hrl_geom")
2 from hrl_geom.pose_converter import PoseConv
3 pos_euler = [[0.1, 0.2, 0.3], [0.3, 0., 0.]]
4 twist_msg = PoseConv.to_twist_msg(pos_euler)
5 print twist_msg
6 #linear:
7 # x: 0.1
8 # y: 0.2
9 # z: 0.3
10 #angular:
11 # x: 0.3
12 # y: 0.0
13 # z: 0.0
14 homo_mat = PoseConv.to_homo_mat(twist_msg)
15 print homo_mat
16 #[[ 1. 0. 0. 0.1 ]
17 # [ 0. 0.95533649 -0.29552021 0.2 ]
18 # [-0. 0.29552021 0.95533649 0.3 ]
19 # [ 0. 0. 0. 1. ]]
20 pos, rot = homo_mat[:3,3], homo_mat[:3,:3]
21 tf_stamped_msg = PoseConv.to_tf_stamped_msg("base_link", pos, rot)
22 print tf_stamped_msg
23 #header:
24 # seq: 0
25 # stamp:
26 # secs: 1341611677
27 # nsecs: 579762935
28 # frame_id: base_link
29 #child_frame_id: ''
30 #transform:
31 # translation:
32 # x: 0.1
33 # y: 0.2
34 # z: 0.3
35 # rotation:
36 # x: 0.149438132474
37 # y: 0.0
38 # z: 0.0
39 # w: 0.988771077936
40 new_pos_euler = PoseConv.to_pos_euler(tf_stamped_msg)
41 print new_pos_euler
42 #([0.10000000000000001, 0.20000000000000001, 0.29999999999999999], (0.30000000000000004, -0.0, 0.0))
Unit Testing
You can also run the main file to see some unit tests converting everything to everything else:
roscore # if not running already rosrun hrl_geom pose_converter.py