From 6d2278e42dafb7c421f9c6255a3856882a9dea82 Mon Sep 17 00:00:00 2001 From: magda_sk Date: Mon, 27 Nov 2023 23:40:38 +0100 Subject: [PATCH] 3732 costmap raw update msgs rebased (fixes github issues with PR #3948) (#3965) * Adding CostmapUpdate msg * CostmapRawUpdate Publisher * remove test logs and typos * change data type to uint8 * pass unique ptr to raw costmap update publisher * adding subsriber for updates * -Werror=type-limits for update lower bounds checking * copy update msg * change code formatting * adding lock guards on costmap in footprint_collision_checker.cpp * adding lock guards for costmap subscriber clients * review suggestions implementation * fixing gtests errors * changes after second round of review * Update nav2_msgs/msg/CostmapUpdate.msg Co-authored-by: Steve Macenski * Integration test for costmap subscriber draft * Change the current grid parameters in separate method * WIP int test * adding method in PublisherCostmap2D for OccupancyGridUpdate population * test full costmap and updates while generating results * Integration tests for subscribers * Expected number of msgs related to number of mapchanges in tests * next round of reviews * refactor names of Costmap2DPublisher * remove unnecessary costmap_received_ flag form CostmapSubscriber --------- Co-authored-by: Steve Macenski --- .../src/constrained_smoother.cpp | 1 + .../test/test_constrained_smoother.cpp | 7 +- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 16 +- .../nav2_costmap_2d/costmap_subscriber.hpp | 28 ++- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 107 ++++++---- nav2_costmap_2d/src/costmap_subscriber.cpp | 129 +++++++++--- .../test/integration/CMakeLists.txt | 21 ++ .../integration/test_costmap_subscriber.cpp | 184 ++++++++++++++++++ .../test_costmap_topic_collision_checker.cpp | 7 +- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/CostmapUpdate.msg | 11 ++ nav2_smoother/src/simple_smoother.cpp | 2 + nav2_smoother/test/test_smoother_server.cpp | 7 +- 13 files changed, 442 insertions(+), 79 deletions(-) create mode 100644 nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp create mode 100644 nav2_msgs/msg/CostmapUpdate.msg diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 305f4141f3..406ca377c3 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -132,6 +132,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat // Smooth plan auto costmap = costmap_sub_->getCostmap(); + std::lock_guard lock(*(costmap->getMutex())); if (!smoother_->smooth(path_world, start_dir, end_dir, costmap.get(), smoother_params_)) { RCLCPP_WARN( logger_, diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 1ccae77f77..6687b3452d 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -82,7 +82,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 2074ad5ea6..08b09504e5 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -48,6 +48,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "map_msgs/msg/occupancy_grid_update.hpp" #include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_msgs/srv/get_costmap.hpp" #include "tf2/transform_datatypes.h" #include "nav2_util/lifecycle_node.hpp" @@ -91,6 +92,7 @@ class Costmap2DPublisher costmap_pub_->on_activate(); costmap_update_pub_->on_activate(); costmap_raw_pub_->on_activate(); + costmap_raw_update_pub_->on_activate(); } /** @@ -101,6 +103,7 @@ class Costmap2DPublisher costmap_pub_->on_deactivate(); costmap_update_pub_->on_deactivate(); costmap_raw_pub_->on_deactivate(); + costmap_raw_update_pub_->on_deactivate(); } /** @@ -136,9 +139,16 @@ class Costmap2DPublisher void prepareGrid(); void prepareCostmap(); + /** @brief Prepare OccupancyGridUpdate msg for publication. */ + std::unique_ptr createGridUpdateMsg(); + /** @brief Prepare CostmapUpdate msg for publication. */ + std::unique_ptr createCostmapUpdateMsg(); + /** @brief Publish the latest full costmap to the new subscriber. */ // void onNewSubscription(const ros::SingleSubscriberPublisher& pub); + void updateGridParams(); + /** @brief GetCostmap callback service */ void costmap_service_callback( const std::shared_ptr request_header, @@ -164,12 +174,14 @@ class Costmap2DPublisher // Publisher for raw costmap values as msg::Costmap from layered costmap rclcpp_lifecycle::LifecyclePublisher::SharedPtr costmap_raw_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + costmap_raw_update_pub_; // Service for getting the costmaps rclcpp::Service::SharedPtr costmap_service_; - float grid_resolution; - unsigned int grid_width, grid_height; + float grid_resolution_; + unsigned int grid_width_, grid_height_; std::unique_ptr grid_; std::unique_ptr costmap_raw_; // Translate from 0-255 values in costmap to -1 to 100 values in message. 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 e4ea745015..4f50614876 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_util/lifecycle_node.hpp" namespace nav2_costmap_2d @@ -52,25 +53,36 @@ class CostmapSubscriber ~CostmapSubscriber() {} /** - * @brief A Get the costmap from topic + * @brief Get current costmap */ std::shared_ptr getCostmap(); - - /** - * @brief Convert an occ grid message into a costmap object - */ - void toCostmap2D(); /** * @brief Callback for the costmap topic */ void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg); + /** + * @brief Callback for the costmap's update topic + */ + void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg); protected: + bool isCostmapReceived() {return costmap_ != nullptr;} + void processCurrentCostmapMsg(); + + bool haveCostmapParametersChanged(); + bool hasCostmapSizeChanged(); + bool hasCostmapResolutionChanged(); + bool hasCostmapOriginPositionChanged(); + + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr costmap_update_sub_; + std::shared_ptr costmap_; nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; + std::string topic_name_; - bool costmap_received_{false}; - rclcpp::Subscription::SharedPtr costmap_sub_; + std::mutex costmap_msg_mutex_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 87881bc7ca..58c7d43d56 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -76,6 +76,8 @@ Costmap2DPublisher::Costmap2DPublisher( custom_qos); costmap_update_pub_ = node->create_publisher( topic_name + "_updates", custom_qos); + costmap_raw_update_pub_ = node->create_publisher( + topic_name + "_raw_updates", custom_qos); // Create a service that will use the callback function to handle requests. costmap_service_ = node->create_service( @@ -115,32 +117,37 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub.publish(grid_); } */ + +void Costmap2DPublisher::updateGridParams() +{ + saved_origin_x_ = costmap_->getOriginX(); + saved_origin_y_ = costmap_->getOriginY(); + grid_resolution_ = costmap_->getResolution(); + grid_width_ = costmap_->getSizeInCellsX(); + grid_height_ = costmap_->getSizeInCellsY(); +} + // prepare grid_ message for publication. void Costmap2DPublisher::prepareGrid() { std::unique_lock lock(*(costmap_->getMutex())); - grid_resolution = costmap_->getResolution(); - grid_width = costmap_->getSizeInCellsX(); - grid_height = costmap_->getSizeInCellsY(); grid_ = std::make_unique(); grid_->header.frame_id = global_frame_; grid_->header.stamp = clock_->now(); - grid_->info.resolution = grid_resolution; + grid_->info.resolution = grid_resolution_; - grid_->info.width = grid_width; - grid_->info.height = grid_height; + grid_->info.width = grid_width_; + grid_->info.height = grid_height_; double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); - grid_->info.origin.position.x = wx - grid_resolution / 2; - grid_->info.origin.position.y = wy - grid_resolution / 2; + grid_->info.origin.position.x = wx - grid_resolution_ / 2; + grid_->info.origin.position.y = wy - grid_resolution_ / 2; grid_->info.origin.position.z = 0.0; grid_->info.origin.orientation.w = 1.0; - saved_origin_x_ = costmap_->getOriginX(); - saved_origin_y_ = costmap_->getOriginY(); grid_->data.resize(grid_->info.width * grid_->info.height); @@ -181,44 +188,74 @@ void Costmap2DPublisher::prepareCostmap() } } -void Costmap2DPublisher::publishCostmap() +std::unique_ptr Costmap2DPublisher::createGridUpdateMsg() +{ + auto update = std::make_unique(); + + update->header.stamp = clock_->now(); + update->header.frame_id = global_frame_; + update->x = x0_; + update->y = y0_; + update->width = xn_ - x0_; + update->height = yn_ - y0_; + update->data.resize(update->width * update->height); + + std::uint32_t i = 0; + for (std::uint32_t y = y0_; y < yn_; y++) { + for (std::uint32_t x = x0_; x < xn_; x++) { + update->data[i++] = cost_translation_table_[costmap_->getCost(x, y)]; + } + } + return update; +} + +std::unique_ptr Costmap2DPublisher::createCostmapUpdateMsg() { - if (costmap_raw_pub_->get_subscription_count() > 0) { - prepareCostmap(); - costmap_raw_pub_->publish(std::move(costmap_raw_)); + auto msg = std::make_unique(); + + msg->header.stamp = clock_->now(); + msg->header.frame_id = global_frame_; + msg->x = x0_; + msg->y = y0_; + msg->size_x = xn_ - x0_; + msg->size_y = yn_ - y0_; + msg->data.resize(msg->size_x * msg->size_y); + + std::uint32_t i = 0; + for (std::uint32_t y = y0_; y < yn_; y++) { + for (std::uint32_t x = x0_; x < xn_; x++) { + msg->data[i++] = costmap_->getCost(x, y); + } } + return msg; +} +void Costmap2DPublisher::publishCostmap() +{ float resolution = costmap_->getResolution(); - if (always_send_full_costmap_ || grid_resolution != resolution || - grid_width != costmap_->getSizeInCellsX() || - grid_height != costmap_->getSizeInCellsY() || + if (always_send_full_costmap_ || grid_resolution_ != resolution || + grid_width_ != costmap_->getSizeInCellsX() || + grid_height_ != costmap_->getSizeInCellsY() || saved_origin_x_ != costmap_->getOriginX() || saved_origin_y_ != costmap_->getOriginY()) { + updateGridParams(); if (costmap_pub_->get_subscription_count() > 0) { prepareGrid(); costmap_pub_->publish(std::move(grid_)); } + if (costmap_raw_pub_->get_subscription_count() > 0) { + prepareCostmap(); + costmap_raw_pub_->publish(std::move(costmap_raw_)); + } } else if (x0_ < xn_) { + // Publish just update msgs + std::unique_lock lock(*(costmap_->getMutex())); if (costmap_update_pub_->get_subscription_count() > 0) { - std::unique_lock lock(*(costmap_->getMutex())); - // Publish Just an Update - auto update = std::make_unique(); - update->header.stamp = rclcpp::Time(); - update->header.frame_id = global_frame_; - update->x = x0_; - update->y = y0_; - update->width = xn_ - x0_; - update->height = yn_ - y0_; - update->data.resize(update->width * update->height); - unsigned int i = 0; - for (unsigned int y = y0_; y < yn_; y++) { - for (unsigned int x = x0_; x < xn_; x++) { - unsigned char cost = costmap_->getCost(x, y); - update->data[i++] = cost_translation_table_[cost]; - } - } - costmap_update_pub_->publish(std::move(update)); + costmap_update_pub_->publish(createGridUpdateMsg()); + } + if (costmap_raw_update_pub_->get_subscription_count() > 0) { + costmap_raw_update_pub_->publish(createCostmapUpdateMsg()); } } diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index d56ac942e9..0426b87be3 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -14,22 +14,30 @@ #include #include +#include #include "nav2_costmap_2d/costmap_subscriber.hpp" namespace nav2_costmap_2d { +constexpr int costmapUpdateQueueDepth = 10; + CostmapSubscriber::CostmapSubscriber( const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name) : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); + costmap_update_sub_ = node->create_subscription( + topic_name_ + "_updates", + rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1)); } CostmapSubscriber::CostmapSubscriber( @@ -38,60 +46,119 @@ CostmapSubscriber::CostmapSubscriber( : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); + costmap_update_sub_ = node->create_subscription( + topic_name_ + "_updates", + rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1)); } std::shared_ptr CostmapSubscriber::getCostmap() { - if (!costmap_received_) { + if (!isCostmapReceived()) { throw std::runtime_error("Costmap is not available"); } - toCostmap2D(); + if (costmap_msg_) { + processCurrentCostmapMsg(); + } return costmap_; } -void CostmapSubscriber::toCostmap2D() +void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) { - auto current_costmap_msg = std::atomic_load(&costmap_msg_); - - if (costmap_ == nullptr) { - costmap_ = std::make_shared( - current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, - current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x, - current_costmap_msg->metadata.origin.position.y); - } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x || // NOLINT - costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y || - costmap_->getResolution() != current_costmap_msg->metadata.resolution || - costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x || - costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y) { - // Update the size of the costmap - costmap_->resizeMap( - current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, - current_costmap_msg->metadata.resolution, - current_costmap_msg->metadata.origin.position.x, - current_costmap_msg->metadata.origin.position.y); + std::lock_guard lock(costmap_msg_mutex_); + costmap_msg_ = msg; } + if (!isCostmapReceived()) { + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); - unsigned char * master_array = costmap_->getCharMap(); - unsigned int index = 0; - for (unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i) { - for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) { - master_array[index] = current_costmap_msg->data[index]; - ++index; + processCurrentCostmapMsg(); + } +} + +void CostmapSubscriber::costmapUpdateCallback( + const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg) +{ + if (isCostmapReceived()) { + if (costmap_msg_) { + processCurrentCostmapMsg(); + } + + std::lock_guard lock(*(costmap_->getMutex())); + + auto map_cell_size_x = costmap_->getSizeInCellsX(); + auto map_call_size_y = costmap_->getSizeInCellsY(); + + if (map_cell_size_x < update_msg->x + update_msg->size_x || + map_call_size_y < update_msg->y + update_msg->size_y) + { + RCLCPP_WARN( + logger_, "Update area outside of original map area. Costmap bounds: %d X %d, " + "Update origin: %d, %d bounds: %d X %d", map_cell_size_x, map_call_size_y, + update_msg->x, update_msg->y, update_msg->size_x, update_msg->size_y); + return; + } + unsigned char * master_array = costmap_->getCharMap(); + // copy update msg row-wise + for (size_t y = 0; y < update_msg->size_y; ++y) { + auto starting_index_of_row_update_in_costmap = (y + update_msg->y) * map_cell_size_x + + update_msg->x; + + std::copy_n( + update_msg->data.begin() + (y * update_msg->size_x), + update_msg->size_x, &master_array[starting_index_of_row_update_in_costmap]); } + } else { + RCLCPP_WARN(logger_, "No costmap received."); } } -void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) +void CostmapSubscriber::processCurrentCostmapMsg() { - std::atomic_store(&costmap_msg_, msg); - if (!costmap_received_) { - costmap_received_ = true; + std::scoped_lock lock(*(costmap_->getMutex()), costmap_msg_mutex_); + if (haveCostmapParametersChanged()) { + 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(); + std::copy(costmap_msg_->data.begin(), costmap_msg_->data.end(), master_array); + costmap_msg_.reset(); +} + +bool CostmapSubscriber::haveCostmapParametersChanged() +{ + return hasCostmapSizeChanged() || + hasCostmapResolutionChanged() || + hasCostmapOriginPositionChanged(); +} + +bool CostmapSubscriber::hasCostmapSizeChanged() +{ + return costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x || + costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y; +} + +bool CostmapSubscriber::hasCostmapResolutionChanged() +{ + return costmap_->getResolution() != costmap_msg_->metadata.resolution; +} + +bool CostmapSubscriber::hasCostmapOriginPositionChanged() +{ + return costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x || + costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y; } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 0768972a0b..868b5081a1 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -76,6 +76,17 @@ target_link_libraries(test_costmap_publisher_exec layers ) +ament_add_gtest_executable(test_costmap_subscriber_exec +test_costmap_subscriber.cpp +) +ament_target_dependencies(test_costmap_subscriber_exec + ${dependencies} +) +target_link_libraries(test_costmap_subscriber_exec + nav2_costmap_2d_core + nav2_costmap_2d_client +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -136,6 +147,16 @@ ament_add_test(test_costmap_publisher_exec TEST_EXECUTABLE=$ ) +ament_add_test(test_costmap_subscriber_exec + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp new file mode 100644 index 0000000000..65fa120cd6 --- /dev/null +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -0,0 +1,184 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; +class TestCostmapSubscriberShould : public ::testing::Test +{ +public: + TestCostmapSubscriberShould() + : topicName("/costmap"), node(nav2_util::LifecycleNode::make_shared("test_subscriber")) + { + dummyCostmapMsgSubscriber = node->create_subscription( + topicName + "_raw", 10, + std::bind(&TestCostmapSubscriberShould::costmapCallback, this, std::placeholders::_1)); + + dummyCostmapUpdateMsgSubscriber = + node->create_subscription( + topicName + "_raw_updates", 10, std::bind( + &TestCostmapSubscriberShould::costmapUpdateCallback, this, + std::placeholders::_1)); + } + + void SetUp() override + { + fullCostmapMsgCount = 0; + updateCostmapMsgCount = 0; + + costmapSubscriber = + std::make_unique(node, topicName + "_raw"); + + costmapToSend = std::make_shared(10, 10, 1.0, 0.0, 0.0); + } + + void TearDown() + { + costmapSubscriber.reset(); + costmapToSend.reset(); + } + + void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr) + { + this->fullCostmapMsgCount++; + } + void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr) + { + this->updateCostmapMsgCount++; + } + + struct CostmapObservation + { + std::uint32_t x; + std::uint32_t y; + std::uint8_t cost; + }; + + struct MapChange + { + std::vector observations; + std::uint32_t x0; + std::uint32_t xn; + std::uint32_t y0; + std::uint32_t yn; + }; + +/* *INDENT-OFF* */ + const std::vector mapChanges {{{{2, 2, 255}, {2, 3, 255}, {3, 2, 255}}, 2, 4, 2, 4}, + {{{7, 7, 255}, {7, 8, 255}}, 7, 8, 7, 9}, + {{{2, 2, 0}, {2, 3, 0}, {3, 2, 0}}, 2, 4, 2, 4}}; +/* *INDENT-ON* */ + +protected: + std::vector getCurrentCharMapFromSubscriber() + { + auto currentSubscriberCostmap = costmapSubscriber->getCostmap(); + return + std::vector( + currentSubscriberCostmap->getCharMap(), + currentSubscriberCostmap->getCharMap() + currentSubscriberCostmap->getSizeInCellsX() * + currentSubscriberCostmap->getSizeInCellsX()); + } + + std::vector getCurrentCharMapToSend() + { + return std::vector( + costmapToSend->getCharMap(), + costmapToSend->getCharMap() + costmapToSend->getSizeInCellsX() * + costmapToSend->getSizeInCellsX()); + } + + int fullCostmapMsgCount; + int updateCostmapMsgCount; + std::string topicName; + + nav2_util::LifecycleNode::SharedPtr node; + rclcpp::Logger logger {rclcpp::get_logger("test_costmap_subscriber_should")}; + + std::unique_ptr costmapSubscriber; + std::shared_ptr costmapToSend; + rclcpp::Subscription::SharedPtr dummyCostmapMsgSubscriber; + rclcpp::Subscription::SharedPtr dummyCostmapUpdateMsgSubscriber; +}; + +TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) +{ + bool always_send_full_costmap = true; + + std::vector> expectedCostmaps; + std::vector> recievedCostmaps; + + auto costmapPublisher = std::make_shared( + node, costmapToSend.get(), "", topicName, always_send_full_costmap); + costmapPublisher->on_activate(); + + for (const auto & mapChange :mapChanges) { + for (const auto & observation : mapChange.observations) { + costmapToSend->setCost(observation.x, observation.y, observation.cost); + } + + expectedCostmaps.emplace_back(getCurrentCharMapToSend()); + + costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); + costmapPublisher->publishCostmap(); + + rclcpp::spin_some(node->get_node_base_interface()); + + recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + } + + ASSERT_EQ(fullCostmapMsgCount, mapChanges.size()); + ASSERT_EQ(updateCostmapMsgCount, 0); + + ASSERT_EQ(expectedCostmaps, recievedCostmaps); + + costmapPublisher->on_deactivate(); +} + +TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) +{ + bool always_send_full_costmap = false; + + std::vector> expectedCostmaps; + std::vector> recievedCostmaps; + + auto costmapPublisher = std::make_shared( + node, costmapToSend.get(), "", topicName, always_send_full_costmap); + costmapPublisher->on_activate(); + + for (const auto & mapChange :mapChanges) { + for (const auto & observation : mapChange.observations) { + costmapToSend->setCost(observation.x, observation.y, observation.cost); + } + + expectedCostmaps.emplace_back(getCurrentCharMapToSend()); + + costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); + costmapPublisher->publishCostmap(); + + rclcpp::spin_some(node->get_node_base_interface()); + + recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + } + + ASSERT_EQ(fullCostmapMsgCount, 1); + ASSERT_EQ(updateCostmapMsgCount, mapChanges.size() - 1); + + ASSERT_EQ(expectedCostmaps, recievedCostmaps); + + costmapPublisher->on_deactivate(); +} + +TEST_F( + TestCostmapSubscriberShould, + throwExceptionIfGetCostmapMethodIsCalledBeforeAnyCostmapMsgReceived) +{ + ASSERT_ANY_THROW(costmapSubscriber->getCostmap()); +} diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 466fd015c8..ec6748f980 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -63,7 +63,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index e588ff0fa5..206ae86322 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/CollisionDetectorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" + "msg/CostmapUpdate.msg" "msg/CostmapFilterInfo.msg" "msg/SpeedLimit.msg" "msg/VoxelGrid.msg" diff --git a/nav2_msgs/msg/CostmapUpdate.msg b/nav2_msgs/msg/CostmapUpdate.msg new file mode 100644 index 0000000000..3c865d326b --- /dev/null +++ b/nav2_msgs/msg/CostmapUpdate.msg @@ -0,0 +1,11 @@ +# Update msg for Costmap containing the modified part of Costmap +std_msgs/Header header + +uint32 x +uint32 y + +uint32 size_x +uint32 size_y + +# The cost data, in row-major order, starting with (x,y) from 0-255 in Costmap format rather than OccupancyGrid 0-100. +uint8[] data diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 29f3b89a82..0b494d96c6 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -72,6 +72,8 @@ bool SimpleSmoother::smooth( std::vector path_segments = findDirectionalPathSegments(path); + std::lock_guard lock(*(costmap->getMutex())); + for (unsigned int i = 0; i != path_segments.size(); i++) { if (path_segments[i].end - path_segments[i].start > 9) { // Populate path segment diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 67472eb46b..8fb1b6ad0d 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -146,7 +146,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } };