robot_pose_publisher.cpp
Go to the documentation of this file.
00001 
00012 #include <geometry_msgs/PoseStamped.h>
00013 #include <geometry_msgs/Pose.h>
00014 #include <ros/ros.h>
00015 #include <tf/transform_listener.h>
00016 
00024 int main(int argc, char ** argv)
00025 {
00026   // initialize ROS and the node
00027   ros::init(argc, argv, "robot_pose_publisher");
00028   ros::NodeHandle nh;
00029   ros::NodeHandle nh_priv("~");
00030 
00031   // configuring parameters
00032   std::string map_frame, base_frame;
00033   double publish_frequency;
00034   bool is_stamped;
00035   ros::Publisher p_pub;
00036 
00037   nh_priv.param<std::string>("map_frame",map_frame,"/map");
00038   nh_priv.param<std::string>("base_frame",base_frame,"/base_link");
00039   nh_priv.param<double>("publish_frequency",publish_frequency,10);
00040   nh_priv.param<bool>("is_stamped", is_stamped, false);
00041 
00042   if(is_stamped)
00043     p_pub = nh.advertise<geometry_msgs::PoseStamped>("robot_pose", 1);
00044   else 
00045     p_pub = nh.advertise<geometry_msgs::Pose>("robot_pose", 1);
00046 
00047   // create the listener
00048   tf::TransformListener listener;
00049   listener.waitForTransform(map_frame, base_frame, ros::Time(), ros::Duration(1.0));
00050 
00051   ros::Rate rate(publish_frequency);
00052   while (nh.ok())
00053   {
00054     tf::StampedTransform transform;
00055     try
00056     {
00057       listener.lookupTransform(map_frame, base_frame, ros::Time(0), transform);
00058 
00059       // construct a pose message
00060       geometry_msgs::PoseStamped pose_stamped;
00061       pose_stamped.header.frame_id = map_frame;
00062       pose_stamped.header.stamp = ros::Time::now();
00063 
00064       pose_stamped.pose.orientation.x = transform.getRotation().getX();
00065       pose_stamped.pose.orientation.y = transform.getRotation().getY();
00066       pose_stamped.pose.orientation.z = transform.getRotation().getZ();
00067       pose_stamped.pose.orientation.w = transform.getRotation().getW();
00068 
00069       pose_stamped.pose.position.x = transform.getOrigin().getX();
00070       pose_stamped.pose.position.y = transform.getOrigin().getY();
00071       pose_stamped.pose.position.z = transform.getOrigin().getZ();
00072 
00073       if(is_stamped)
00074         p_pub.publish(pose_stamped);
00075       else
00076         p_pub.publish(pose_stamped.pose);
00077     }
00078     catch (tf::TransformException &ex)
00079     {
00080       // just continue on
00081     }
00082 
00083     rate.sleep();
00084   }
00085 
00086   return EXIT_SUCCESS;
00087 }


robot_pose_publisher
Author(s): Russell Toris
autogenerated on Thu Jun 6 2019 20:42:37