diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 2124a12a44..46a71c8e76 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -320,7 +320,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_{nullptr}; + std::unique_ptr costmap_publisher_; + + std::vector> layer_publishers_; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 906a9f0324..87881bc7ca 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -187,8 +187,8 @@ void Costmap2DPublisher::publishCostmap() prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); } - float resolution = costmap_->getResolution(); + float resolution = costmap_->getResolution(); if (always_send_full_costmap_ || grid_resolution != resolution || grid_width != costmap_->getSizeInCellsX() || grid_height != costmap_->getSizeInCellsY() || diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 52b538b6bb..aa8983c153 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -210,6 +210,20 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); + auto layers = layered_costmap_->getPlugins(); + + for (auto & layer : *layers) { + auto costmap_layer = std::dynamic_pointer_cast(layer); + if (costmap_layer != nullptr) { + layer_publishers_.emplace_back( + std::make_unique( + shared_from_this(), + costmap_layer.get(), global_frame_, + layer->getName(), always_send_full_costmap_) + ); + } + } + // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -233,8 +247,12 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_publisher_->on_activate(); footprint_pub_->on_activate(); + costmap_publisher_->on_activate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_activate(); + } // First, make sure that the transform between the robot base frame // and the global frame is available @@ -280,8 +298,13 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); dyn_params_handler.reset(); - costmap_publisher_->on_deactivate(); + footprint_pub_->on_deactivate(); + costmap_publisher_->on_deactivate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_deactivate(); + } stop(); @@ -297,6 +320,13 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + costmap_publisher_.reset(); + clear_costmap_service_.reset(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub.reset(); + } + layered_costmap_.reset(); tf_listener_.reset(); @@ -305,8 +335,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - costmap_publisher_.reset(); - clear_costmap_service_.reset(); executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; @@ -448,12 +476,22 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); + for (auto & layer_pub: layer_publishers_) { + layer_pub->updateBounds(x0, xn, y0, yn); + } + auto current_time = now(); if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + (current_time < + last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); + + for (auto & layer_pub: layer_publishers_) { + layer_pub->publishCostmap(); + } + last_publish_ = current_time; } } diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index ef0b148440..0768972a0b 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -64,6 +64,18 @@ target_link_libraries(dyn_params_tests nav2_costmap_2d_core ) +ament_add_gtest_executable(test_costmap_publisher_exec + test_costmap_2d_publisher.cpp +) +ament_target_dependencies(test_costmap_publisher_exec + ${dependencies} +) +target_link_libraries(test_costmap_publisher_exec + nav2_costmap_2d_core + nav2_costmap_2d_client + layers +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -114,6 +126,16 @@ ament_add_test(range_tests TEST_EXECUTABLE=$ ) +ament_add_test(test_costmap_publisher_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/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 5e8c19a485..706290d826 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -30,6 +30,20 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') + map_to_odom = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'] + ) + + odom_to_base_link = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'] + ) + lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -39,6 +53,8 @@ def main(argv=sys.argv[1:]): ld = LaunchDescription([ IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + map_to_odom, + odom_to_base_link, lifecycle_manager ]) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp new file mode 100644 index 0000000000..1e678d67c7 --- /dev/null +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -0,0 +1,178 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include + +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "tf2_ros/transform_listener.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class CostmapRosLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit CostmapRosLifecycleNode(const std::string & name) + : LifecycleNode(name), + name_(name) {} + + ~CostmapRosLifecycleNode() override = default; + + nav2_util::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) override + { + costmap_ros_ = std::make_shared( + name_, + std::string{get_namespace()}, + name_, + get_parameter("use_sim_time").as_bool()); + costmap_thread_ = std::make_unique(costmap_ros_); + + costmap_ros_->configure(); + + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) override + { + costmap_ros_->activate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) override + { + costmap_ros_->deactivate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) override + { + costmap_thread_.reset(); + costmap_ros_->deactivate(); + return nav2_util::CallbackReturn::SUCCESS; + } + +protected: + std::shared_ptr costmap_ros_; + std::unique_ptr costmap_thread_; + const std::string name_; +}; + +class LayerSubscriber +{ +public: + explicit LayerSubscriber(const nav2_util::LifecycleNode::WeakPtr & parent) + { + auto node = parent.lock(); + + callback_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + + std::string topic_name = "/fake_costmap/static_layer_raw"; + layer_sub_ = node->create_subscription( + topic_name, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1), + sub_option); + + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, node->get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); + } + + ~LayerSubscriber() + { + executor_thread_.reset(); + } + + std::promise layer_promise_; + +protected: + void layerCallback(const nav2_msgs::msg::Costmap::SharedPtr layer) + { + if (!callback_hit_) { + layer_promise_.set_value(layer); + callback_hit_ = true; + } + } + + rclcpp::Subscription::SharedPtr layer_sub_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + bool callback_hit_{false}; +}; + +class CostmapRosTestFixture : public ::testing::Test +{ +public: + CostmapRosTestFixture() + { + costmap_lifecycle_node_ = std::make_shared("fake_costmap"); + layer_subscriber_ = std::make_shared( + costmap_lifecycle_node_->shared_from_this()); + costmap_lifecycle_node_->on_configure(costmap_lifecycle_node_->get_current_state()); + costmap_lifecycle_node_->on_activate(costmap_lifecycle_node_->get_current_state()); + } + + ~CostmapRosTestFixture() override + { + costmap_lifecycle_node_->on_deactivate(costmap_lifecycle_node_->get_current_state()); + costmap_lifecycle_node_->on_cleanup(costmap_lifecycle_node_->get_current_state()); + } + +protected: + std::shared_ptr costmap_lifecycle_node_; + std::shared_ptr layer_subscriber_; +}; + +TEST_F(CostmapRosTestFixture, costmap_pub_test) +{ + auto future = layer_subscriber_->layer_promise_.get_future(); + auto status = future.wait_for(std::chrono::seconds(5)); + EXPECT_TRUE(status == std::future_status::ready); + + auto costmap_raw = future.get(); + + // Check first 20 cells of the 10by10 map + unsigned int i = 0; + for (; i < 7; ++i) { + EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + } + for (; i < 10; ++i) { + EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); + } + for (; i < 17; ++i) { + EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + } + for (; i < 20; ++i) { + EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); + } +} diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index 3079e3a3fc..aeaff1ac44 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -26,6 +26,7 @@ def main(argv=sys.argv[1:]): rclpy.init() + time.sleep(5) navigator = BasicNavigator()