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 @@ class CollisionChecker
   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,18 +46,17 @@ 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;
   }
 }
 
 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,59 +20,71 @@ 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;
     }
   }
 }
 
 void CostmapSubscriber::costmap_callback(const nav2_msgs::msg::Costmap::SharedPtr msg)
 {
-  msg_ = msg;
-  toCostmap2D();
+  costmap_msg_ = msg;
   if (!costmap_received_) {
     costmap_received_ = true;
   }