| Note: This tutorial assumes you are familiar with ROS topics and C++.. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
C++ Pose Listener for P2OS Robots
Description: This tutorial will go over the process by which one can read position data from a P2OS robot with ROS and C++.Tutorial Level: BEGINNER
Next Tutorial: P2OS Velocity Controller
Receiving Pose Data
The Pioneer Robots send out the location of the robot on the aptly-named \pose topic. The Velocity data is also included on this topic.
#include <ros/ros.h>
#include <ros/network.h>
#include <nav_msgs/Odometry.h>
/**
* This tutorial demonstrates how to receive a position from a p2os robot.
*/
void poseCallback(const nav_msgs::Odometry& msg)
{
float m_xPos = msg.pose.pose.position.x;
float m_yPos = msg.pose.pose.position.y;
float m_aPos = msg.pose.pose.orientation.w;
ROS_INFO("Pose: (%f, %f, %f)", m_xPos, m_yPos, m_aPos);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pose_listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("pose", 1000, poseCallback);
ros::spin();
return 0;
}Save the above code as echo_pose.cc in the source directory of the workspace established in the setup tutorial. Then, you will need to add the following line to the CMakeLists.txt file:
rosbuild_add_executable(echo_pose src/echo_pose.cc)
You can now build your package and run the program.
rosmake p2osTutorials rosrun p2osTutorials echo_pose
You should get something like this:
allenh1@allenh1-Vostro-430:~/filter_ws/src/p2osTutorial$ rosrun p2osTutorial echo_pose [ INFO] [1370978321.971799307]: Pose: (0.444000, 0.060000, 0.965926) [ INFO] [1370978322.072227095]: Pose: (0.466000, 0.073000, 0.965926) [ INFO] [1370978322.171977566]: Pose: (0.488000, 0.086000, 0.965926) [ INFO] [1370978322.273637683]: Pose: (0.510000, 0.099000, 0.965926) [ INFO] [1370978322.379544725]: Pose: (0.532000, 0.112000, 0.965926) [ INFO] [1370978322.471058046]: Pose: (0.554000, 0.125000, 0.965926) [ INFO] [1370978322.575270839]: Pose: (0.576000, 0.138000, 0.965926) [ INFO] [1370978322.671572332]: Pose: (0.597000, 0.151000, 0.965926) [ INFO] [1370978322.772321894]: Pose: (0.619000, 0.164000, 0.965926) [ INFO] [1370978322.871517830]: Pose: (0.641000, 0.177000, 0.965926) [ INFO] [1370978322.972331363]: Pose: (0.660000, 0.188000, 0.965926) [ INFO] [1370978323.074671745]: Pose: (0.673000, 0.195000, 0.965926) [ INFO] [1370978323.171153673]: Pose: (0.676000, 0.197000, 0.963630) [ INFO] [1370978323.271778271]: Pose: (0.676000, 0.197000, 0.958820) [ INFO] [1370978323.370652138]: Pose: (0.676000, 0.197000, 0.953717) [ INFO] [1370978323.471004712]: Pose: (0.676000, 0.197000, 0.948324) [ INFO] [1370978323.575059937]: Pose: (0.676000, 0.197000, 0.942641) [ INFO] [1370978323.671498591]: Pose: (0.676000, 0.197000, 0.936672) [ INFO] [1370978323.774092285]: Pose: (0.676000, 0.197000, 0.930418) [ INFO] [1370978323.879477949]: Pose: (0.678000, 0.199000, 0.927184) [ INFO] [1370978323.971059995]: Pose: (0.682000, 0.204000, 0.923880) [ INFO] [1370978324.075550571]: Pose: (0.691000, 0.213000, 0.923880) [ INFO] [1370978324.171853674]: Pose: (0.706000, 0.228000, 0.923880) [ INFO] [1370978324.273042023]: Pose: (0.723000, 0.245000, 0.923880) [ INFO] [1370978324.382818795]: Pose: (0.742000, 0.263000, 0.923880) [ INFO] [1370978324.472303890]: Pose: (0.760000, 0.281000, 0.923880) [ INFO] [1370978324.575456970]: Pose: (0.778000, 0.299000, 0.923880) [ INFO] [1370978324.671267870]: Pose: (0.796000, 0.317000, 0.923880) [ INFO] [1370978324.772792072]: Pose: (0.814000, 0.334000, 0.923880) [ INFO] [1370978324.871772716]: Pose: (0.832000, 0.352000, 0.923880)
