address review feedback

ros-navigation · Jun 27, 2019 · 6a4b217 · 6a4b217
1 parent 7e8df0d
commit 6a4b217
Show file tree

Hide file tree

Showing 7 changed files with 51 additions and 26 deletions.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
@@ -37,29 +37,29 @@ class CollisionChecker
 {
 public:
   CollisionChecker(
-    std::shared_ptr<CostmapSubscriber> costmap_sub,
-    std::shared_ptr<FootprintSubscriber> footprint_sub,
+    CostmapSubscriber & costmap_sub,
+    FootprintSubscriber & footprint_sub,
     std::string name = "collision_checker");
 
   ~CollisionChecker();
 
-  double scorePose(
-    const geometry_msgs::msg::Pose2D & pose);
-  bool isCollisionFree(
-    const geometry_msgs::msg::Pose2D & pose);
+  // Returns the obstacle footprint score for a particular pose
+  double scorePose(const geometry_msgs::msg::Pose2D & pose);
+  bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose);
 
 protected:
-  double lineCost(int x0, int x1, int y0, int y1);
-  double pointCost(int x, int y);
+  double lineCost(int x0, int x1, int y0, int y1) const;
+  double pointCost(int x, int y) const;
   bool getRobotPose(geometry_msgs::msg::Pose & current_pose);
   void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
 
   std::shared_ptr<Costmap2D> costmap_;
 
+  // Name used for logging
   std::string name_;
   nav2_util::GetRobotPoseClient get_robot_pose_client_{"collision_checker"};
-  std::shared_ptr<CostmapSubscriber> costmap_sub_;
-  std::shared_ptr<FootprintSubscriber> footprint_sub_;
+  CostmapSubscriber & costmap_sub_;
+  FootprintSubscriber & footprint_sub_;
 };
 }  // namespace nav2_costmap_2d
 

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
@@ -32,6 +32,10 @@ class CostmapSubscriber
     nav2_util::LifecycleNode::SharedPtr node,
     std::string & topic_name);
 
+  CostmapSubscriber(
+    rclcpp::Node::SharedPtr node,
+    std::string & topic_name);
+
   CostmapSubscriber(
     const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
     const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
@@ -31,6 +31,10 @@ class FootprintSubscriber
     nav2_util::LifecycleNode::SharedPtr node,
     std::string & topic_name);
 
+  FootprintSubscriber(
+    rclcpp::Node::SharedPtr node,
+    std::string & topic_name);
+
   FootprintSubscriber(
     const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
     const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,

diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "nav2_costmap_2d/collision_checker.hpp"
+
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_costmap_2d/exceptions.hpp"
 #include "nav2_costmap_2d/footprint.hpp"
@@ -24,8 +25,8 @@ namespace nav2_costmap_2d
 {
 
 CollisionChecker::CollisionChecker(
-  std::shared_ptr<CostmapSubscriber> costmap_sub,
-  std::shared_ptr<FootprintSubscriber> footprint_sub,
+  CostmapSubscriber & costmap_sub,
+  FootprintSubscriber & footprint_sub,
   std::string name)
 : name_(name),
   costmap_sub_(costmap_sub),
@@ -55,7 +56,7 @@ double CollisionChecker::scorePose(
   const geometry_msgs::msg::Pose2D & pose)
 {
   try {
-    costmap_ = costmap_sub_->getCostmap();
+    costmap_ = costmap_sub_.getCostmap();
   } catch (const std::runtime_error & e) {
     throw CollisionCheckerException(e.what());
   }
@@ -67,7 +68,7 @@ double CollisionChecker::scorePose(
   }
 
   Footprint footprint;
-  if (!footprint_sub_->getFootprint(footprint)) {
+  if (!footprint_sub_.getFootprint(footprint)) {
     throw CollisionCheckerException("Footprint not available.");
   }
 
@@ -118,7 +119,7 @@ double CollisionChecker::scorePose(
   return footprint_cost;
 }
 
-double CollisionChecker::lineCost(int x0, int x1, int y0, int y1)
+double CollisionChecker::lineCost(int x0, int x1, int y0, int y1) const
 {
   double line_cost = 0.0;
   double point_cost = -1.0;
@@ -134,7 +135,7 @@ double CollisionChecker::lineCost(int x0, int x1, int y0, int y1)
   return line_cost;
 }
 
-double CollisionChecker::pointCost(int x, int y)
+double CollisionChecker::pointCost(int x, int y) const
 {
   unsigned char cost = costmap_->getCost(x, y);
   // if the cell is in an obstacle the path is invalid or unknown
@@ -155,10 +156,10 @@ CollisionChecker::getRobotPose(geometry_msgs::msg::Pose & current_pose)
   auto request = std::make_shared<nav2_util::GetRobotPoseClient::GetRobotPoseRequest>();
 
   auto result = get_robot_pose_client_.invoke(request, 1s);
-  if (!result.get()->is_pose_valid) {
+  if (!result->is_pose_valid) {
     return false;
   }
-  current_pose = result.get()->pose.pose;
+  current_pose = result->pose.pose;
   return true;
 }
 

diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp
@@ -28,6 +28,15 @@ CostmapSubscriber::CostmapSubscriber(
     topic_name)
 {}
 
+CostmapSubscriber::CostmapSubscriber(
+  rclcpp::Node::SharedPtr node,
+  std::string & topic_name)
+: 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,

diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp
@@ -28,6 +28,15 @@ FootprintSubscriber::FootprintSubscriber(
     topic_name)
 {}
 
+FootprintSubscriber::FootprintSubscriber(
+  rclcpp::Node::SharedPtr node,
+  std::string & topic_name)
+: 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,

diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
@@ -113,7 +113,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
       footprint_topic);
 
     collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
-      costmap_sub_, footprint_sub_, get_name());
+      *costmap_sub_, *footprint_sub_, get_name());
 
     get_robot_pose_service_ = rclcpp_node_->create_service<nav2_msgs::srv::GetRobotPose>(
       "GetRobotPose", std::bind(&TestCollisionChecker::get_robot_pose_callback, this, _1, _2, _3));
@@ -218,26 +218,24 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
   {
     double resolution = costmap->getResolution();
 
+    double wx, wy;
+    costmap->mapToWorld(0, 0, wx, wy);
+
+    unsigned char * data = costmap->getCharMap();
+
     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];
     }