Used the robot pose taken from the costmap in the odom frame (TF) to get the...

Used the robot pose taken from the costmap in the odom frame (TF) to get the current position of the robot (instead of the odom topic.
The rest of Twist and covariance values of the odom topic are still used.

include/ackermann_odom_helper.h

@@ -42,6 +42,7 @@
 #include <nav_msgs/Odometry.h>
 #include <ackermann_msgs/AckermannDriveStamped.h>
 #include <ackermann_msgs/AckermannDrive.h>
+#include <costmap_2d/costmap_2d_ros.h>
 #include <ros/ros.h>
 #include <boost/thread.hpp>
 
@@ -67,6 +68,7 @@ class AckermannOdomHelper
     void set_odom_topic(const std::string &odom_topic);
     void set_ackermann_topic(const std::string &ackermann_topic);
     void set_average_samples(int num_average_samples);
+    void set_costmap(costmap_2d::Costmap2DROS* costmap_ros);
 
     /** @brief Return the current odometry topic. */
     std::string get_odom_topic(void) const;
@@ -96,6 +98,7 @@ class AckermannOdomHelper
     ros::Subscriber odom_sub_;
     nav_msgs::Odometry base_odom_;
     boost::mutex odom_mutex_;
+    costmap_2d::Costmap2DROS* costmap_ros_;
 
     ros::Subscriber ackermann_sub_;
     std::vector<ackermann_msgs::AckermannDrive> ackermann_data_;

src/ackermann_odom_helper.cpp

@@ -41,15 +41,26 @@ AckermannOdomHelper::AckermannOdomHelper(const std::string &odom_topic,const std
   set_odom_topic(odom_topic);
   set_ackermann_topic(ackermann_topic);
   ackermann_data_.resize(num_avg_samples);
+  this->costmap_ros_=NULL;
 }
 
 void AckermannOdomHelper::odom_callback(const nav_msgs::Odometry::ConstPtr& msg) 
 {
+  geometry_msgs::PoseStamped pose_in;
   ROS_INFO_ONCE("Odom received!");
-
+  
   //we assume that the odometry is published in the frame of the base
   boost::mutex::scoped_lock lock(odom_mutex_);
   base_odom_=*msg;
+  if(this->costmap_ros_!=NULL)
+  {
+    if(!this->costmap_ros_->getRobotPose(pose_in))
+    {
+      ROS_ERROR("AckermannOdomHelper: Could not get robot pose");
+      return;
+    }
+    base_odom_.pose.pose=pose_in.pose;
+  }
 }
 
 void AckermannOdomHelper::ackermann_callback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg) 
@@ -147,6 +158,12 @@ void AckermannOdomHelper::set_average_samples(int num_average_samples)
     this->ackermann_data_.resize(num_average_samples);
 }
 
+void AckermannOdomHelper::set_costmap(costmap_2d::Costmap2DROS* costmap_ros)
+{
+  boost::mutex::scoped_lock lock(odom_mutex_);
+  this->costmap_ros_=costmap_ros;
+}
+
 std::string AckermannOdomHelper::get_odom_topic(void) const
 {
   return this->odom_topic_;

src/ackermann_planner_ros.cpp

@@ -107,6 +107,7 @@ void AckermannPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costma
 
     if( private_nh.getParam( "ackermann_topic", ackermann_topic_ ))
       odom_helper_.set_ackermann_topic( ackermann_topic_ );
+    odom_helper_.set_costmap(costmap_ros_);
 
     initialized_ = true;