Add ability to publish layers of the costmap

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -319,7 +319,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode
  // Publishers and subscribers
  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
  footprint_pub_;
- std::unique_ptr<Costmap2DPublisher> costmap_publisher_{nullptr};
+ std::unique_ptr<Costmap2DPublisher> costmap_publisher_;
+
+ std::vector<std::unique_ptr<Costmap2DPublisher>> layer_publishers_;
 
  rclcpp::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;
  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_sub_;

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() ||

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<CostmapLayer>(layer);
+ if (costmap_layer != nullptr) {
+ layer_publishers_.emplace_back(
+ std::make_unique<Costmap2DPublisher>(
+ 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;
@@ -446,12 +474,21 @@ 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
  {
  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;
  }
  }

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=$<TARGET_FILE:range_tests_exec>
 )
 
+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=$<TARGET_FILE:test_costmap_publisher_exec>
+)
+
 ## 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

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
  ])
 

nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp

@@ -0,0 +1,151 @@
+// 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 <gtest/gtest.h>
+
+#include <future>
+
+#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 TestCostmap2dPublisher : public nav2_util::LifecycleNode
+{
+public:
+ explicit TestCostmap2dPublisher(const std::string & name)
+ : LifecycleNode(name)
+ {
+ RCLCPP_INFO(get_logger(), "Constructing");
+ }
+
+ ~TestCostmap2dPublisher() override = default;
+
+ nav2_util::CallbackReturn
+ on_configure(const rclcpp_lifecycle::State &) override
+ {
+ RCLCPP_INFO(get_logger(), "Configuring");
+
+ callback_group_ = create_callback_group(
+ rclcpp::CallbackGroupType::MutuallyExclusive, false);
+
+ rclcpp::SubscriptionOptions sub_option;
+ sub_option.callback_group = callback_group_;
+
+ std::string topic_name = "/dummy_costmap/static_layer_raw";
+ costmap_sub_ = this->create_subscription<nav2_msgs::msg::Costmap>(
+ topic_name,
+ rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+ std::bind(&TestCostmap2dPublisher::costmapCallback, this, std::placeholders::_1),
+ sub_option);
+
+ executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+ executor_->add_callback_group(callback_group_, get_node_base_interface());
+ executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
+
+ costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+ "dummy_costmap",
+ std::string{get_namespace()},
+ "dummy_costmap",
+ get_parameter("use_sim_time").as_bool());
+ costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
+
+ costmap_ros_->configure();
+
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ nav2_util::CallbackReturn
+ on_activate(const rclcpp_lifecycle::State &) override
+ {
+ RCLCPP_INFO(get_logger(), "Activating");
+ costmap_ros_->activate();
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ nav2_util::CallbackReturn
+ on_deactivate(const rclcpp_lifecycle::State &) override
+ {
+ RCLCPP_INFO(get_logger(), "Deactivating");
+ costmap_ros_->deactivate();
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ nav2_util::CallbackReturn
+ on_cleanup(const rclcpp_lifecycle::State &) override
+ {
+ executor_thread_.reset();
+ costmap_thread_.reset();
+ costmap_ros_->deactivate();
+ return nav2_util::CallbackReturn::SUCCESS;
+ }
+
+ void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr costmap)
+ {
+ promise_.set_value(costmap);
+ }
+
+ rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
+
+ rclcpp::CallbackGroup::SharedPtr callback_group_;
+ rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+ std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+ std::promise<nav2_msgs::msg::Costmap::SharedPtr> promise_;
+
+ std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+ std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
+};
+
+class TestNode : public ::testing::Test
+{
+public:
+ TestNode()
+ {
+ costmap_publisher_ = std::make_shared<TestCostmap2dPublisher>("test_costmap_publisher");
+ costmap_publisher_->on_configure(costmap_publisher_->get_current_state());
+ costmap_publisher_->on_activate(costmap_publisher_->get_current_state());
+ }
+
+ ~TestNode() override
+ {
+ costmap_publisher_->on_deactivate(costmap_publisher_->get_current_state());
+ costmap_publisher_->on_cleanup(costmap_publisher_->get_current_state());
+ }
+
+protected:
+ std::shared_ptr<TestCostmap2dPublisher> costmap_publisher_;
+};
+
+TEST_F(TestNode, costmap_pub_test)
+{
+ auto future = costmap_publisher_->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 that the first row is free space
+ for (int i = 0; i < 10; ++i) {
+ EXPECT_EQ(costmap_raw->data[0], nav2_costmap_2d::FREE_SPACE);
+ }
+}