diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index eb32a73ab4..0386c06fc7 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -242,7 +242,10 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_ros_->activate(); + const auto costmap_ros_state = costmap_ros_->activate(); + if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + return nav2_util::CallbackReturn::FAILURE; + } ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); 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..f319e52f33 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 @@ -360,7 +360,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool always_send_full_costmap_{false}; std::string footprint_; float footprint_padding_{0}; - std::string global_frame_; ///< The global frame for the costmap + std::string global_frame_; ///< The global frame for the costmap int map_height_meters_{0}; double map_publish_frequency_{0}; double map_update_frequency_{0}; @@ -374,11 +374,12 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector filter_names_; std::vector filter_types_; double resolution_{0}; - std::string robot_base_frame_; ///< The frame_id of the robot base + std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; - bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap + bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap bool track_unknown_space_{false}; - double transform_tolerance_{0}; ///< The timeout before transform errors + double transform_tolerance_{0}; ///< The timeout before transform errors + double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors // Derived parameters bool use_radius_{false}; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 581db0065c..c35933c908 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -129,6 +129,7 @@ void Costmap2DROS::init() declare_parameter("rolling_window", rclcpp::ParameterValue(false)); declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + declare_parameter("initial_transform_timeout", rclcpp::ParameterValue(60.0)); declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); @@ -265,13 +266,6 @@ 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(); - } - // First, make sure that the transform between the robot base frame // and the global frame is available @@ -279,6 +273,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Checking transform"); rclcpp::Rate r(2); + const auto initial_transform_timeout = rclcpp::Duration::from_seconds( + initial_transform_timeout_); + const auto initial_transform_timeout_point = now() + initial_transform_timeout; while (rclcpp::ok() && !tf_buffer_->canTransform( global_frame_, robot_base_frame_, tf2::TimePointZero, &tf_error)) @@ -288,12 +285,29 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) " to become available, tf error: %s", robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); + // Check timeout + if (now() > initial_transform_timeout_point) { + RCLCPP_ERROR( + get_logger(), "Failed to activate %s because transform from %s to %s did not become available before timeout", + get_name(), robot_base_frame_.c_str(), global_frame_.c_str()); + + return nav2_util::CallbackReturn::FAILURE; + } + // The error string will accumulate and errors will typically be the same, so the last // will do for the warning above. Reset the string here to avoid accumulation tf_error.clear(); r.sleep(); } + // Activate publishers + footprint_pub_->on_activate(); + costmap_publisher_->on_activate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_activate(); + } + // Create a thread to handle updating the map stopped_ = true; // to active plugins stop_updates_ = false; @@ -386,6 +400,7 @@ Costmap2DROS::getParameters() get_parameter("rolling_window", rolling_window_); get_parameter("track_unknown_space", track_unknown_space_); get_parameter("transform_tolerance", transform_tolerance_); + get_parameter("initial_transform_timeout", initial_transform_timeout_); get_parameter("update_frequency", map_update_frequency_); get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 19c2edb56f..b9480fdffa 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -55,3 +55,8 @@ ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_p target_link_libraries(denoise_layer_test nav2_costmap_2d_core layers ) + +ament_add_gtest(lifecycle_test lifecycle_test.cpp) +target_link_libraries(lifecycle_test + nav2_costmap_2d_core +) \ No newline at end of file diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp new file mode 100644 index 0000000000..8a657d81ad --- /dev/null +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -0,0 +1,54 @@ +// 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. + +#include + +#include "gtest/gtest.h" + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "rclcpp/rclcpp.hpp" +#include "lifecycle_msgs/msg/state.hpp" + + +TEST(LifecylceTest, CheckInitialTfTimeout) { + rclcpp::init(0, nullptr); + + auto costmap = std::make_shared("test_costmap"); + costmap->set_parameter({"initial_transform_timeout", 0.0}); + + std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; + + { + const auto state_after_configure = costmap->configure(); + ASSERT_EQ(state_after_configure.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // Without providing the transform from global to robot base the activation should fail + // and the costmap should transition into the inactive state. + const auto state_after_activate = costmap->activate(); + ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + + // Set a dummy transform from global to robot base + geometry_msgs::msg::TransformStamped transform_global_to_robot{}; + transform_global_to_robot.header.frame_id = costmap->getGlobalFrameID(); + transform_global_to_robot.child_frame_id = costmap->getBaseFrameID(); + costmap->getTfBuffer()->setTransform(transform_global_to_robot, "test", true); + // Now the costmap should successful transition into the active state + { + const auto state_after_activate = costmap->activate(); + ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + rclcpp::shutdown(); + if (spin_thread.joinable()) { + spin_thread.join(); + } +} diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 779cdfe1bb..84edc6e4df 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -174,7 +174,10 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) plan_publisher_->on_activate(); action_server_pose_->activate(); action_server_poses_->activate(); - costmap_ros_->activate(); + const auto costmap_ros_state = costmap_ros_->activate(); + if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + return nav2_util::CallbackReturn::FAILURE; + } PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) {