diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 5f72cfd363..ce987eb118 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -65,9 +65,9 @@ jobs: steps: - uses: actions/checkout@v4 - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Login to Docker Hub - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: registry: ghcr.io username: ${{ github.repository_owner }} @@ -99,7 +99,7 @@ jobs: - name: Build and push ${{ github.ref_name }} if: steps.config.outputs.trigger == 'true' id: docker_build - uses: docker/build-push-action@v4 + uses: docker/build-push-action@v5 with: context: . pull: true diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp index c3b406cfd4..713478d24d 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp @@ -49,7 +49,7 @@ typedef struct // Return a zero vector -pf_vector_t pf_vector_zero(); +pf_vector_t pf_vector_zero(void); // Check for NAN or INF in any component // int pf_vector_finite(pf_vector_t a); @@ -71,7 +71,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); // Return a zero matrix -pf_matrix_t pf_matrix_zero(); +pf_matrix_t pf_matrix_zero(void); // Check for NAN or INF in any component // int pf_matrix_finite(pf_matrix_t a); diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index de4e3247d1..9d1f5a8289 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -463,7 +463,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) // Workspace double m[4], c[2][2]; - size_t count; double weight; // Cluster the samples @@ -474,7 +473,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) for (i = 0; i < set->cluster_max_count; i++) { cluster = set->clusters + i; - cluster->count = 0; cluster->weight = 0; cluster->mean = pf_vector_zero(); cluster->cov = pf_matrix_zero(); @@ -490,7 +488,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) } // Initialize overall filter stats - count = 0; weight = 0.0; set->mean = pf_vector_zero(); set->cov = pf_matrix_zero(); @@ -521,10 +518,8 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) cluster = set->clusters + cidx; - cluster->count += 1; cluster->weight += sample->weight; - count += 1; weight += sample->weight; // Compute mean diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c index a7a5cd39c6..00fa384060 100644 --- a/nav2_amcl/src/pf/pf_vector.c +++ b/nav2_amcl/src/pf/pf_vector.c @@ -35,7 +35,7 @@ // Return a zero vector -pf_vector_t pf_vector_zero() +pf_vector_t pf_vector_zero(void) { pf_vector_t c; @@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b) // Return a zero matrix -pf_matrix_t pf_matrix_zero() +pf_matrix_t pf_matrix_zero(void) { int i, j; pf_matrix_t c; diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index db761cb31d..84e16e7d4e 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/ +For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 64cc20b4e9..5e316835ae 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -162,7 +162,7 @@ class BtActionNode : public BT::ActionNodeBase } /** - * @brief Function to perform some user-defined operation whe the action is aborted. + * @brief Function to perform some user-defined operation when the action is aborted. * @return BT::NodeStatus Returns FAILURE by default, user may override return another value */ virtual BT::NodeStatus on_aborted() @@ -206,7 +206,8 @@ class BtActionNode : public BT::ActionNodeBase // if new goal was sent and action server has not yet responded // check the future goal handle if (future_goal_handle_) { - auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + auto elapsed = + (node_->now() - time_goal_sent_).template to_chrono(); if (!is_future_goal_handle_complete(elapsed)) { // return RUNNING if there is still some time before timeout happens if (elapsed < server_timeout_) { @@ -237,7 +238,8 @@ class BtActionNode : public BT::ActionNodeBase { goal_updated_ = false; send_new_goal(); - auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + auto elapsed = + (node_->now() - time_goal_sent_).template to_chrono(); if (!is_future_goal_handle_complete(elapsed)) { if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; @@ -300,6 +302,7 @@ class BtActionNode : public BT::ActionNodeBase void halt() override { if (should_cancel_goal()) { + auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) @@ -308,6 +311,16 @@ class BtActionNode : public BT::ActionNodeBase node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); } + + if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), + "Failed to get result for %s in node halt!", action_name_.c_str()); + } + + on_cancelled(); } setStatus(BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 79b1df5bb4..2dcb21e42b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -196,6 +196,11 @@ class BtActionServer */ void populateErrorCode(typename std::shared_ptr result); + /** + * @brief Setting BT error codes to success. Used to clean blackboard between different BT runs + */ + void cleanErrorCodes(); + // Action name std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 9a8ddc613c..3e2ee0aee8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -61,6 +61,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + if (!node->has_parameter("action_server_result_timeout")) { + node->declare_parameter("action_server_result_timeout", 900.0); + } std::vector error_code_names = { "follow_path_error_code", @@ -123,30 +126,38 @@ bool BtActionServer::on_configure() // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); - // Declare parameters for client node to share with BT nodes - // Declare if not declared in case being used an external application + // Declare parameters for common client node applications to share with BT nodes + // Declare if not declared in case being used an external application, then copying + // all of the main node's parameters to the client for BT nodes to obtain nav2_util::declare_parameter_if_not_declared( node, "global_frame", rclcpp::ParameterValue(std::string("map"))); nav2_util::declare_parameter_if_not_declared( node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); - client_node_->declare_parameter( - "robot_base_frame", node->get_parameter("robot_base_frame").as_string()); - client_node_->declare_parameter( - "global_frame", node->get_parameter("global_frame").as_string()); + nav2_util::declare_parameter_if_not_declared( + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + nav2_util::copy_all_parameters(node, client_node_); + + // set the timeout in seconds for the action server to discard goal handles if not finished + double action_server_result_timeout; + node->get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); action_server_ = std::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - action_name_, std::bind(&BtActionServer::executeCallback, this)); + action_name_, std::bind(&BtActionServer::executeCallback, this), + nullptr, std::chrono::milliseconds(500), false, server_options); // Get parameters for BT timeouts - int timeout; - node->get_parameter("bt_loop_duration", timeout); - bt_loop_duration_ = std::chrono::milliseconds(timeout); - node->get_parameter("default_server_timeout", timeout); - default_server_timeout_ = std::chrono::milliseconds(timeout); + int bt_loop_duration; + node->get_parameter("bt_loop_duration", bt_loop_duration); + bt_loop_duration_ = std::chrono::milliseconds(bt_loop_duration); + int default_server_timeout; + node->get_parameter("default_server_timeout", default_server_timeout); + default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -236,6 +247,7 @@ void BtActionServer::executeCallback() { if (!on_goal_received_callback_(action_server_->get_current_goal())) { action_server_->terminate_current(); + cleanErrorCodes(); return; } @@ -290,6 +302,8 @@ void BtActionServer::executeCallback() action_server_->terminate_all(result); break; } + + cleanErrorCodes(); } template @@ -316,6 +330,14 @@ void BtActionServer::populateErrorCode( } } +template +void BtActionServer::cleanErrorCodes() +{ + for (const auto & error_code : error_code_names_) { + blackboard_->set(error_code, 0); //NOLINT + } +} + } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index d76aa693ff..179c93e4d2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -116,7 +116,7 @@ class BtCancelActionNode : public BT::ActionNodeBase return basic; } - void halt() + void halt() override { } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 26efba04c9..a027ac7760 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -183,7 +183,7 @@ class BtServiceNode : public BT::ActionNodeBase */ virtual BT::NodeStatus check_future() { - auto elapsed = (node_->now() - sent_time_).to_chrono(); + auto elapsed = (node_->now() - sent_time_).template to_chrono(); auto remaining = server_timeout_ - elapsed; if (remaining > std::chrono::milliseconds(0)) { @@ -199,7 +199,7 @@ class BtServiceNode : public BT::ActionNodeBase if (rc == rclcpp::FutureReturnCode::TIMEOUT) { on_wait_for_result(); - elapsed = (node_->now() - sent_time_).to_chrono(); + elapsed = (node_->now() - sent_time_).template to_chrono(); if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; } diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index 211254dafe..a898dec947 100644 --- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp @@ -95,7 +95,7 @@ void IsStuckCondition::logStuck(const std::string & msg) const return; } - RCLCPP_INFO(node_->get_logger(), msg.c_str()); + RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str()); prev_msg = msg; } diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index ddc6753446..8daca5afed 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -39,7 +39,7 @@ TransformAvailableCondition::TransformAvailableCondition( RCLCPP_FATAL( node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.", child_frame_.c_str(), parent_frame_.c_str()); - exit(-1); + throw std::runtime_error("TransformAvailableCondition: Child or parent frames not provided!"); } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index e5c96eba2c..f47dfed38a 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -43,7 +43,7 @@ SpeedController::SpeedController( if (min_rate_ <= 0.0 || max_rate_ <= 0.0) { std::string err_msg = "SpeedController node cannot have rate <= 0.0"; - RCLCPP_FATAL(node_->get_logger(), err_msg.c_str()); + RCLCPP_FATAL(node_->get_logger(), "%s", err_msg.c_str()); throw BT::BehaviorTreeException(err_msg); } diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index d95cbd4a84..12c25d30d6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -54,11 +54,17 @@ class FibonacciActionServer : public rclcpp::Node sleep_duration_ = sleep_duration; } + void setServerLoopRate(std::chrono::nanoseconds server_loop_rate) + { + server_loop_rate_ = server_loop_rate; + } + protected: rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID &, std::shared_ptr) { + RCLCPP_INFO(this->get_logger(), "Goal is received.."); if (sleep_duration_ > 0ms) { std::this_thread::sleep_for(sleep_duration_); } @@ -73,6 +79,13 @@ class FibonacciActionServer : public rclcpp::Node void handle_accepted( const std::shared_ptr> handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&FibonacciActionServer::execute, this, _1), handle}.detach(); + } + + void execute( + const std::shared_ptr> handle) { // this needs to return quickly to avoid blocking the executor, so spin up a new thread if (handle) { @@ -88,8 +101,17 @@ class FibonacciActionServer : public rclcpp::Node sequence.push_back(0); sequence.push_back(1); + rclcpp::Rate rate(server_loop_rate_); for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + if (handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Goal is canceling."); + handle->canceled(result); + return; + } + + RCLCPP_INFO(this->get_logger(), "Goal is feedbacking."); sequence.push_back(sequence[i] + sequence[i - 1]); + rate.sleep(); } handle->succeed(result); @@ -99,6 +121,7 @@ class FibonacciActionServer : public rclcpp::Node protected: rclcpp_action::Server::SharedPtr action_server_; std::chrono::milliseconds sleep_duration_; + std::chrono::nanoseconds server_loop_rate_; }; class FibonacciAction : public nav2_behavior_tree::BtActionNode @@ -121,6 +144,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset>("sequence", result_.result->sequence); + config().blackboard->set("on_cancelled_triggered", true); + return BT::NodeStatus::SUCCESS; + } + static BT::PortsList providedPorts() { return providedBasicPorts({BT::InputPort("order", "Fibonacci order")}); @@ -144,6 +174,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("on_cancelled_triggered", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -220,6 +251,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -255,15 +287,22 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // start a new execution cycle with the previous BT to ensure previous state doesn't leak into // the new cycle - // halt BT for a new execution cycle + // halt BT for a new execution cycle, + // get if the on_cancelled is triggered from blackboard and assert + // that the on_cancelled triggers after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a large action server goal handling duration action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -300,6 +339,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the action server will take 100ms before accepting the goal action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -327,14 +367,21 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the new cycle // halt BT for a new execution cycle + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancelled never can trigger after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(25ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -348,6 +395,90 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) EXPECT_EQ(result, BT::NodeStatus::SUCCESS); } +TEST_F(BTActionNodeTestFixture, test_server_cancel) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // setting a server timeout smaller than the time the action server will take to accept the goal + // to simulate a server timeout scenario + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 50ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // to keep track of the number of ticks it took to reach expected tick count + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(100ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 5 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 5); + + // send new goal to the action server for a new execution cycle + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 1000ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // reset state variable + ticks = 0; + config_->blackboard->set("on_cancelled_triggered", false); + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 7 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 7); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index cd32a9f48c..de4aec7532 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -93,7 +93,7 @@ class DriveOnHeading : public TimedBehavior * @brief Loop function to run behavior * @return Status of behavior */ - ResultStatus onCycleUpdate() + ResultStatus onCycleUpdate() override { rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 821818dd2f..6af6490b61 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -133,9 +133,19 @@ class TimedBehavior : public nav2_core::Behavior node->get_parameter("robot_base_frame", robot_base_frame_); node->get_parameter("transform_tolerance", transform_tolerance_); + if (!node->has_parameter("action_server_result_timeout")) { + node->declare_parameter("action_server_result_timeout", 10.0); + } + + double action_server_result_timeout; + node->get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + action_server_ = std::make_shared( node, behavior_name_, - std::bind(&TimedBehavior::execute, this)); + std::bind(&TimedBehavior::execute, this), nullptr, std::chrono::milliseconds( + 500), false, server_options); local_collision_checker_ = local_collision_checker; global_collision_checker_ = global_collision_checker; diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index b5ec19ba62..c7beffd786 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -105,13 +105,13 @@ BehaviorServer::loadBehaviorPlugins() auto node = shared_from_this(); for (size_t i = 0; i != behavior_ids_.size(); i++) { - behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]); try { + behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]); RCLCPP_INFO( get_logger(), "Creating behavior plugin %s of type %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str()); behaviors_.push_back(plugin_loader_.createUniqueInstance(behavior_types_[i])); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), "Failed to create behavior %s of type %s." " Exception: %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str(), diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index c2b2dda69a..692ca6ec80 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -277,6 +278,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 54704a2e7a..2d57aef383 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -276,6 +277,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 372dcdd3f6..6f7e66cd61 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -324,6 +325,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index cf26d389db..112a1de98b 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -320,6 +321,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index bc9c8049ce..2b552e1b02 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -44,7 +44,7 @@ class BtNavigator : public nav2_util::LifecycleNode * @brief A constructor for nav2_bt_navigator::BtNavigator class * @param options Additional options to control creation of the node. */ - explicit BtNavigator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit BtNavigator(rclcpp::NodeOptions options = rclcpp::NodeOptions()); /** * @brief A destructor for nav2_bt_navigator::BtNavigator class */ diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index d256ebf014..4b690bb296 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -72,7 +72,7 @@ class NavigateToPoseNavigator * @brief Get action name for this navigator * @return string Name of action server */ - std::string getName() {return std::string("navigate_to_pose");} + std::string getName() override {return std::string("navigate_to_pose");} /** * @brief Get navigator's default BT diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 401a5b0eb5..82beb93999 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -29,8 +29,9 @@ using nav2_util::declare_parameter_if_not_declared; namespace nav2_bt_navigator { -BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("bt_navigator", "", options), +BtNavigator::BtNavigator(rclcpp::NodeOptions options) +: nav2_util::LifecycleNode("bt_navigator", "", + options.automatically_declare_parameters_from_overrides(true)), class_loader_("nav2_core", "nav2_core::NavigatorBase") { RCLCPP_INFO(get_logger(), "Creating"); @@ -89,11 +90,16 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_is_battery_charging_condition_bt_node" }; - declare_parameter("plugin_lib_names", plugin_libs); - declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter("global_frame", std::string("map")); - declare_parameter("robot_base_frame", std::string("base_link")); - declare_parameter("odom_topic", std::string("odom")); + declare_parameter_if_not_declared( + this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs)); + declare_parameter_if_not_declared( + this, "transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + this, "global_frame", rclcpp::ParameterValue(std::string("map"))); + declare_parameter_if_not_declared( + this, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter_if_not_declared( + this, "odom_topic", rclcpp::ParameterValue(std::string("odom"))); } BtNavigator::~BtNavigator() @@ -155,8 +161,8 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) // Load navigator plugins for (size_t i = 0; i != navigator_ids.size(); i++) { - std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]); try { + std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]); RCLCPP_INFO( get_logger(), "Creating navigator id %s of type %s", navigator_ids[i].c_str(), navigator_type.c_str()); @@ -167,11 +173,10 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) { return nav2_util::CallbackReturn::FAILURE; } - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( - get_logger(), "Failed to create navigator id %s of type %s." - " Exception: %s", navigator_ids[i].c_str(), navigator_type.c_str(), - ex.what()); + get_logger(), "Failed to create navigator id %s." + " Exception: %s", navigator_ids[i].c_str(), ex.what()); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 864b28bbe4..1c5b32dc7d 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -14,6 +14,10 @@ The costmaps / trajectory planners will handle most situations, but this is to h ![polygons.png](doc/polygons.png) +Demonstration of Collision Monitor abilities presented at 6th ROS Developers Day 2023, could be found below: + +[![cm-ros-devday.png](doc/cm_ros_devday.png)](https://www.youtube.com/watch?v=bWliK0PC5Ms) + ### Features The Collision Monitor uses polygons relative the robot's base frame origin to define "zones". @@ -83,4 +87,4 @@ The zones around the robot and the data sources are the same as for the Collisio ### Configuration -Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-detector.html). \ No newline at end of file +Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). diff --git a/nav2_collision_monitor/doc/cm_ros_devday.png b/nav2_collision_monitor/doc/cm_ros_devday.png new file mode 100644 index 0000000000..63cc8fdea4 Binary files /dev/null and b/nav2_collision_monitor/doc/cm_ros_devday.png differ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index a68fece757..815694d83c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -85,6 +85,11 @@ class Polygon * @return Action type for current polygon */ ActionType getActionType() const; + /** + * @brief Obtains polygon enabled state + * @return Whether polygon is enabled + */ + bool getEnabled() const; /** * @brief Obtains polygon minimum points to enter inside polygon causing the action * @return Minimum number of data readings within a zone to trigger the action @@ -191,6 +196,13 @@ class Polygon */ void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + /** * @brief Checks if point is inside polygon * @param point Given point to check @@ -204,6 +216,8 @@ class Polygon nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of polygon @@ -222,6 +236,8 @@ class Polygon double time_before_collision_; /// @brief Time step for robot movement simulation double simulation_time_step_; + /// @brief Whether polygon is enabled + bool enabled_; /// @brief Polygon subscription rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 8bc750cc71..30d58d53bc 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -74,7 +74,19 @@ class Source const rclcpp::Time & curr_time, std::vector & data) const = 0; + /** + * @brief Obtains source enabled state + * @return Whether source is enabled + */ + bool getEnabled() const; + protected: + /** + * @brief Source configuration routine. + * @return True in case of everything is configured correctly, or false otherwise + */ + bool configure(); + /** * @brief Supporting routine obtaining ROS-parameters common for all data sources * @param source_topic Output name of source subscription topic @@ -91,12 +103,21 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + // ----- Variables ----- /// @brief Collision Monitor node nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of data source @@ -116,6 +137,8 @@ class Source /// @brief Whether to correct source data towards to base frame movement, /// considering the difference between current time and latest source time bool base_shift_correction_; + /// @brief Whether source is enabled + bool enabled_; }; // class Source } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 218ce45239..68b4cd7b50 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -14,12 +14,15 @@ collision_detector: min_points: 4 visualize: True polygon_pub_topic: "polygon_front" + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 239960e593..fcb01f5482 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -2,7 +2,7 @@ collision_monitor: ros__parameters: base_frame_id: "base_footprint" odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_raw" + cmd_vel_in_topic: "cmd_vel_smoothed" cmd_vel_out_topic: "cmd_vel" state_topic: "collision_monitor_state" transform_tolerance: 0.5 @@ -22,6 +22,7 @@ collision_monitor: min_points: 4 visualize: True polygon_pub_topic: "polygon_stop" + enabled: True PolygonSlow: type: "polygon" points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] @@ -30,6 +31,7 @@ collision_monitor: slowdown_ratio: 0.3 visualize: True polygon_pub_topic: "polygon_slowdown" + enabled: True PolygonLimit: type: "polygon" points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] @@ -39,6 +41,7 @@ collision_monitor: angular_limit: 0.5 visualize: True polygon_pub_topic: "polygon_limit" + enabled: True FootprintApproach: type: "polygon" action_type: "approach" @@ -47,12 +50,15 @@ collision_monitor: simulation_time_step: 0.1 min_points: 6 visualize: False + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index e12d9795db..6a5aca7dfe 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -224,7 +224,6 @@ bool CollisionDetector::configurePolygons( polygon_name.c_str()); return false; } - } } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); @@ -304,17 +303,22 @@ void CollisionDetector::process() // Fill collision_points array from different data sources for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); + if (source->getEnabled()) { + source->getData(curr_time, collision_points); + } } std::unique_ptr state_msg = std::make_unique(); for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } state_msg->polygons.push_back(polygon->getName()); state_msg->detections.push_back( polygon->getPointsInside( - collision_points) > polygon->getMinPoints()); + collision_points) >= polygon->getMinPoints()); } state_pub_->publish(std::move(state_msg)); @@ -326,7 +330,9 @@ void CollisionDetector::process() void CollisionDetector::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 6b678c9c76..a1395a7b70 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -205,7 +205,7 @@ bool CollisionMonitor::getParameters( auto node = shared_from_this(); nav2_util::declare_parameter_if_not_declared( - node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_raw")); + node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_smoothed")); cmd_vel_in_topic = get_parameter("cmd_vel_in_topic").as_string(); nav2_util::declare_parameter_if_not_declared( node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel")); @@ -373,7 +373,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) // Fill collision_points array from different data sources for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); + if (source->getEnabled()) { + source->getData(curr_time, collision_points); + } } // By default - there is no action @@ -382,6 +384,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) std::shared_ptr action_polygon; for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } if (robot_action.action_type == STOP) { // If robot already should stop, do nothing break; @@ -545,7 +550,9 @@ void CollisionMonitor::notifyActionState( void CollisionMonitor::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 4582703b2f..f23f5e0bfe 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -49,6 +49,7 @@ PointCloud::~PointCloud() void PointCloud::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 8b7a3c7c00..248bdcec6d 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -48,6 +48,7 @@ Polygon::~Polygon() polygon_sub_.reset(); polygon_pub_.reset(); poly_.clear(); + dyn_params_handler_.reset(); } bool Polygon::configure() @@ -102,6 +103,10 @@ bool Polygon::configure() polygon_pub_topic, polygon_qos); } + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1)); + return true; } @@ -129,6 +134,11 @@ ActionType Polygon::getActionType() const return action_type_; } +bool Polygon::getEnabled() const +{ + return enabled_; +} + int Polygon::getMinPoints() const { return min_points_; @@ -302,6 +312,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return false; } + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool(); + nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".min_points", rclcpp::ParameterValue(4)); min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int(); @@ -487,6 +501,26 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m polygon_ = *msg; } +rcl_interfaces::msg::SetParametersResult +Polygon::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == polygon_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) { RCLCPP_INFO( diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index cd070d70be..56c0a4750f 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -49,6 +49,7 @@ Range::~Range() void Range::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 731ee8baa8..603e2f04e6 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -47,6 +47,7 @@ Scan::~Scan() void Scan::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index df539495f2..8dcf0c85e2 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -43,6 +43,17 @@ Source::~Source() { } +bool Source::configure() +{ + auto node = node_.lock(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Source::dynamicParametersCallback, this, std::placeholders::_1)); + + return true; +} + void Source::getCommonParameters(std::string & source_topic) { auto node = node_.lock(); @@ -54,6 +65,10 @@ void Source::getCommonParameters(std::string & source_topic) node, source_name_ + ".topic", rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner source_topic = node->get_parameter(source_name_ + ".topic").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool(); } bool Source::sourceValid( @@ -75,4 +90,29 @@ bool Source::sourceValid( return true; } +bool Source::getEnabled() const +{ + return enabled_; +} + +rcl_interfaces::msg::SetParametersResult +Source::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == source_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 3400f1d50d..eab3e17754 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -45,7 +45,6 @@ static constexpr double EPSILON = 1e-5; static const char BASE_FRAME_ID[]{"base_link"}; static const char SOURCE_FRAME_ID[]{"base_source"}; static const char ODOM_FRAME_ID[]{"odom"}; -static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index e11f3b1b3d..e66e33e5cb 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -160,6 +160,9 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp); bool waitCmdVel(const std::chrono::nanoseconds & timeout); + bool waitFuture( + rclcpp::Client::SharedFuture, + const std::chrono::nanoseconds & timeout); bool waitActionState(const std::chrono::nanoseconds & timeout); protected: @@ -186,6 +189,9 @@ class Tester : public ::testing::Test // CollisionMonitor Action state rclcpp::Subscription::SharedPtr action_state_sub_; nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; + + // Service client for setting CollisionMonitor parameters + rclcpp::Client::SharedPtr parameters_client_; }; // Tester Tester::Tester() @@ -211,6 +217,10 @@ Tester::Tester() action_state_sub_ = cm_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), std::bind(&Tester::actionStateCallback, this, std::placeholders::_1)); + parameters_client_ = + cm_->create_client( + std::string( + cm_->get_name()) + "/set_parameters"); } Tester::~Tester() @@ -308,6 +318,11 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".type", "unknown")); } + cm_->declare_parameter( + polygon_name + ".enabled", rclcpp::ParameterValue(true)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".enabled", true)); + cm_->declare_parameter( polygon_name + ".action_type", rclcpp::ParameterValue(at)); cm_->set_parameter( @@ -579,6 +594,22 @@ bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitFuture( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) { rclcpp::Time start_time = cm_->now(); @@ -961,6 +992,116 @@ TEST_F(Tester, testCeasePublishZeroVel) cm_->stop(); } +TEST_F(Tester, testPolygonNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when polygon is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "Stop"); + + // Disable polygon by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = "Stop.enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when polygon is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testSourceNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when source is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "Stop"); + + // Disable source by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = std::string(SCAN_NAME) + ".enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when source is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testProcessNonActive) { rclcpp::Time curr_time = cm_->now(); @@ -1022,7 +1163,7 @@ TEST_F(Tester, testSourcesNotSet) setCommonParameters(); addPolygon("Stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - cm_->declare_parameter("polygons", rclcpp::ParameterValue({"Stop"})); + cm_->declare_parameter("polygons", rclcpp::ParameterValue(std::vector{"Stop"})); cm_->set_parameter(rclcpp::Parameter("polygons", std::vector{"Stop"})); // Check that Collision Monitor node can not be configured for this parameters set diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 2887fa5503..071c884864 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -87,6 +87,7 @@ def perform(self, context: launch.LaunchContext) -> Text: param_rewrites, keys_rewrites = self.resolve_rewrites(context) data = yaml.safe_load(open(yaml_filename, 'r')) self.substitute_params(data, param_rewrites) + self.add_params(data, param_rewrites) self.substitute_keys(data, keys_rewrites) if self.__root_key is not None: root_key = launch.utilities.perform_substitutions(context, self.__root_key) @@ -121,6 +122,15 @@ def substitute_params(self, yaml, param_rewrites): yaml_keys = path.split('.') yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val) + def add_params(self, yaml, param_rewrites): + # add new total path parameters + yaml_paths = self.pathify(yaml) + for path in param_rewrites: + if not path in yaml_paths: + new_val = self.convert(param_rewrites[path]) + yaml_keys = path.split('.') + if 'ros__parameters' in yaml_keys: + yaml = self.updateYamlPathVals(yaml, yaml_keys, new_val) def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): for key in yaml_key_list: diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 463af33fd0..1ccae77f77 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -100,7 +100,7 @@ class SmootherTest : public ::testing::Test SmootherTest() {SetUp();} ~SmootherTest() {} - void SetUp() + void SetUp() override { node_lifecycle_ = std::make_shared( diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c76f9f86cc..eb32a73ab4 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -50,6 +50,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("controller_frequency", 20.0); + declare_parameter("action_server_result_timeout", 10.0); + declare_parameter("progress_checker_plugins", default_progress_checker_ids_); declare_parameter("goal_checker_plugins", default_goal_checker_ids_); declare_parameter("controller_plugins", default_ids_); @@ -84,20 +86,6 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "getting progress checker plugins.."); get_parameter("progress_checker_plugins", progress_checker_ids_); - try { - nav2_util::declare_parameter_if_not_declared( - node, "progress_checker_plugin", rclcpp::PARAMETER_STRING); - std::string progress_checker_plugin; - progress_checker_plugin = node->get_parameter("progress_checker_plugin").as_string(); - progress_checker_ids_.clear(); - progress_checker_ids_.push_back(progress_checker_plugin); - RCLCPP_WARN( - get_logger(), - "\"progress_checker_plugin\" parameter was deprecated and will be removed soon. Use " - "\"progress_checker_plugins\" instead to specify a list of plugins"); - } catch (const std::exception &) { - // This is normal situation: progress_checker_plugin parameter should not being declared - } if (progress_checker_ids_ == default_progress_checker_ids_) { for (size_t i = 0; i < default_progress_checker_ids_.size(); ++i) { nav2_util::declare_parameter_if_not_declared( @@ -154,7 +142,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) progress_checker_ids_[i].c_str(), progress_checker_types_[i].c_str()); progress_checker->initialize(node, progress_checker_ids_[i]); progress_checkers_.insert({progress_checker_ids_[i], progress_checker}); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); @@ -227,6 +215,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) odom_sub_ = std::make_unique(node); vel_publisher_ = create_publisher("cmd_vel", 1); + double action_server_result_timeout; + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + // Create the action server that we implement with our followPath method action_server_ = std::make_unique( shared_from_this(), @@ -234,7 +227,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) std::bind(&ControllerServer::computeControl, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); // Set subscribtion to the speed limiting topic speed_limit_sub_ = create_subscription( @@ -601,7 +594,7 @@ void ControllerServer::computeAndPublishVelocity() // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & e) { if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { - RCLCPP_WARN(this->get_logger(), e.what()); + RCLCPP_WARN(this->get_logger(), "%s", e.what()); cmd_vel_2d.twist.angular.x = 0; cmd_vel_2d.twist.angular.y = 0; cmd_vel_2d.twist.angular.z = 0; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index 6fbb535244..7ae3f4d1c7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp @@ -72,5 +72,5 @@ static constexpr unsigned char LETHAL_OBSTACLE = 254; static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; static constexpr unsigned char MAX_NON_OBSTACLE = 252; static constexpr unsigned char FREE_SPACE = 0; -} +} // namespace nav2_costmap_2d #endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp index 4527e07fdd..99d24240d2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include namespace nav2_costmap_2d { @@ -77,7 +79,7 @@ void morphologyOperation( const Image & input, Image & output, const Image & shape, AggregateFn aggregate); -using ShapeBuffer3x3 = std::array; +using ShapeBuffer3x3 = std::array; // NOLINT inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity); } // namespace imgproc_impl @@ -95,7 +97,7 @@ inline void dilate( const Image & input, Image & output, ConnectivityType connectivity, Max && max_function) { - using namespace imgproc_impl; + using namespace imgproc_impl; // NOLINT ShapeBuffer3x3 shape_buffer; Image shape = createShape(shape_buffer, connectivity); morphologyOperation(input, output, shape, max_function); @@ -376,7 +378,7 @@ struct EquivalenceLabelTreesBase struct LabelOverflow : public std::runtime_error { - LabelOverflow(const std::string & message) + explicit LabelOverflow(const std::string & message) : std::runtime_error(message) {} }; @@ -464,7 +466,6 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase { Label k = 1; for (Label i = 1; i < next_free_; ++i) { - if (labels_[i] < i) { labels_[i] = labels_[labels_[i]]; } else { @@ -504,7 +505,7 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase * '~' - row continuation in the same style */ max_labels = (rows * columns) / 3 + 1; } - ++max_labels; // add zero label + ++max_labels; // add zero label max_labels = std::min(max_labels, size_t(std::numeric_limits