[ros-users] Subscribing to PR2 Base Odometry

[John Hsu]

pretty much, here's a code that compiles
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
void odomCB(const nav_msgs::Odometry::ConstPtr& msg)
{
    double xPos=msg->pose.pose.position.x;
    double yPos=msg->pose.pose.position.y;
    //get Quaternion anglular information
    double x=msg->pose.pose.orientation.x;
    double y=msg->pose.pose.orientation.y;
    double z=msg->pose.pose.orientation.z;
    double w=msg->pose.pose.orientation.w;
    //convert to pitch
    double angle=atan2(2*(y*x+w*z),w*w+x*x-y*y-z*z);
    ROS_ERROR("%f %f %f",xPos,yPos,angle);
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "stuff");
  ros::NodeHandle nh_("~");
  ros::Subscriber sub_image_ =
nh_.subscribe<nav_msgs::Odometry>("/base_pose_ground_truth", 1, &odomCB);
  ros::spin();
}

On Wed, Mar 31, 2010 at 5:40 PM, Sebastian Castro <sac77 at cornell.edu> wrote:

> 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&#174; 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 at lists.sourceforge.net
> https://lists.sourceforge.net/lists/listinfo/ros-users
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: /discuss/ros-users/attachments/20100331/2ecb061b/attachment-0001.htm