address review feedback, refactor test

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

@@ -41,7 +41,6 @@ class CollisionChecker
 {
 public:
   CollisionChecker(
-    rclcpp::Node::SharedPtr ros_node,
     std::shared_ptr<CostmapSubscriber> costmap_sub,
     std::shared_ptr<FootprintSubscriber> footprint_sub,
     tf2_ros::Buffer & tf_buffer,
@@ -67,6 +66,7 @@ protected:
   std::string global_frame_;
   std::string robot_base_frame_;
 
+  std::shared_ptr<Costmap2D> costmap_;
   rclcpp::Node::SharedPtr node_;
   std::string name_;
   std::shared_ptr<CostmapSubscriber> costmap_sub_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

@@ -20,6 +20,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_util/lifecycle_node.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -28,21 +29,32 @@ class CostmapSubscriber
 {
 public:
   CostmapSubscriber(
-    rclcpp::Node::SharedPtr ros_node,
+    nav2_util::LifecycleNode::SharedPtr node,
     std::string & topic_name);
+
+  CostmapSubscriber(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    std::string & topic_name);
+
   ~CostmapSubscriber() {}
 
-  Costmap2D * getCostmap();
+  std::shared_ptr<Costmap2D> getCostmap();
 
 protected:
+  // Interfaces used for logging and creating publishers and subscribers
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+
   void toCostmap2D();
   void costmap_callback(const nav2_msgs::msg::Costmap::SharedPtr msg);
 
-  rclcpp::Node::SharedPtr node_;
-  Costmap2D * costmap_;
-  nav2_msgs::msg::Costmap::SharedPtr msg_;
+  std::shared_ptr<Costmap2D> costmap_;
+  nav2_msgs::msg::Costmap::SharedPtr costmap_msg_;
   std::string topic_name_;
-  bool costmap_received_;
+  bool costmap_received_{false};
   rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
 };
 

nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp

@@ -19,6 +19,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_util/lifecycle_node.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -27,19 +28,30 @@ class FootprintSubscriber
 {
 public:
   FootprintSubscriber(
-    rclcpp::Node::SharedPtr ros_node,
+    nav2_util::LifecycleNode::SharedPtr node,
     std::string & topic_name);
+
+  FootprintSubscriber(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    std::string & topic_name);
+
   ~FootprintSubscriber() {}
 
   bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint);
 
 protected:
+  // Interfaces used for logging and creating publishers and subscribers
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+
   void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
 
-  rclcpp::Node::SharedPtr node_;
   std::string topic_name_;
-  bool footprint_received_;
-  std::vector<geometry_msgs::msg::Point> footprint_;
+  bool footprint_received_{false};
+  geometry_msgs::msg::PolygonStamped::SharedPtr footprint_;
   rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_sub_;
 };
 

nav2_costmap_2d/src/collision_checker.cpp

@@ -22,19 +22,17 @@ namespace nav2_costmap_2d
 {
 
 CollisionChecker::CollisionChecker(
-  rclcpp::Node::SharedPtr ros_node,
   std::shared_ptr<CostmapSubscriber> costmap_sub,
   std::shared_ptr<FootprintSubscriber> footprint_sub,
   tf2_ros::Buffer & tf_buffer,
   std::string name)
 : tf_buffer_(tf_buffer),
-  node_(ros_node), name_(name),
-  costmap_sub_(costmap_sub), footprint_sub_(footprint_sub)
+  name_(name),
+  costmap_sub_(costmap_sub),
+  footprint_sub_(footprint_sub)
 {
-  node_->get_parameter_or<std::string>(
-    "global_frame", global_frame_, std::string("map"));
-  node_->get_parameter_or<std::string>(
-    "robot_base_frame", robot_base_frame_, std::string("base_link"));
+  global_frame_ = std::string("map");
+  robot_base_frame_ = std::string("base_link");
 }
 
 CollisionChecker::~CollisionChecker() {}
@@ -48,10 +46,10 @@ bool CollisionChecker::isCollisionFree(
     }
     return true;
   } catch (const IllegalPoseException & e) {
-    RCLCPP_ERROR(node_->get_logger(), "%s", e.what());
+    RCLCPP_ERROR(rclcpp::get_logger(name_), "%s", e.what());
     return false;
   } catch (const CollisionCheckerException & e) {
-    RCLCPP_ERROR(node_->get_logger(), "%s", e.what());
+    RCLCPP_ERROR(rclcpp::get_logger(name_), "%s", e.what());
     return false;
   }
 }
@@ -59,7 +57,6 @@ bool CollisionChecker::isCollisionFree(
 double CollisionChecker::scorePose(
   const geometry_msgs::msg::Pose2D & pose)
 {
-  Costmap2D * costmap_;
   try {
     costmap_ = costmap_sub_->getCostmap();
   } catch (const std::runtime_error & e) {
@@ -68,7 +65,7 @@ double CollisionChecker::scorePose(
 
   unsigned int cell_x, cell_y;
   if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
-    RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", cell_x, cell_y);
+    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", cell_x, cell_y);
     throw IllegalPoseException(name_, "Pose Goes Off Grid.");
   }
 
@@ -81,7 +78,7 @@ double CollisionChecker::scorePose(
   unorientFootprint(footprint, footprint_spec);
   transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);
 
-  // now we really have to lay down the footprint in the costmap grid
+  // now we really have to lay down the footprint in the costmap_ grid
   unsigned int x0, x1, y0, y1;
   double line_cost = 0.0;
   double footprint_cost = 0.0;
@@ -90,13 +87,13 @@ double CollisionChecker::scorePose(
   for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
     // get the cell coord of the first point
     if (!costmap_->worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
-      RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x0, y0);
+      RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x0, y0);
       throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
     }
 
     // get the cell coord of the second point
     if (!costmap_->worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
-      RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x1, y1);
+      RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x1, y1);
       throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
     }
 
@@ -107,13 +104,13 @@ double CollisionChecker::scorePose(
   // we also need to connect the first point in the footprint to the last point
   // get the cell coord of the last point
   if (!costmap_->worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
-    RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x0, y0);
+    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x0, y0);
     throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
   }
 
   // get the cell coord of the first point
   if (!costmap_->worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
-    RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x1, y1);
+    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x1, y1);
     throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
   }
 
@@ -142,15 +139,13 @@ double CollisionChecker::lineCost(int x0, int x1, int y0, int y1)
 
 double CollisionChecker::pointCost(int x, int y)
 {
-  Costmap2D * costmap_ = costmap_sub_->getCostmap();
-
   unsigned char cost = costmap_->getCost(x, y);
   // if the cell is in an obstacle the path is invalid or unknown
   if (cost == LETHAL_OBSTACLE) {
-    RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x, y);
+    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y);
     throw IllegalPoseException(name_, "Footprint Hits Obstacle.");
   } else if (cost == NO_INFORMATION) {
-    RCLCPP_DEBUG(node_->get_logger(), "Map Cell: [%d, %d]", x, y);
+    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y);
     throw IllegalPoseException(name_, "Footprint Hits Unknown Region.");
   }
 
@@ -167,20 +162,19 @@ CollisionChecker::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) co
   robot_pose.header.frame_id = robot_base_frame_;
   robot_pose.header.stamp = rclcpp::Time();
 
-  rclcpp::Time current_time = node_->now();  // save time for checking tf delay later
   // get the global pose of the robot
   try {
     tf_buffer_.transform(robot_pose, global_pose, global_frame_);
   } catch (tf2::LookupException & ex) {
-    RCLCPP_ERROR(node_->get_logger(),
+    RCLCPP_ERROR(rclcpp::get_logger(name_),
       "No Transform available Error looking up robot pose: %s\n", ex.what());
     return false;
   } catch (tf2::ConnectivityException & ex) {
-    RCLCPP_ERROR(node_->get_logger(),
+    RCLCPP_ERROR(rclcpp::get_logger(name_),
       "Connectivity Error looking up robot pose: %s\n", ex.what());
     return false;
   } catch (tf2::ExtrapolationException & ex) {
-    RCLCPP_ERROR(node_->get_logger(),
+    RCLCPP_ERROR(rclcpp::get_logger(name_),
       "Extrapolation Error looking up robot pose: %s\n", ex.what());
     return false;
   }

nav2_costmap_2d/src/costmap_2d_publisher.cpp

@@ -60,7 +60,7 @@ Costmap2DPublisher::Costmap2DPublisher(
   costmap_pub_ = ros_node->create_publisher<nav_msgs::msg::OccupancyGrid>(topic_name,
       custom_qos);
   costmap_raw_pub_ = ros_node->create_publisher<nav2_msgs::msg::Costmap>(topic_name + "_raw",
-    custom_qos);      
+      custom_qos);
   costmap_update_pub_ = ros_node->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
     topic_name + "_updates", custom_qos);
 

nav2_costmap_2d/src/costmap_subscriber.cpp

@@ -20,50 +20,63 @@ namespace nav2_costmap_2d
 {
 
 CostmapSubscriber::CostmapSubscriber(
-  rclcpp::Node::SharedPtr ros_node,
+  nav2_util::LifecycleNode::SharedPtr node,
   std::string & topic_name)
-: node_(ros_node),
-  topic_name_(topic_name),
-  costmap_received_(false),
-  costmap_(nullptr)
+: CostmapSubscriber(node->get_node_base_interface(),
+    node->get_node_topics_interface(),
+    node->get_node_logging_interface(),
+    topic_name)
+{}
+
+CostmapSubscriber::CostmapSubscriber(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  std::string & topic_name)
+: node_base_(node_base),
+  node_topics_(node_topics),
+  node_logging_(node_logging),
+  topic_name_(topic_name)
 {
-  costmap_sub_ = node_->create_subscription<nav2_msgs::msg::Costmap>(topic_name,
+  costmap_sub_ = rclcpp::create_subscription<nav2_msgs::msg::Costmap>(node_topics_, topic_name,
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
       std::bind(&CostmapSubscriber::costmap_callback, this, std::placeholders::_1));
 }
 
-Costmap2D * CostmapSubscriber::getCostmap()
+std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap()
 {
-  if (costmap_ == nullptr) {
+  if (!costmap_received_) {
     throw std::runtime_error("Costmap is not available");
   }
+  toCostmap2D();
   return costmap_;
 }
 
 void CostmapSubscriber::toCostmap2D()
 {
-  if (!costmap_received_) {
-    costmap_ = new Costmap2D(
-      msg_->metadata.size_x, msg_->metadata.size_y,
-      msg_->metadata.resolution, msg_->metadata.origin.position.x,
-      msg_->metadata.origin.position.y);
-  } else if (costmap_->getSizeInCellsX() != msg_->metadata.size_x ||
-    costmap_->getSizeInCellsY() != msg_->metadata.size_y ||
-    costmap_->getResolution() != msg_->metadata.resolution ||
-    costmap_->getOriginX() != msg_->metadata.origin.position.x ||
-    costmap_->getOriginY() != msg_->metadata.origin.position.y)
+  if (costmap_ == nullptr) {
+    costmap_ = std::make_shared<Costmap2D>(
+      costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y,
+      costmap_msg_->metadata.resolution, costmap_msg_->metadata.origin.position.x,
+      costmap_msg_->metadata.origin.position.y);
+  } else if (costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x ||
+    costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y ||
+    costmap_->getResolution() != costmap_msg_->metadata.resolution ||
+    costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x ||
+    costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y)
   {
     // Update the size of the costmap
-    costmap_->resizeMap(msg_->metadata.size_x, msg_->metadata.size_y,
-      msg_->metadata.resolution,
-      msg_->metadata.origin.position.x,
-      msg_->metadata.origin.position.y);
+    costmap_->resizeMap(costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y,
+      costmap_msg_->metadata.resolution,
+      costmap_msg_->metadata.origin.position.x,
+      costmap_msg_->metadata.origin.position.y);
   }
 
   unsigned char * master_array = costmap_->getCharMap();
   unsigned int index = 0;
-  for (unsigned int i = 0; i < msg_->metadata.size_x; ++i) {
-    for (unsigned int j = 0; j < msg_->metadata.size_y; ++j) {
-      master_array[index] = msg_->data[index];
+  for (unsigned int i = 0; i < costmap_msg_->metadata.size_x; ++i) {
+    for (unsigned int j = 0; j < costmap_msg_->metadata.size_y; ++j) {
+      master_array[index] = costmap_msg_->data[index];
       ++index;
     }
   }
@@ -71,8 +84,7 @@ void CostmapSubscriber::toCostmap2D()
 
 void CostmapSubscriber::costmap_callback(const nav2_msgs::msg::Costmap::SharedPtr msg)
 {
-  msg_ = msg;
-  toCostmap2D();
+  costmap_msg_ = msg;
   if (!costmap_received_) {
     costmap_received_ = true;
   }

nav2_costmap_2d/src/footprint_subscriber.cpp

@@ -20,11 +20,26 @@ namespace nav2_costmap_2d
 {
 
 FootprintSubscriber::FootprintSubscriber(
-  rclcpp::Node::SharedPtr ros_node,
+  nav2_util::LifecycleNode::SharedPtr node,
   std::string & topic_name)
-: node_(ros_node), topic_name_(topic_name), footprint_received_(false)
+: FootprintSubscriber(node->get_node_base_interface(),
+    node->get_node_topics_interface(),
+    node->get_node_logging_interface(),
+    topic_name)
+{}
+
+FootprintSubscriber::FootprintSubscriber(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  std::string & topic_name)
+: node_base_(node_base),
+  node_topics_(node_topics),
+  node_logging_(node_logging),
+  topic_name_(topic_name)
 {
-  footprint_sub_ = node_->create_subscription<geometry_msgs::msg::PolygonStamped>(topic_name,
+  footprint_sub_ = rclcpp::create_subscription<geometry_msgs::msg::PolygonStamped>(node_topics_,
+      topic_name, rclcpp::SystemDefaultsQoS(),
       std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1));
 }
 
@@ -34,19 +49,19 @@ FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point> & footp
   if (!footprint_received_) {
     return false;
   }
-  footprint = footprint_;
+
+  footprint = toPointVector(
+    std::make_shared<geometry_msgs::msg::Polygon>(footprint_->polygon));
   return true;
 }
 
 void
 FootprintSubscriber::footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg)
 {
+  footprint_ = msg;
   if (!footprint_received_) {
     footprint_received_ = true;
   }
-
-  footprint_ = toPointVector(
-    std::make_shared<geometry_msgs::msg::Polygon>(msg->polygon));
 }
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/test/integration/test_collision_checker.cpp

@@ -43,22 +43,53 @@ public:
 };
 RclCppFixture g_rclcppfixture;
 
+class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
+{
+public:
+  DummyCostmapSubscriber(
+    nav2_util::LifecycleNode::SharedPtr node,
+    std::string & topic_name)
+  : CostmapSubscriber(node, topic_name)
+  {}
+
+  void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
+  {
+    costmap_msg_ = msg;
+    costmap_received_ = true;
+  }
+};
+
+class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
+{
+public:
+  DummyFootprintSubscriber(
+    nav2_util::LifecycleNode::SharedPtr node,
+    std::string & topic_name)
+  : FootprintSubscriber(node, topic_name)
+  {}
+
+  void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg)
+  {
+    footprint_ = msg;
+    footprint_received_ = true;
+  }
+};
+
 class TestCollisionChecker : public nav2_util::LifecycleNode
 {
 public:
   TestCollisionChecker(std::string name)
-  : LifecycleNode(name, "", true),
-    costmap_received_(false),
+  : LifecycleNode(name, "", false),
     global_frame_("map")
   {
     // Declare non-plugin specific costmap parameters
     declare_parameter("map_topic", rclcpp::ParameterValue(std::string("/map")));
-    declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
+    declare_parameter("track_unknown_space", rclcpp::ParameterValue(true));
     declare_parameter("use_maximum", rclcpp::ParameterValue(false));
     declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
     declare_parameter("unknown_cost_value",
       rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
-    declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));    
+    declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
   }
 
   nav2_util::CallbackReturn
@@ -69,14 +100,18 @@ public:
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
     std::string costmap_topic = "costmap_raw";
-    std::string footprint_topic = "footprint";
+    std::string footprint_topic = "published_footprint";
+
+    costmap_sub_ = std::make_shared<DummyCostmapSubscriber>(
+      shared_from_this(),
+      costmap_topic);
+
+    footprint_sub_ = std::make_shared<DummyFootprintSubscriber>(
+      shared_from_this(),
+      footprint_topic);
 
-    costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
-      rclcpp_node_, costmap_topic);
-    footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
-      rclcpp_node_, footprint_topic);
     collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
-      rclcpp_node_, costmap_sub_, footprint_sub_, *tf_buffer_);
+      costmap_sub_, footprint_sub_, *tf_buffer_);
 
     base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity());
     base_rel_map.child_frame_id = "base_link";
@@ -84,20 +119,11 @@ public:
     base_rel_map.header.stamp = now();
     tf_buffer_->setTransform(base_rel_map, "collision_checker_test");
 
-
     layers_ = new nav2_costmap_2d::LayeredCostmap("frame", false, false);
     // Add Static Layer
     addStaticLayer(*layers_, *tf_buffer_, shared_from_this());
-
     // Add Inflation Layer
-    nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(
-      *layers_, *tf_buffer_, shared_from_this());
-
-    footprint_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>(
-      footprint_topic, rclcpp::SystemDefaultsQoS());
-
-    costmap_pub_ = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(shared_from_this(),
-        layers_->getCostmap(), global_frame_, "costmap", true);
+    addInflationLayer(*layers_, *tf_buffer_, shared_from_this());
 
     return nav2_util::CallbackReturn::SUCCESS;
   }
@@ -106,24 +132,6 @@ public:
   on_activate(const rclcpp_lifecycle::State & /*state*/)
   {
     RCLCPP_INFO(get_logger(), "Activating");
-
-    costmap_pub_->on_activate();
-    footprint_pub_->on_activate();
-
-    auto timer_callback = [this]() -> void
-      {
-        try {
-          costmap_sub_->getCostmap();
-          costmap_received_ = true;
-        } catch (const std::runtime_error & e) {
-          costmap_received_ = false;
-        }
-        publishFootprint();
-        publishCostmap();
-      };
-
-    timer_ = create_wall_timer(0.1s, timer_callback);
-
     return nav2_util::CallbackReturn::SUCCESS;
   }
 
@@ -131,11 +139,6 @@ public:
   on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   {
     RCLCPP_INFO(get_logger(), "Deactivating");
-
-    costmap_pub_->on_deactivate();
-    footprint_pub_->on_deactivate();
-    timer_->cancel();
-
     return nav2_util::CallbackReturn::SUCCESS;
   }
 
@@ -146,15 +149,11 @@ public:
     delete layers_;
     layers_ = nullptr;
 
-    timer_->reset();
     tf_buffer_.reset();
     tf_listener_.reset();
 
     footprint_sub_.reset();
-    footprint_pub_.reset();
-
     costmap_sub_.reset();
-    costmap_pub_.reset();
 
     return nav2_util::CallbackReturn::SUCCESS;
   }
@@ -169,12 +168,8 @@ public:
     pose.theta = theta;
 
     setPose(x, y, theta);
-    costmap_received_ = false;
-
-    while (!costmap_received_) {
-      rclcpp::spin_some(get_node_base_interface());
-    }
-
+    publishFootprint();
+    publishCostmap();
     return collision_checker_->isCollisionFree(pose);
   }
 
@@ -215,31 +210,62 @@ protected:
     oriented_footprint.header.frame_id = global_frame_;
     oriented_footprint.header.stamp = now();
     nav2_costmap_2d::transformFootprint(x_, y_, yaw_, footprint_, oriented_footprint);
-    footprint_pub_->publish(oriented_footprint);
+    footprint_sub_->setFootprint(
+      std::make_shared<geometry_msgs::msg::PolygonStamped>(oriented_footprint));
   }
 
   void publishCostmap()
   {
     layers_->updateMap(x_, y_, yaw_);
-    costmap_pub_->publishCostmap();
+    costmap_sub_->setCostmap(
+      std::make_shared<nav2_msgs::msg::Costmap>(toCostmapMsg(layers_->getCostmap())));
+  }
+
+  nav2_msgs::msg::Costmap
+  toCostmapMsg(nav2_costmap_2d::Costmap2D * costmap)
+  {
+    double resolution = costmap->getResolution();
+
+    nav2_msgs::msg::Costmap costmap_msg;
+    costmap_msg.header.frame_id = global_frame_;
+    costmap_msg.header.stamp = now();
+
+    costmap_msg.metadata.layer = "master";
+    costmap_msg.metadata.resolution = resolution;
+
+    costmap_msg.metadata.size_x = costmap->getSizeInCellsX();
+    costmap_msg.metadata.size_y = costmap->getSizeInCellsY();
+
+    double wx, wy;
+    costmap->mapToWorld(0, 0, wx, wy);
+    costmap_msg.metadata.origin.position.x = wx - resolution / 2;
+    costmap_msg.metadata.origin.position.y = wy - resolution / 2;
+    costmap_msg.metadata.origin.position.z = 0.0;
+    costmap_msg.metadata.origin.orientation.w = 1.0;
+
+    costmap_msg.data.resize(costmap_msg.metadata.size_x * costmap_msg.metadata.size_y);
+
+    unsigned char * data = costmap->getCharMap();
+    for (unsigned int i = 0; i < costmap_msg.data.size(); i++) {
+      costmap_msg.data[i] = data[i];
+    }
+
+    return costmap_msg;
   }
 
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
-  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
-  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
+  std::shared_ptr<DummyFootprintSubscriber> footprint_sub_;
   std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
+  std::string global_frame_;
+  geometry_msgs::msg::TransformStamped base_rel_map;
   double x_, y_, yaw_;
   nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
-  std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> costmap_pub_;
   std::vector<geometry_msgs::msg::Point> footprint_;
-  std::string global_frame_;
-  bool costmap_received_;
-  geometry_msgs::msg::TransformStamped base_rel_map;
-  rclcpp::TimerBase::SharedPtr timer_;
 };
 
+
 class TestNode : public ::testing::Test
 {
 public: