CostmapTopicCollisionChecker tf stamp fix

nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

@@ -49,6 +49,7 @@ class CostmapTopicCollisionChecker
    * @brief A constructor
    */
   CostmapTopicCollisionChecker(
+    const nav2_util::LifecycleNode::WeakPtr & parent,
     CostmapSubscriber & costmap_sub,
     FootprintSubscriber & footprint_sub,
     tf2_ros::Buffer & tf,
@@ -74,7 +75,10 @@ class CostmapTopicCollisionChecker
   /**
    * @brief Set a new footprint
    */
-  void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
+  void unorientFootprint(
+    const Footprint & oriented_footprint, Footprint & reset_footprint,
+    const rclcpp::Time & stamp);
+
   /**
    * @brief Get a footprint at a set pose
    */
@@ -89,6 +93,7 @@ class CostmapTopicCollisionChecker
   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

@@ -60,10 +60,15 @@ class FootprintSubscriber
     std::vector<geometry_msgs::msg::Point> & footprint,
     rclcpp::Duration & valid_footprint_timeout);
   /**
-   * @brief Returns an oriented robot footprint without timeout
+   * @brief Returns an oriented robot footprint using timeout specified in constructor
    */
   bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint);
 
+  /**
+   * @brief Returns an oriented robot footprint and stamp using timeout specified in constructor
+   */
+  bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint, rclcpp::Time & stamp);
+
   /**
    * @brief Returns an oriented robot footprint at stamped time.
    */

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

@@ -33,6 +33,7 @@ namespace nav2_costmap_2d
 {
 
 CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
+  const nav2_util::LifecycleNode::WeakPtr & parent,
   CostmapSubscriber & costmap_sub,
   FootprintSubscriber & footprint_sub,
   tf2_ros::Buffer & tf,
@@ -49,6 +50,8 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   transform_tolerance_(transform_tolerance),
   collision_checker_(nullptr)
 {
+  auto node = parent.lock();
+  clock_ = node->get_clock();
 }
 
 bool CostmapTopicCollisionChecker::isCollisionFree(
@@ -92,25 +95,27 @@ double CostmapTopicCollisionChecker::scorePose(
 Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose)
 {
   Footprint footprint;
-  if (!footprint_sub_.getFootprint(footprint)) {
+  rclcpp::Time stamp(0L, clock_->get_clock_type());
+  if (!footprint_sub_.getFootprint(footprint, stamp)) {
     throw CollisionCheckerException("Current footprint not available.");
   }
 
   Footprint footprint_spec;
-  unorientFootprint(footprint, footprint_spec);
+  unorientFootprint(footprint, footprint_spec, stamp);
   transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);
 
   return footprint;
 }
 
 void CostmapTopicCollisionChecker::unorientFootprint(
   const std::vector<geometry_msgs::msg::Point> & oriented_footprint,
-  std::vector<geometry_msgs::msg::Point> & reset_footprint)
+  std::vector<geometry_msgs::msg::Point> & reset_footprint,
+  const rclcpp::Time & stamp)
 {
   geometry_msgs::msg::PoseStamped current_pose;
   if (!nav2_util::getCurrentPose(
       current_pose, tf_, global_frame_, robot_base_frame_,
-      transform_tolerance_))
+      transform_tolerance_, stamp))
   {
     throw CollisionCheckerException("Robot pose unavailable.");
   }

nav2_costmap_2d/src/footprint_subscriber.cpp

@@ -68,9 +68,19 @@ FootprintSubscriber::getFootprint(
     return false;
   }
 
+  stamp = footprint_stamp;
+
   return true;
 }
 
+bool
+FootprintSubscriber::getFootprint(
+  std::vector<geometry_msgs::msg::Point> & footprint,
+  rclcpp::Time & stamp)
+{
+  return getFootprint(footprint, stamp, footprint_timeout_);
+}
+
 bool
 FootprintSubscriber::getFootprint(
   std::vector<geometry_msgs::msg::Point> & footprint,

nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp

@@ -125,7 +125,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
       footprint_topic);
 
     collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-      *costmap_sub_, *footprint_sub_, *tf_buffer_, get_name(), "map");
+      shared_from_this(), *costmap_sub_, *footprint_sub_, *tf_buffer_, get_name(), "map");
 
     layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false);
     // Add Static Layer

nav2_recoveries/src/recovery_server.cpp

@@ -53,6 +53,9 @@ RecoveryServer::RecoveryServer(const rclcpp::NodeOptions & options)
   declare_parameter(
     "transform_tolerance",
     rclcpp::ParameterValue(0.1));
+  declare_parameter(
+    "footprint_tolerance",
+    rclcpp::ParameterValue(1.0));
 }
 
 
@@ -74,19 +77,21 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
 
   std::string costmap_topic, footprint_topic;
+  double footprint_tolerance;
   this->get_parameter("costmap_topic", costmap_topic);
   this->get_parameter("footprint_topic", footprint_topic);
   this->get_parameter("transform_tolerance", transform_tolerance_);
+  this->get_parameter("footprint_tolerance", footprint_tolerance);
   costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
     shared_from_this(), costmap_topic);
   footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
-    shared_from_this(), footprint_topic, 1.0);
+    shared_from_this(), footprint_topic, footprint_tolerance);
 
   std::string global_frame, robot_base_frame;
   get_parameter("global_frame", global_frame);
   get_parameter("robot_base_frame", robot_base_frame);
   collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(),
+    shared_from_this(), *costmap_sub_, *footprint_sub_, *tf_, this->get_name(),
     global_frame, robot_base_frame, transform_tolerance_);
 
   recovery_types_.resize(recovery_ids_.size());

nav2_recoveries/test/test_recoveries.cpp

@@ -128,7 +128,7 @@ class RecoveryTest : public ::testing::Test
       node_lifecycle_, footprint_topic, 1.0);
     std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_ =
       std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-      *costmap_sub_, *footprint_sub_, *tf_buffer_,
+      node_lifecycle_, *costmap_sub_, *footprint_sub_, *tf_buffer_,
       node_lifecycle_->get_name(), "odom");
 
     recovery_ = std::make_shared<DummyRecovery>();

nav2_util/include/nav2_util/robot_utils.hpp

@@ -42,7 +42,8 @@ namespace nav2_util
 bool getCurrentPose(
   geometry_msgs::msg::PoseStamped & global_pose,
   tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map",
-  const std::string robot_frame = "base_link", const double transform_timeout = 0.1);
+  const std::string robot_frame = "base_link", const double transform_timeout = 0.1,
+  const rclcpp::Time stamp = rclcpp::Time());
 
 /**
 * @brief get an arbitrary pose in a target frame

nav2_util/src/robot_utils.cpp

@@ -26,11 +26,12 @@ namespace nav2_util
 bool getCurrentPose(
   geometry_msgs::msg::PoseStamped & global_pose,
   tf2_ros::Buffer & tf_buffer, const std::string global_frame,
-  const std::string robot_frame, const double transform_timeout)
+  const std::string robot_frame, const double transform_timeout,
+  const rclcpp::Time stamp)
 {
   tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
   global_pose.header.frame_id = robot_frame;
-  global_pose.header.stamp = rclcpp::Time();
+  global_pose.header.stamp = stamp;
 
   return transformPoseInTargetFrame(
     global_pose, global_pose, tf_buffer, global_frame, transform_timeout);