Publish layers

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<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;
@@ -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;
         }
       }

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,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 <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 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<nav2_costmap_2d::Costmap2DROS>(
+      name_,
+      std::string{get_namespace()},
+      name_,
+      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
+  {
+    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<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  std::unique_ptr<nav2_util::NodeThread> 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<nav2_msgs::msg::Costmap>(
+      topic_name,
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+      std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1),
+      sub_option);
+
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_callback_group(callback_group_, node->get_node_base_interface());
+    executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
+  }
+
+  ~LayerSubscriber()
+  {
+    executor_thread_.reset();
+  }
+
+  std::promise<nav2_msgs::msg::Costmap::SharedPtr> 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<nav2_msgs::msg::Costmap>::SharedPtr layer_sub_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+  bool callback_hit_{false};
+};
+
+class CostmapRosTestFixture : public ::testing::Test
+{
+public:
+  CostmapRosTestFixture()
+  {
+    costmap_lifecycle_node_ = std::make_shared<CostmapRosLifecycleNode>("fake_costmap");
+    layer_subscriber_ = std::make_shared<LayerSubscriber>(
+      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<CostmapRosLifecycleNode> costmap_lifecycle_node_;
+  std::shared_ptr<LayerSubscriber> 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);
+  }
+}

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