From b61d0bc34ec6d742c6da81b61beb7aa97ce37ee8 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 11 Oct 2022 16:06:15 +0200
Subject: [PATCH] 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 | 3 +++
src/ackermann_odom_helper.cpp | 19 ++++++++++++++++++-
src/ackermann_planner_ros.cpp | 1 +
3 files changed, 22 insertions(+), 1 deletion(-)
diff --git a/include/ackermann_odom_helper.h b/include/ackermann_odom_helper.h
index 8f09f7c..de57bcb 100644
--- a/include/ackermann_odom_helper.h
+++ b/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_;
diff --git a/src/ackermann_odom_helper.cpp b/src/ackermann_odom_helper.cpp
index 5056a82..9276b8e 100644
--- a/src/ackermann_odom_helper.cpp
+++ b/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_;
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index ca394c7..6f8c488 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/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;
--
GitLab