From b4c665b819540a4d6578d00f469d52479bbc3105 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Thu, 20 Jun 2019 18:45:42 -0700 Subject: [PATCH] address review feedback, refactor test --- .../nav2_costmap_2d/collision_checker.hpp | 2 +- .../nav2_costmap_2d/costmap_subscriber.hpp | 24 ++- .../nav2_costmap_2d/footprint_subscriber.hpp | 20 ++- nav2_costmap_2d/src/collision_checker.cpp | 42 +++-- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- nav2_costmap_2d/src/costmap_subscriber.cpp | 66 ++++---- nav2_costmap_2d/src/footprint_subscriber.cpp | 29 +++- .../integration/test_collision_checker.cpp | 152 ++++++++++-------- 8 files changed, 204 insertions(+), 133 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 39b236fa47f..93b37333774 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp +++ b/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 costmap_sub, std::shared_ptr 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 costmap_; rclcpp::Node::SharedPtr node_; std::string name_; std::shared_ptr costmap_sub_; 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 77203c5934f..f94526d2cdf 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/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 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 costmap_; + nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; std::string topic_name_; - bool costmap_received_; + bool costmap_received_{false}; rclcpp::Subscription::SharedPtr costmap_sub_; }; 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 e4f6e2bc97d..0eae01fa6bb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/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 & 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 footprint_; + bool footprint_received_{false}; + geometry_msgs::msg::PolygonStamped::SharedPtr footprint_; rclcpp::Subscription::SharedPtr footprint_sub_; }; diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp index 2566d3e45a7..3bf0206a1f6 100644 --- a/nav2_costmap_2d/src/collision_checker.cpp +++ b/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 costmap_sub, std::shared_ptr 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( - "global_frame", global_frame_, std::string("map")); - node_->get_parameter_or( - "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; } diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 475b9043efc..fc9490886d7 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -60,7 +60,7 @@ Costmap2DPublisher::Costmap2DPublisher( costmap_pub_ = ros_node->create_publisher(topic_name, custom_qos); costmap_raw_pub_ = ros_node->create_publisher(topic_name + "_raw", - custom_qos); + custom_qos); costmap_update_pub_ = ros_node->create_publisher( topic_name + "_updates", custom_qos); diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index f03bf8434db..3c8cb40fdc6 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/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(topic_name, + costmap_sub_ = rclcpp::create_subscription(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 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( + 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; } diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp index 3f75cec12b0..d4463de9baf 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/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(topic_name, + footprint_sub_ = rclcpp::create_subscription(node_topics_, + topic_name, rclcpp::SystemDefaultsQoS(), std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1)); } @@ -34,19 +49,19 @@ FootprintSubscriber::getFootprint(std::vector & footp if (!footprint_received_) { return false; } - footprint = footprint_; + + footprint = toPointVector( + std::make_shared(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(msg->polygon)); } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_collision_checker.cpp index 382a7a9b1f8..8d824786007 100644 --- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_collision_checker.cpp @@ -43,22 +43,53 @@ class RclCppFixture }; 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(0xff))); - declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); } nav2_util::CallbackReturn @@ -69,14 +100,18 @@ class TestCollisionChecker : public nav2_util::LifecycleNode tf_listener_ = std::make_shared(*tf_buffer_); std::string costmap_topic = "costmap_raw"; - std::string footprint_topic = "footprint"; + std::string footprint_topic = "published_footprint"; + + costmap_sub_ = std::make_shared( + shared_from_this(), + costmap_topic); + + footprint_sub_ = std::make_shared( + shared_from_this(), + footprint_topic); - costmap_sub_ = std::make_shared( - rclcpp_node_, costmap_topic); - footprint_sub_ = std::make_shared( - rclcpp_node_, footprint_topic); collision_checker_ = std::make_unique( - 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 @@ class TestCollisionChecker : public nav2_util::LifecycleNode 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( - footprint_topic, rclcpp::SystemDefaultsQoS()); - - costmap_pub_ = std::make_unique(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 @@ class TestCollisionChecker : public nav2_util::LifecycleNode 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 @@ class TestCollisionChecker : public nav2_util::LifecycleNode 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 @@ class TestCollisionChecker : public nav2_util::LifecycleNode 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 @@ class TestCollisionChecker : public nav2_util::LifecycleNode 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 @@ class TestCollisionChecker : public nav2_util::LifecycleNode 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(oriented_footprint)); } void publishCostmap() { layers_->updateMap(x_, y_, yaw_); - costmap_pub_->publishCostmap(); + costmap_sub_->setCostmap( + std::make_shared(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 tf_buffer_; std::shared_ptr tf_listener_; - std::shared_ptr costmap_sub_; - std::shared_ptr footprint_sub_; + std::shared_ptr costmap_sub_; + std::shared_ptr footprint_sub_; std::unique_ptr 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::SharedPtr footprint_pub_; - std::unique_ptr costmap_pub_; std::vector 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: