From 6a4b21754c7546338489750729658f3f0aec698f Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Thu, 27 Jun 2019 14:37:59 -0700 Subject: [PATCH] address review feedback --- .../nav2_costmap_2d/collision_checker.hpp | 20 +++++++++---------- .../nav2_costmap_2d/costmap_subscriber.hpp | 4 ++++ .../nav2_costmap_2d/footprint_subscriber.hpp | 4 ++++ nav2_costmap_2d/src/collision_checker.cpp | 17 ++++++++-------- nav2_costmap_2d/src/costmap_subscriber.cpp | 9 +++++++++ nav2_costmap_2d/src/footprint_subscriber.cpp | 9 +++++++++ .../integration/test_collision_checker.cpp | 14 ++++++------- 7 files changed, 51 insertions(+), 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 index 7449b3a5f5..fbbd31b6b3 100644 --- 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 costmap_sub, - std::shared_ptr 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 costmap_; + // Name used for logging std::string name_; nav2_util::GetRobotPoseClient get_robot_pose_client_{"collision_checker"}; - std::shared_ptr costmap_sub_; - std::shared_ptr 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 index f94526d2cd..de5066d5f9 100644 --- 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 index 0eae01fa6b..2eee395ba5 100644 --- 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 index 0494bad43d..252868d305 100644 --- 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 costmap_sub, - std::shared_ptr 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(); 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 index 22bf83223b..1e7dcec46a 100644 --- 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 index d4463de9ba..d5f52fa871 100644 --- 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 index 46b623ccac..b9f3b45aca 100644 --- 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( - costmap_sub_, footprint_sub_, get_name()); + *costmap_sub_, *footprint_sub_, get_name()); get_robot_pose_service_ = rclcpp_node_->create_service( "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]; }