Revert "Add ability to publish layers of the costmap (ros-navigation#…

…3254)" (ros-navigation#3319)

This reverts commit 9538ada.

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -320,9 +320,7 @@ 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_;
-
-  std::vector<std::unique_ptr<Costmap2DPublisher>> layer_publishers_;
+  std::unique_ptr<Costmap2DPublisher> costmap_publisher_{nullptr};
 
   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();
+
   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,20 +210,6 @@ 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_));
@@ -247,12 +233,8 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Activating");
 
-  footprint_pub_->on_activate();
   costmap_publisher_->on_activate();
-
-  for (auto & layer_pub : layer_publishers_) {
-    layer_pub->on_activate();
-  }
+  footprint_pub_->on_activate();
 
   // First, make sure that the transform between the robot base frame
   // and the global frame is available
@@ -298,13 +280,8 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_INFO(get_logger(), "Deactivating");
 
   dyn_params_handler.reset();
-
-  footprint_pub_->on_deactivate();
   costmap_publisher_->on_deactivate();
-
-  for (auto & layer_pub : layer_publishers_) {
-    layer_pub->on_deactivate();
-  }
+  footprint_pub_->on_deactivate();
 
   stop();
 
@@ -320,13 +297,6 @@ 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();
@@ -335,6 +305,8 @@ 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;
@@ -476,22 +448,12 @@ 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,18 +64,6 @@ 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"
@@ -126,16 +114,6 @@ 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,20 +30,6 @@ 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',
@@ -53,8 +39,6 @@ def main(argv=sys.argv[1:]):
 
     ld = LaunchDescription([
         IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
-        map_to_odom,
-        odom_to_base_link,
         lifecycle_manager
     ])