pose.cpp
Go to the documentation of this file.
00001 #include "common/pose.h"
00002 
00006 slam::Pose::Pose(){}
00007 
00012 void slam::Pose::advertisePoseMsg(ros::NodeHandle nh)
00013 {
00014   // Advertise the pose publication
00015   pose_pub_ = nh.advertise<nav_msgs::Odometry>("corrected_odom", 1);
00016   graph_pub_ = nh.advertise<nav_msgs::Odometry>("graph", 1);
00017 }
00018 
00025 tf::Transform slam::Pose::correctOdom( tf::Transform current_odom,
00026                                               tf::Transform last_graph_pose,
00027                                               tf::Transform last_graph_odom)
00028 {
00029   // Odometry difference
00030   tf::Transform odom_diff = last_graph_odom.inverse() * current_odom;
00031 
00032   // Compute the corrected pose
00033   return last_graph_pose * odom_diff;
00034 }
00035 
00042 void slam::Pose::publish(nav_msgs::Odometry odom_msg, tf::Transform pose, bool publish_graph)
00043 {
00044   // Publish pose
00045   if (pose_pub_.getNumSubscribers() > 0)
00046   {
00047     nav_msgs::Odometry pose_msg = odom_msg;
00048     pose_msg.header.stamp = odom_msg.header.stamp;
00049     pose_msg.header.frame_id = params_.pose_frame_id;
00050     pose_msg.child_frame_id = params_.pose_child_frame_id;
00051     tf::poseTFToMsg(pose, pose_msg.pose.pose);
00052     pose_pub_.publish(pose_msg);
00053   }
00054   if (graph_pub_.getNumSubscribers() > 0 and publish_graph)
00055   {
00056     nav_msgs::Odometry pose_msg = odom_msg;
00057     pose_msg.header.stamp = odom_msg.header.stamp;
00058     pose_msg.header.frame_id = params_.pose_frame_id;
00059     pose_msg.child_frame_id = params_.pose_child_frame_id;
00060     tf::poseTFToMsg(pose, pose_msg.pose.pose);
00061     graph_pub_.publish(pose_msg);
00062   }
00063 }
00064 


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Aug 27 2015 15:24:22