From 13fd057498b21c861c5054ad15fc0f6b1a892705 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 8 Dec 2022 10:49:17 -0800 Subject: [PATCH] Revert "Add ability to publish layers of the costmap (#3254)" (#3319) This reverts commit 9538ada18a1d0d41b1283e8be6267c2a47ff8767. --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 4 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 48 +---- .../test/integration/CMakeLists.txt | 22 --- .../test/integration/costmap_tests_launch.py | 16 -- .../integration/test_costmap_2d_publisher.cpp | 178 ------------------ 6 files changed, 7 insertions(+), 263 deletions(-) delete mode 100644 nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp 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 46a71c8e76..2124a12a44 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,9 +320,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_; - - std::vector> layer_publishers_; + std::unique_ptr costmap_publisher_{nullptr}; 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 87881bc7ca..906a9f0324 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(); + 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 aa7fd7f12b..52b538b6bb 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/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(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_)); @@ -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; } } diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 0768972a0b..ef0b148440 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/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=$ ) -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 706290d826..5e8c19a485 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/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 ]) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp deleted file mode 100644 index 1e678d67c7..0000000000 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ /dev/null @@ -1,178 +0,0 @@ -// 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); - } -}