Is there any way to subscribe to the pose of the base of a simulated PR2
robot? Ideally it should look something like this:
ros::Subscriber sub_odom = n.subscribe("base_pose_ground_truth", 100,
recordOdom);
//callback to store 0dometry data
void recordOdom(const nav_msgs::Odometry::ConstPtr& data){
xPos=data->pose.pose.position.x;
yPos=data->pose.pose.position.y;
//get Quaternion anglular information
double x=data->pose.pose.orientation.x;
double y=data->pose.pose.orientation.y;
double z=data->pose.pose.orientation.z;
double w=data->pose.pose.orientation.w;
//convert to pitch
angle=atan2(2*(y*x+w*z),w*w+x*x-y*y-z*z);
}
The bottom line is, I would like to keep track of the angle of the PR2 robot
base over time, but I cannot figure out how to subscribe to the pose. Any
ideas?
Thanks,
--
Sebastian Castro
Mechanical & Aerospace Engineering
Cornell University, Class of 2010
------------------------------------------------------------------------------
Download Intel® Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev_______________________________________________
ros-users mailing list
ros-users@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/ros-users