CostmapTopicCollisionChecker tf stamp fix

nav2_bringup/params/nav2_multirobot_params_1.yaml

@@ -266,7 +266,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0

nav2_bringup/params/nav2_multirobot_params_2.yaml

@@ -266,7 +266,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0

nav2_bringup/params/nav2_params.yaml

@@ -307,7 +307,7 @@ recoveries_server:
       plugin: "nav2_recoveries/Wait"
     global_frame: odom
     robot_base_frame: base_link
-    transform_timeout: 0.1
+    transform_tolerance: 0.1
     use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0

nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

@@ -29,11 +29,6 @@
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
-#include "nav2_util/robot_utils.hpp"
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wpedantic"
-#include "tf2/utils.h"
-#pragma GCC diagnostic pop
 
 namespace nav2_costmap_2d
 {
@@ -51,11 +46,8 @@ class CostmapTopicCollisionChecker
   CostmapTopicCollisionChecker(
     CostmapSubscriber & costmap_sub,
     FootprintSubscriber & footprint_sub,
-    tf2_ros::Buffer & tf,
-    std::string name = "collision_checker",
-    std::string global_frame = "map",
-    std::string robot_base_frame = "base_link",
-    double transform_tolerance = 0.1);
+    std::string name = "collision_checker");
+
   /**
    * @brief A destructor
    */
@@ -71,24 +63,17 @@ class CostmapTopicCollisionChecker
   bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose, bool updateCostmap = true);
 
 protected:
-  /**
-   * @brief Set a new footprint
-   */
-  void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
   /**
    * @brief Get a footprint at a set pose
    */
   Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose);
 
   // Name used for logging
   std::string name_;
-  std::string global_frame_;
-  std::string robot_base_frame_;
-  tf2_ros::Buffer & tf_;
   CostmapSubscriber & costmap_sub_;
   FootprintSubscriber & footprint_sub_;
-  double transform_tolerance_;
   FootprintCollisionChecker<std::shared_ptr<Costmap2D>> collision_checker_;
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp

@@ -21,6 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/footprint.hpp"
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -38,50 +39,58 @@ class FootprintSubscriber
   FootprintSubscriber(
     const nav2_util::LifecycleNode::WeakPtr & parent,
     const std::string & topic_name,
-    const double & footprint_timeout);
+    tf2_ros::Buffer & tf,
+    std::string robot_base_frame = "base_link",
+    double transform_tolerance = 0.1);
 
   /**
    * @brief A constructor
    */
   FootprintSubscriber(
     const rclcpp::Node::WeakPtr & parent,
     const std::string & topic_name,
-    const double & footprint_timeout);
+    tf2_ros::Buffer & tf,
+    std::string robot_base_frame = "base_link",
+    double transform_tolerance = 0.1);
 
   /**
    * @brief A destructor
    */
   ~FootprintSubscriber() {}
 
   /**
-   * @brief Returns an oriented robot footprint at current time.
+   * @brief Returns the latest robot footprint, in the form as received from topic (oriented).
+   *
+   * @param footprint Output param. Latest received footprint
+   * @param footprint_header Output param. Header associated with the footprint
+   * @return False if no footprint has been received
    */
-  bool getFootprint(
+  bool getFootprintRaw(
     std::vector<geometry_msgs::msg::Point> & footprint,
-    rclcpp::Duration & valid_footprint_timeout);
-  /**
-   * @brief Returns an oriented robot footprint without timeout
-   */
-  bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint);
+    std_msgs::msg::Header & footprint_header);
 
   /**
-   * @brief Returns an oriented robot footprint at stamped time.
+   * @brief Returns the latest robot footprint, transformed into robot base frame (unoriented).
+   *
+   * @param footprint Output param. Latest received footprint, unoriented
+   * @param footprint_header Output param. Header associated with the footprint
+   * @return False if no footprint has been received or if transformation failed
    */
-  bool getFootprint(
+  bool getFootprintInRobotFrame(
     std::vector<geometry_msgs::msg::Point> & footprint,
-    rclcpp::Time & stamp, rclcpp::Duration valid_footprint_timeout);
+    std_msgs::msg::Header & footprint_header);
 
 protected:
-  rclcpp::Clock::SharedPtr clock_;
-
   /**
    * @brief Callback to process new footprint updates.
    */
   void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
 
   std::string topic_name_;
+  tf2_ros::Buffer & tf_;
+  std::string robot_base_frame_;
+  double transform_tolerance_;
   bool footprint_received_{false};
-  rclcpp::Duration footprint_timeout_;
   geometry_msgs::msg::PolygonStamped::SharedPtr footprint_;
   rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_sub_;
 };

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -466,8 +466,7 @@ Costmap2DROS::updateMap()
       layered_costmap_->updateMap(x, y, yaw);
 
       auto footprint = std::make_unique<geometry_msgs::msg::PolygonStamped>();
-      footprint->header.frame_id = global_frame_;
-      footprint->header.stamp = now();
+      footprint->header = pose.header;
       transformFootprint(x, y, yaw, padded_footprint_, *footprint);
 
       RCLCPP_DEBUG(get_logger(), "Publishing footprint");

nav2_costmap_2d/src/costmap_topic_collision_checker.cpp

@@ -35,21 +35,12 @@ namespace nav2_costmap_2d
 CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   CostmapSubscriber & costmap_sub,
   FootprintSubscriber & footprint_sub,
-  tf2_ros::Buffer & tf,
-  std::string name,
-  std::string global_frame,
-  std::string robot_base_frame,
-  double transform_tolerance)
+  std::string name)
 : name_(name),
-  global_frame_(global_frame),
-  robot_base_frame_(robot_base_frame),
-  tf_(tf),
   costmap_sub_(costmap_sub),
   footprint_sub_(footprint_sub),
-  transform_tolerance_(transform_tolerance),
   collision_checker_(nullptr)
-{
-}
+{}
 
 bool CostmapTopicCollisionChecker::isCollisionFree(
   const geometry_msgs::msg::Pose2D & pose,
@@ -96,37 +87,13 @@ double CostmapTopicCollisionChecker::scorePose(
 Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose)
 {
   Footprint footprint;
-  if (!footprint_sub_.getFootprint(footprint)) {
+  std_msgs::msg::Header header;
+  if (!footprint_sub_.getFootprintInRobotFrame(footprint, header)) {
     throw CollisionCheckerException("Current footprint not available.");
   }
-
-  Footprint footprint_spec;
-  unorientFootprint(footprint, footprint_spec);
-  transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);
+  transformFootprint(pose.x, pose.y, pose.theta, footprint, footprint);
 
   return footprint;
 }
 
-void CostmapTopicCollisionChecker::unorientFootprint(
-  const std::vector<geometry_msgs::msg::Point> & oriented_footprint,
-  std::vector<geometry_msgs::msg::Point> & reset_footprint)
-{
-  geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(
-      current_pose, tf_, global_frame_, robot_base_frame_,
-      transform_tolerance_))
-  {
-    throw CollisionCheckerException("Robot pose unavailable.");
-  }
-
-  double x = current_pose.pose.position.x;
-  double y = current_pose.pose.position.y;
-  double theta = tf2::getYaw(current_pose.pose.orientation);
-
-  Footprint temp;
-  transformFootprint(-x, -y, 0, oriented_footprint, temp);
-  transformFootprint(0, 0, -theta, temp, reset_footprint);
-}
-
-
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/src/footprint.cpp

@@ -112,14 +112,15 @@ void transformFootprint(
   std::vector<geometry_msgs::msg::Point> & oriented_footprint)
 {
   // build the oriented footprint at a given location
-  oriented_footprint.clear();
+  oriented_footprint.resize(footprint_spec.size());
   double cos_th = cos(theta);
   double sin_th = sin(theta);
   for (unsigned int i = 0; i < footprint_spec.size(); ++i) {
-    geometry_msgs::msg::Point new_pt;
-    new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
-    new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
-    oriented_footprint.push_back(new_pt);
+    double new_x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
+    double new_y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
+    geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
+    new_pt.x = new_x;
+    new_pt.y = new_y;
   }
 }
 

nav2_costmap_2d/src/footprint_subscriber.cpp

@@ -17,19 +17,26 @@
 #include <memory>
 
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
 
 namespace nav2_costmap_2d
 {
 
 FootprintSubscriber::FootprintSubscriber(
   const nav2_util::LifecycleNode::WeakPtr & parent,
   const std::string & topic_name,
-  const double & footprint_timeout)
+  tf2_ros::Buffer & tf,
+  std::string robot_base_frame,
+  double transform_tolerance)
 : topic_name_(topic_name),
-  footprint_timeout_(rclcpp::Duration::from_seconds(footprint_timeout))
+  tf_(tf),
+  robot_base_frame_(robot_base_frame),
+  transform_tolerance_(transform_tolerance)
 {
   auto node = parent.lock();
-  clock_ = node->get_clock();
   footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
     topic_name, rclcpp::SystemDefaultsQoS(),
     std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
@@ -38,22 +45,24 @@ FootprintSubscriber::FootprintSubscriber(
 FootprintSubscriber::FootprintSubscriber(
   const rclcpp::Node::WeakPtr & parent,
   const std::string & topic_name,
-  const double & footprint_timeout)
+  tf2_ros::Buffer & tf,
+  std::string robot_base_frame,
+  double transform_tolerance)
 : topic_name_(topic_name),
-  footprint_timeout_(rclcpp::Duration::from_seconds(footprint_timeout))
+  tf_(tf),
+  robot_base_frame_(robot_base_frame),
+  transform_tolerance_(transform_tolerance)
 {
   auto node = parent.lock();
-  clock_ = node->get_clock();
   footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
     topic_name, rclcpp::SystemDefaultsQoS(),
     std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
 }
 
 bool
-FootprintSubscriber::getFootprint(
+FootprintSubscriber::getFootprintRaw(
   std::vector<geometry_msgs::msg::Point> & footprint,
-  rclcpp::Time & stamp,
-  rclcpp::Duration valid_footprint_timeout)
+  std_msgs::msg::Header & footprint_header)
 {
   if (!footprint_received_) {
     return false;
@@ -62,29 +71,40 @@ FootprintSubscriber::getFootprint(
   auto current_footprint = std::atomic_load(&footprint_);
   footprint = toPointVector(
     std::make_shared<geometry_msgs::msg::Polygon>(current_footprint->polygon));
-  auto & footprint_stamp = current_footprint->header.stamp;
-
-  if (stamp - footprint_stamp > valid_footprint_timeout) {
-    return false;
-  }
+  footprint_header = current_footprint->header;
 
   return true;
 }
 
 bool
-FootprintSubscriber::getFootprint(
+FootprintSubscriber::getFootprintInRobotFrame(
   std::vector<geometry_msgs::msg::Point> & footprint,
-  rclcpp::Duration & valid_footprint_timeout)
+  std_msgs::msg::Header & footprint_header)
 {
-  rclcpp::Time t = clock_->now();
-  return getFootprint(footprint, t, valid_footprint_timeout);
-}
+  if (!getFootprintRaw(footprint, footprint_header)) {
+    return false;
+  }
 
-bool
-FootprintSubscriber::getFootprint(
-  std::vector<geometry_msgs::msg::Point> & footprint)
-{
-  return getFootprint(footprint, footprint_timeout_);
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, tf_, footprint_header.frame_id, robot_base_frame_,
+      transform_tolerance_, footprint_header.stamp))
+  {
+    return false;
+  }
+
+  double x = current_pose.pose.position.x;
+  double y = current_pose.pose.position.y;
+  double theta = tf2::getYaw(current_pose.pose.orientation);
+
+  std::vector<geometry_msgs::msg::Point> temp;
+  transformFootprint(-x, -y, 0, footprint, temp);
+  transformFootprint(0, 0, -theta, temp, footprint);
+
+  footprint_header.frame_id = robot_base_frame_;
+  footprint_header.stamp = current_pose.header.stamp;
+
+  return true;
 }
 
 void