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@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@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/ros-users

_______________________________________________
ros-users mailing list
ros-users@code.ros.org
https://code.ros.org/mailman/listinfo/ros-users