Revert "Add ability to publish layers of the costmap (#3254)" (#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
  ])