diff --git a/.circleci/config.yml b/.circleci/config.yml index 9d3e0f490d..190ba5d7f1 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v16\ + - "<< parameters.key >>-v18\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v16\ + - "<< parameters.key >>-v18\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v16\ + key: "<< parameters.key >>-v18\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 630c2a0226..c1cfa2787f 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -9,6 +9,7 @@ | Ticket(s) this addresses | (add tickets here #1) | | Primary OS tested on | (Ubuntu, MacOS, Windows) | | Robotic platform tested on | (Steve's Robot, gazebo simulation of Tally, hardware turtlebot) | +| Does this PR contain AI generated software? | (No; Yes and it is marked inline in the code) | --- diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml new file mode 100644 index 0000000000..8732b239d9 --- /dev/null +++ b/.github/workflows/lint.yml @@ -0,0 +1,21 @@ +name: Lint +on: + pull_request: + +jobs: + ament_lint_general: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-latest + container: + image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + strategy: + fail-fast: false + matrix: + linter: [xmllint, cpplint, uncrustify, pep257, flake8] + steps: + - uses: actions/checkout@v4 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + linter: ${{ matrix.linter }} + distribution: rolling + package-name: "*" diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000000..cfba094dac --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/README.md b/README.md index 1d5b83ab57..e910e77b01 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,8 @@ For detailed instructions on how to: Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). +If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org. + ## Our Sponsors Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index dd03b3be02..85dc80b39c 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1167,7 +1167,7 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha1_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha1 to be negative," @@ -1177,7 +1177,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha2_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha2 to be negative," @@ -1187,7 +1187,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha3_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha3 to be negative," @@ -1197,7 +1197,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha4_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha4 to be negative," @@ -1207,7 +1207,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha5_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha5 to be negative," diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 596fda9791..daf609d067 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -25,6 +25,7 @@ #include "behaviortree_cpp_v3/xml_parsing.h" #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "rclcpp/rclcpp.hpp" namespace nav2_behavior_tree { @@ -46,7 +47,8 @@ class BehaviorTreeEngine * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine * @param plugin_libraries vector of BT plugin library names to load */ - explicit BehaviorTreeEngine(const std::vector & plugin_libraries); + explicit BehaviorTreeEngine( + const std::vector & plugin_libraries); virtual ~BehaviorTreeEngine() {} /** 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 e6eef2992f..39c5c5f7d9 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 @@ -61,6 +61,8 @@ class BtActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); // Initialize the input and output messages goal_ = typename ActionT::Goal(); @@ -93,7 +95,7 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); @@ -462,6 +464,9 @@ class BtActionNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_duration_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // To track the action server acknowledgement when a new goal is sent std::shared_ptr::SharedPtr>> future_goal_handle_; 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 467e4c80d0..331ca237c8 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 @@ -248,6 +248,9 @@ class BtActionServer // Default timeout value while waiting for response from a server std::chrono::milliseconds default_server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; 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 8d06ee7075..49574999c4 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 @@ -135,7 +135,7 @@ bool BtActionServer::on_configure() node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); nav2_util::declare_parameter_if_not_declared( node, "transform_tolerance", rclcpp::ParameterValue(0.1)); - nav2_util::copy_all_parameters(node, client_node_); + rclcpp::copy_all_parameter_values(node, client_node_); // set the timeout in seconds for the action server to discard goal handles if not finished double action_server_result_timeout; @@ -158,6 +158,9 @@ bool BtActionServer::on_configure() int default_server_timeout; node->get_parameter("default_server_timeout", default_server_timeout); default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); + int wait_for_service_timeout; + node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); + wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -172,6 +175,9 @@ bool BtActionServer::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT + blackboard_->set( + "wait_for_service_timeout", + wait_for_service_timeout_); return true; } @@ -235,6 +241,9 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); + blackboard->set( + "wait_for_service_timeout", + wait_for_service_timeout_); } } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); 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 179c93e4d2..ea3e41d859 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 @@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); std::string remapped_action_name; if (getInput("server_name", remapped_action_name)) { @@ -89,7 +91,7 @@ class BtCancelActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); @@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase // The timeout value while waiting for response from a server when a // new action goal is canceled std::chrono::milliseconds server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; }; } // namespace nav2_behavior_tree 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 a027ac7760..1afdb5adfd 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 @@ -62,6 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); @@ -77,7 +79,7 @@ class BtServiceNode : public BT::ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); - if (!service_client_->wait_for_service(1s)) { + if (!service_client_->wait_for_service(wait_for_service_timeout_)) { RCLCPP_ERROR( node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", service_node_name.c_str()); @@ -249,6 +251,9 @@ class BtServiceNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_duration_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + // To track the server response when a new request is sent std::shared_future future_result_; bool request_sent_{false}; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index fb3f683b46..97511608d5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -64,6 +64,11 @@ class AssistedTeleopAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -79,6 +84,9 @@ class BackUpAction : public BtActionNode "error_code_id", "The back up behavior server error code") }); } + +private: + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp index 7e5b27af9c..dd1a6f2908 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -43,6 +43,11 @@ class DriveOnHeadingAction : public BtActionNode tf_; std::string robot_base_frame_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index 2e442c0e11..4f9b300e44 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -48,6 +48,11 @@ class SpinAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -81,6 +86,7 @@ class SpinAction : public BtActionNode private: bool is_recovery_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index 5143f11351..fdc320c16b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -45,6 +45,11 @@ class WaitAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -56,6 +61,9 @@ class WaitAction : public BtActionNode BT::InputPort("wait_duration", 1.0, "Wait time") }); } + +private: + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index d29f40e66d..3c87c7370c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -52,6 +52,11 @@ class DistanceTraveledCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -74,6 +79,7 @@ class DistanceTraveledCondition : public BT::ConditionNode double distance_; double transform_tolerance_; std::string global_frame_, robot_base_frame_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 5c4753cf9a..ff47910580 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -51,6 +51,11 @@ class IsBatteryLowCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -81,6 +86,7 @@ class IsBatteryLowCondition : public BT::ConditionNode double min_battery_; bool is_voltage_; bool is_battery_low_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index a04b1263f4..36890f2a8a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -50,6 +50,11 @@ class IsPathValidCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -68,6 +73,7 @@ class IsPathValidCondition : public BT::ConditionNode // The timeout value while waiting for a responce from the // is path valid service std::chrono::milliseconds server_timeout_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index 9f8c47afab..356a79857d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -48,6 +48,11 @@ class TimeExpiredCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -63,6 +68,7 @@ class TimeExpiredCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; rclcpp::Time start_; double period_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 64572e21b7..5fdefa9b8b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -55,6 +55,11 @@ class TransformAvailableCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -75,6 +80,7 @@ class TransformAvailableCondition : public BT::ConditionNode std::string child_frame_; std::string parent_frame_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp index e53d6e4c35..729271a8c5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp @@ -18,6 +18,7 @@ #include #include "nav2_msgs/action/compute_path_to_pose.hpp" +#include "nav2_msgs/action/compute_path_through_poses.hpp" #include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp" namespace nav2_behavior_tree @@ -27,6 +28,8 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent { using Action = nav2_msgs::action::ComputePathToPose; using ActionResult = Action::Result; + using ThroughAction = nav2_msgs::action::ComputePathThroughPoses; + using ThroughActionResult = Action::Result; public: WouldAPlannerRecoveryHelp( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index 201f4868cf..24a0207e3b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -38,6 +38,11 @@ class RateController : public BT::DecoratorNode const std::string & name, const BT::NodeConfiguration & conf); + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -59,6 +64,7 @@ class RateController : public BT::DecoratorNode std::chrono::time_point start_; double period_; bool first_time_; + bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index 1d2bad9f37..64eb61d788 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -24,7 +24,11 @@ AssistedTeleopAction::AssistedTeleopAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{} + +void AssistedTeleopAction::initialize() { double time_allowance; getInput("time_allowance", time_allowance); @@ -32,10 +36,15 @@ AssistedTeleopAction::AssistedTeleopAction( // Populate the input message goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + initialized_ = true; } void AssistedTeleopAction::on_tick() { + if (!initialized_) { + initialize(); + } + if (is_recovery_) { increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index cb15fc9333..09d79c7e23 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -24,7 +24,12 @@ BackUpAction::BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void nav2_behavior_tree::BackUpAction::initialize() { double dist; getInput("backup_dist", dist); @@ -39,10 +44,15 @@ BackUpAction::BackUpAction( goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + initialized_ = true; } void BackUpAction::on_tick() { + if (!initialized_) { + initialize(); + } + increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index be062357b1..7e0b613f62 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -24,7 +24,12 @@ DriveOnHeadingAction::DriveOnHeadingAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initalized_(false) +{ +} + +void DriveOnHeadingAction::initialize() { double dist; getInput("dist_to_travel", dist); @@ -39,6 +44,14 @@ DriveOnHeadingAction::DriveOnHeadingAction( goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + initalized_ = true; +} + +void DriveOnHeadingAction::on_tick() +{ + if (!initalized_) { + initialize(); + } } BT::NodeStatus DriveOnHeadingAction::on_success() diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 7a5816973d..bf8b95c84f 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -29,7 +29,11 @@ RemovePassedGoals::RemovePassedGoals( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), - viapoint_achieved_radius_(0.5) + viapoint_achieved_radius_(0.5), + initialized_(false) +{} + +void RemovePassedGoals::initialize() { getInput("radius", viapoint_achieved_radius_); @@ -45,6 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick() { setStatus(BT::NodeStatus::RUNNING); + if (!initialized_) { + initialize(); + } + Goals goal_poses; getInput("input_goals", goal_poses); diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index 78fa131175..cab9c8f821 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -23,7 +23,12 @@ SpinAction::SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void SpinAction::initialize() { double dist; getInput("spin_dist", dist); @@ -32,10 +37,16 @@ SpinAction::SpinAction( goal_.target_yaw = dist; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); getInput("is_recovery", is_recovery_); + + initialized_ = true; } void SpinAction::on_tick() { + if (!initialized_) { + initialize(); + } + if (is_recovery_) { increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp index 7f919a30f3..b394a804c7 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -33,12 +33,12 @@ TruncatePath::TruncatePath( : BT::ActionNodeBase(name, conf), distance_(1.0) { - getInput("distance", distance_); } inline BT::NodeStatus TruncatePath::tick() { setStatus(BT::NodeStatus::RUNNING); + getInput("distance", distance_); nav_msgs::msg::Path input_path; diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index f08fed3147..b931707673 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -25,7 +25,12 @@ WaitAction::WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void WaitAction::initialize() { double duration; getInput("wait_duration", duration); @@ -37,10 +42,15 @@ WaitAction::WaitAction( } goal_.time = rclcpp::Duration::from_seconds(duration); + initialized_ = true; } void WaitAction::on_tick() { + if (!initialized_) { + initialize(); + } + increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 5840b8882d..64394ee378 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -30,7 +30,12 @@ DistanceTraveledCondition::DistanceTraveledCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), distance_(1.0), - transform_tolerance_(0.1) + transform_tolerance_(0.1), + initialized_(false) +{ +} + +void DistanceTraveledCondition::initialize() { getInput("distance", distance_); @@ -42,10 +47,15 @@ DistanceTraveledCondition::DistanceTraveledCondition( node_, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); + initialized_ = true; } BT::NodeStatus DistanceTraveledCondition::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { if (!nav2_util::getCurrentPose( start_pose_, *tf_, global_frame_, robot_base_frame_, diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index d81df60a9e..2697680415 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -42,18 +42,6 @@ GoalReachedCondition::~GoalReachedCondition() cleanup(); } -BT::NodeStatus GoalReachedCondition::tick() -{ - if (!initialized_) { - initialize(); - } - - if (isGoalReached()) { - return BT::NodeStatus::SUCCESS; - } - return BT::NodeStatus::FAILURE; -} - void GoalReachedCondition::initialize() { node_ = config().blackboard->get("node"); @@ -69,6 +57,18 @@ void GoalReachedCondition::initialize() initialized_ = true; } +BT::NodeStatus GoalReachedCondition::tick() +{ + if (!initialized_) { + initialize(); + } + + if (isGoalReached()) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + bool GoalReachedCondition::isGoalReached() { geometry_msgs::msg::PoseStamped goal; diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index b8630261e6..9d221cf8c8 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -27,7 +27,12 @@ IsBatteryLowCondition::IsBatteryLowCondition( battery_topic_("/battery_status"), min_battery_(0.0), is_voltage_(false), - is_battery_low_(false) + is_battery_low_(false), + initialized_(false) +{ +} + +void IsBatteryLowCondition::initialize() { getInput("min_battery", min_battery_); getInput("battery_topic", battery_topic_); @@ -45,10 +50,15 @@ IsBatteryLowCondition::IsBatteryLowCondition( rclcpp::SystemDefaultsQoS(), std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), sub_option); + initialized_ = true; } BT::NodeStatus IsBatteryLowCondition::tick() { + if (!initialized_) { + initialize(); + } + callback_group_executor_.spin_some(); if (is_battery_low_) { return BT::NodeStatus::SUCCESS; diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 4a02dede9a..59d593a147 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -23,17 +23,27 @@ namespace nav2_behavior_tree IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf) +: BT::ConditionNode(condition_name, conf), + initialized_(false) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); server_timeout_ = config().blackboard->template get("server_timeout"); +} + +void IsPathValidCondition::initialize() +{ getInput("server_timeout", server_timeout_); + initialized_ = true; } BT::NodeStatus IsPathValidCondition::tick() { + if (!initialized_) { + initialize(); + } + nav_msgs::msg::Path path; getInput("path", path); diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp index e018e02535..1347998602 100644 --- a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -29,13 +29,13 @@ PathExpiringTimerCondition::PathExpiringTimerCondition( period_(1.0), first_time_(true) { - getInput("seconds", period_); node_ = config().blackboard->get("node"); } BT::NodeStatus PathExpiringTimerCondition::tick() { if (first_time_) { + getInput("seconds", period_); getInput("path", prev_path_); first_time_ = false; start_ = node_->now(); diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index 4dc7e58893..64afd1a34b 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -27,15 +27,25 @@ TimeExpiredCondition::TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - period_(1.0) + period_(1.0), + initialized_(false) +{ +} + +void TimeExpiredCondition::initialize() { getInput("seconds", period_); node_ = config().blackboard->get("node"); start_ = node_->now(); + initialized_ = true; } BT::NodeStatus TimeExpiredCondition::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { start_ = node_->now(); return BT::NodeStatus::FAILURE; diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 8daca5afed..33e94178de 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -27,11 +27,20 @@ TransformAvailableCondition::TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - was_found_(false) + was_found_(false), + initialized_(false) { node_ = config().blackboard->get("node"); tf_ = config().blackboard->get>("tf_buffer"); +} +TransformAvailableCondition::~TransformAvailableCondition() +{ + RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node"); +} + +void TransformAvailableCondition::initialize() +{ getInput("child", child_frame_); getInput("parent", parent_frame_); @@ -43,15 +52,15 @@ TransformAvailableCondition::TransformAvailableCondition( } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); -} - -TransformAvailableCondition::~TransformAvailableCondition() -{ - RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node"); + initialized_ = true; } BT::NodeStatus TransformAvailableCondition::tick() { + if (!initialized_) { + initialize(); + } + if (was_found_) { return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp index 6a8d3f33ca..91de9c2e22 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp @@ -26,7 +26,10 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp( error_codes_to_check_ = { ActionResult::UNKNOWN, ActionResult::NO_VALID_PATH, - ActionResult::TIMEOUT + ActionResult::TIMEOUT, + ThroughActionResult::UNKNOWN, + ThroughActionResult::TIMEOUT, + ThroughActionResult::NO_VALID_PATH }; } diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 42fc8076be..44e56dc55e 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -26,11 +26,11 @@ RecoveryNode::RecoveryNode( number_of_retries_(1), retry_count_(0) { - getInput("number_of_retries", number_of_retries_); } BT::NodeStatus RecoveryNode::tick() { + getInput("number_of_retries", number_of_retries_); const unsigned children_count = children_nodes_.size(); if (children_count != 2) { diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index 13e98e0d29..b710ec3009 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -24,15 +24,25 @@ RateController::RateController( const std::string & name, const BT::NodeConfiguration & conf) : BT::DecoratorNode(name, conf), - first_time_(false) + first_time_(false), + initialized_(false) +{ +} + +void RateController::initialize() { double hz = 1.0; getInput("hz", hz); period_ = 1.0 / hz; + initialized_ = true; } BT::NodeStatus RateController::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { // Reset the starting point since we're starting a new iteration of // the rate controller (moving from IDLE to RUNNING) diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 947778da5d..429368ea96 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -25,7 +25,8 @@ namespace nav2_behavior_tree { -BehaviorTreeEngine::BehaviorTreeEngine(const std::vector & plugin_libraries) +BehaviorTreeEngine::BehaviorTreeEngine( + const std::vector & plugin_libraries) { BT::SharedLibrary loader; for (const auto & p : plugin_libraries) { diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index 38033598d0..4bb9905759 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -68,6 +68,9 @@ class AssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index a7af3efef6..40f6f65c88 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "assisted_teleop"); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 6f0d54e7ca..85e3d3764d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -68,6 +68,9 @@ class BackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index 35ab9d2f88..99e6eb1b45 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelBackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "back_up"); 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 12c25d30d6..a283dee33e 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 @@ -173,6 +173,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("node", node_); config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("wait_for_service_timeout", 1000ms); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("on_cancelled_triggered", false); diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index 4351152acf..88173e8420 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -53,6 +53,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -139,6 +142,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -231,6 +237,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 40bbeb1a37..ec619b0120 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -76,6 +76,9 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index ca5bcea666..d726186cd3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -74,6 +74,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 5128391d03..290c9133a5 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelControllerActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "follow_path"); diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index 043f1aedb6..06478df588 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -69,6 +69,9 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index cc2e7f0693..5378123a46 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "drive_on_heading_cancel"); diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 4fa291c4b7..c4fcc57519 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -67,6 +67,9 @@ class FollowPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index bd028827af..5fa2c9aa03 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -70,6 +70,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); std::vector poses; config_->blackboard->set>( diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 13e3a2a2ca..264b775a68 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -68,6 +68,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 122e6d0e0b..2f7a36e7fa 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -53,6 +53,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index d1c930b5e4..c2e4389021 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -67,6 +67,9 @@ class SmoothPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index 56f5cdae74..a6d9f36d57 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -68,6 +68,9 @@ class SpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index d773e88bca..ea3b01b983 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelSpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "spin"); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 2df39599a9..f54250a2b6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -59,6 +59,9 @@ class WaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index e8d4d053c7..0773c72c6d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelWaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "wait"); 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 de4aec7532..cd0a167428 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -76,7 +76,7 @@ class DriveOnHeading : public TimedBehavior command_speed_ = command->speed; command_time_allowance_ = command->time_allowance; - end_time_ = this->steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; if (!nav2_util::getCurrentPose( initial_pose_, *this->tf_, this->local_frame_, this->robot_base_frame_, @@ -95,7 +95,7 @@ class DriveOnHeading : public TimedBehavior */ ResultStatus onCycleUpdate() override { - rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { this->stopRobot(); RCLCPP_WARN( diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 6af6490b61..d01e14179f 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -119,8 +119,8 @@ class TimedBehavior : public nav2_core::Behavior { node_ = parent; auto node = node_.lock(); - logger_ = node->get_logger(); + clock_ = node->get_clock(); RCLCPP_INFO(logger_, "Configuring %s", name.c_str()); @@ -200,7 +200,7 @@ class TimedBehavior : public nav2_core::Behavior rclcpp::Duration elasped_time_{0, 0}; // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + rclcpp::Clock::SharedPtr clock_; // Logger rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")}; @@ -231,11 +231,11 @@ class TimedBehavior : public nav2_core::Behavior return; } - auto start_time = steady_clock_.now(); + auto start_time = clock_->now(); rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { - elasped_time_ = steady_clock_.now() - start_time; + elasped_time_ = clock_->now() - start_time; if (action_server_->is_cancel_requested()) { RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); @@ -252,7 +252,7 @@ class TimedBehavior : public nav2_core::Behavior " however feature is currently not implemented. Aborting and stopping.", behavior_name_.c_str()); stopRobot(); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; onActionCompletion(result); action_server_->terminate_current(result); return; @@ -264,14 +264,14 @@ class TimedBehavior : public nav2_core::Behavior RCLCPP_INFO( logger_, "%s completed successfully", behavior_name_.c_str()); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; onActionCompletion(result); action_server_->succeeded_current(result); return; case Status::FAILED: RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str()); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; result->error_code = on_cycle_update_result.error_code; onActionCompletion(result); action_server_->terminate_current(result); diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 4628010a3d..a0fc972999 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -67,7 +67,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptrtime_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE}; } @@ -82,7 +82,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() feedback_->current_teleop_duration = elasped_time_; action_server_->publish_feedback(feedback_); - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { stopRobot(); RCLCPP_WARN_STREAM( @@ -125,7 +125,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() if (time == simulation_time_step_) { RCLCPP_DEBUG_STREAM_THROTTLE( logger_, - steady_clock_, + *clock_, 1000, behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); scaled_twist.linear.x = 0.0f; @@ -135,7 +135,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() } else { RCLCPP_DEBUG_STREAM_THROTTLE( logger_, - steady_clock_, + *clock_, 1000, behavior_name_.c_str() << " collision approaching in " << time << " seconds"); double scale_factor = time / projection_time_; diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index 1ec9e83264..7a194874c7 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -31,7 +31,7 @@ ResultStatus BackUp::onRun(const std::shared_ptr comma command_speed_ = -std::fabs(command->speed); command_time_allowance_ = command->time_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; if (!nav2_util::getCurrentPose( initial_pose_, *tf_, local_frame_, robot_base_frame_, diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 1e3b07fbf1..55a7872247 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -91,14 +91,14 @@ ResultStatus Spin::onRun(const std::shared_ptr command) cmd_yaw_); command_time_allowance_ = command->time_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE}; } ResultStatus Spin::onCycleUpdate() { - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { stopRobot(); RCLCPP_WARN( diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 2f438c66e0..36cf6384cd 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -45,9 +45,10 @@ def generate_launch_description(): 'smoother_server', 'planner_server', 'behavior_server', + 'velocity_smoother', + 'collision_monitor', 'bt_navigator', 'waypoint_follower', - 'velocity_smoother', ] # Map fully qualified names to relative ones so the node's namespace can be prepended. @@ -198,7 +199,18 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings - + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + + [('cmd_vel', 'cmd_vel_nav')], + ), + Node( + package='nav2_collision_monitor', + executable='collision_monitor', + name='collision_monitor', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, ), Node( package='nav2_lifecycle_manager', @@ -266,7 +278,14 @@ def generate_launch_description(): name='velocity_smoother', parameters=[configured_params], remappings=remappings - + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + + [('cmd_vel', 'cmd_vel_nav')], + ), + ComposableNode( + package='nav2_collision_monitor', + plugin='nav2_collision_monitor::CollisionMonitor', + name='collision_monitor', + parameters=[configured_params], + remappings=remappings, ), ComposableNode( package='nav2_lifecycle_manager', diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 692ca6ec80..baceb0e531 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 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -284,3 +285,34 @@ waypoint_follower: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/robot1/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 2d57aef383..583bb64b56 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 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -283,3 +284,34 @@ waypoint_follower: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/robot2/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 6f7e66cd61..5fb4b9fe63 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 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -345,3 +346,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 112a1de98b..0e3427a17b 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 + wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -121,6 +122,7 @@ controller_server: progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: @@ -197,7 +199,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -254,7 +256,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True # The yaml_filename does not need to be specified since it going to be set by defaults in launch. @@ -275,12 +277,34 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + GridBased: + plugin: "nav2_smac_planner/SmacPlannerLattice" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + debug_visualizations: true + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 smoother_server: ros__parameters: smoother_plugins: ["simple_smoother"] @@ -341,3 +365,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 9257836be8..ebbe3cbbe5 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -13,26 +13,38 @@ - + + + + - + + + + - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 815694d83c..b15672807f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -210,6 +210,14 @@ class Polygon */ bool isPointInside(const Point & point) const; + /** + * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] + * @param poly_string Input String containing the verteceis of the polygon + * @param polygon Output Point vector with all the vertecies of the polygon + * @return True if all parameters were obtained or false in failure case + */ + bool getPolygonFromString(std::string & poly_string, std::vector & polygon); + // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 68b4cd7b50..eb3ee0a873 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -9,7 +9,7 @@ collision_detector: polygons: ["PolygonFront"] PolygonFront: type: "polygon" - points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "none" min_points: 4 visualize: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index fcb01f5482..ab5b08bbb9 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -17,7 +17,7 @@ collision_monitor: polygons: ["PolygonStop"] PolygonStop: type: "polygon" - points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" action_type: "stop" min_points: 4 visualize: True @@ -25,7 +25,7 @@ collision_monitor: enabled: True PolygonSlow: type: "polygon" - points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] + points: "[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]" action_type: "slowdown" min_points: 4 slowdown_ratio: 0.3 @@ -34,7 +34,7 @@ collision_monitor: enabled: True PolygonLimit: type: "polygon" - points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] + points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]" action_type: "limit" min_points: 4 linear_limit: 0.4 diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 481cafbb65..29e3f3031d 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -77,6 +77,20 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) collision_points_marker_pub_ = this->create_publisher( "~/collision_points_marker", 1); + auto node = shared_from_this(); + nav2_util::declare_parameter_if_not_declared( + node, "use_realtime_priority", rclcpp::ParameterValue(false)); + bool use_realtime_priority = false; + node->get_parameter("use_realtime_priority", use_realtime_priority); + if (use_realtime_priority) { + try { + nav2_util::setSoftRealTimePriority(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + } + return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 248bdcec6d..53480a0974 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -22,6 +22,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/array_parser.hpp" #include "nav2_collision_monitor/kinematics.hpp" @@ -401,34 +402,13 @@ bool Polygon::getParameters( try { // Leave it uninitialized: it will throw an inner exception if the parameter is not set nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); - std::vector poly_row = - node->get_parameter(polygon_name_ + ".points").as_double_array(); - // Check for points format correctness - if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { - RCLCPP_ERROR( - logger_, - "[%s]: Polygon has incorrect points description", - polygon_name_.c_str()); - return false; - } - - // Obtain polygon vertices - Point point; - bool first = true; - for (double val : poly_row) { - if (first) { - point.x = val; - } else { - point.y = val; - poly_.push_back(point); - } - first = !first; - } + node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING); + std::string poly_string = + node->get_parameter(polygon_name_ + ".points").as_string(); // Do not need to proceed further, if "points" parameter is defined. // Static polygon will be used. - return true; + return getPolygonFromString(poly_string, poly_); } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, @@ -562,4 +542,44 @@ inline bool Polygon::isPointInside(const Point & point) const return res; } +bool Polygon::getPolygonFromString( + std::string & poly_string, + std::vector & polygon) +{ + std::string error; + std::vector> vvf = nav2_util::parseVVF(poly_string, error); + + if (error != "") { + RCLCPP_ERROR( + logger_, "Error parsing polygon parameter %s: '%s'", + poly_string.c_str(), error.c_str()); + return false; + } + + // Check for minimum 4 points + if (vvf.size() <= 3) { + RCLCPP_ERROR( + logger_, + "Polygon must have at least three points."); + return false; + } + for (unsigned int i = 0; i < vvf.size(); i++) { + if (vvf[i].size() == 2) { + Point point; + point.x = vvf[i][0]; + point.y = vvf[i][1]; + polygon.push_back(point); + } else { + RCLCPP_ERROR( + logger_, + "Points in the polygon specification must be pairs of numbers" + "Found a point with %d numbers.", + static_cast(vvf[i].size())); + return false; + } + } + + return true; +} + } // 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 b0b926fde0..e36ef6e583 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -162,9 +162,9 @@ class Tester : public ::testing::Test nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; // CollisionMonitor collision points markers - rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + rclcpp::Subscription::SharedPtr + collision_points_marker_sub_; visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; - }; // Tester Tester::Tester() @@ -269,8 +269,11 @@ void Tester::addPolygon( cd_->set_parameter( rclcpp::Parameter(polygon_name + ".type", "polygon")); - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; + const std::string points = "[[" + + std::to_string(size) + ", " + std::to_string(size) + "], [" + + std::to_string(size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(size) + "]]"; cd_->declare_parameter( polygon_name + ".points", rclcpp::ParameterValue(points)); cd_->set_parameter( diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 94801a3d2a..757956f3f8 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -195,7 +195,8 @@ class Tester : public ::testing::Test nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; // CollisionMonitor collision points markers - rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + rclcpp::Subscription::SharedPtr + collision_points_marker_sub_; visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; // Service client for setting CollisionMonitor parameters @@ -301,8 +302,11 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".type", "polygon")); if (at != "approach") { - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; + const std::string points = "[[" + + std::to_string(size) + ", " + std::to_string(size) + "], [" + + std::to_string(size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(-size) + "], [" + + std::to_string(-size) + ", " + std::to_string(size) + "]]"; cm_->declare_parameter( polygon_name + ".points", rclcpp::ParameterValue(points)); cm_->set_parameter( diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 2ab46cdced..8d3ad3dc5e 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -50,6 +50,19 @@ static const std::vector SQUARE_POLYGON { 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; static const std::vector ARBITRARY_POLYGON { 1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 2.0, -1.0, -1.0, -1.0, -1.0, 1.0}; +static const char SQUARE_POLYGON_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"}; +static const char ARBITRARY_POLYGON_STR[]{ + "[[1.0, 1.0], [1.0, 0.0], [2.0, 0.0], [2.0, -1.0], [-1.0, -1.0], [-1.0, 1.0]]"}; +static const char INCORRECT_POINTS_1_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5]]" +}; +static const char INCORRECT_POINTS_2_STR[]{ + "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5], [0]]" +}; +static const char INVALID_POINTS_STR[]{ + "[[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5], 0]]" +}; static const double CIRCLE_RADIUS{0.5}; static const int MIN_POINTS{2}; static const double SLOWDOWN_RATIO{0.7}; @@ -212,7 +225,7 @@ class Tester : public ::testing::Test // Working with parameters void setCommonParameters(const std::string & polygon_name, const std::string & action_type); void setPolygonParameters( - const std::vector & points, + const char * points, const bool is_static); void setCircleParameters(const double radius); bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); @@ -311,7 +324,7 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st } void Tester::setPolygonParameters( - const std::vector & points, const bool is_static) + const char * points, const bool is_static) { if (is_static) { test_node_->declare_parameter( @@ -360,7 +373,7 @@ bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const st void Tester::createPolygon(const std::string & action_type, const bool is_static) { setCommonParameters(POLYGON_NAME, action_type); - setPolygonParameters(SQUARE_POLYGON, is_static); + setPolygonParameters(SQUARE_POLYGON_STR, is_static); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -546,7 +559,7 @@ TEST_F(Tester, testPolygonUndeclaredPoints) TEST_F(Tester, testPolygonIncorrectActionType) { setCommonParameters(POLYGON_NAME, "incorrect_action_type"); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -558,12 +571,11 @@ TEST_F(Tester, testPolygonIncorrectPoints1) { setCommonParameters(POLYGON_NAME, "stop"); - std::vector incorrect_points = SQUARE_POLYGON; - incorrect_points.resize(6); // Not enough for triangle + // Triangle points test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INCORRECT_POINTS_1_STR)); test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INCORRECT_POINTS_1_STR)); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -575,12 +587,11 @@ TEST_F(Tester, testPolygonIncorrectPoints2) { setCommonParameters(POLYGON_NAME, "stop"); - std::vector incorrect_points = SQUARE_POLYGON; - incorrect_points.resize(9); // Odd number of points + // Odd number of elements test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points)); + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INCORRECT_POINTS_2_STR)); test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points)); + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INCORRECT_POINTS_2_STR)); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -592,7 +603,7 @@ TEST_F(Tester, testPolygonIncorrectPoints2) TEST_F(Tester, testPolygonMaxPoints) { setCommonParameters(POLYGON_NAME, "stop"); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); const int max_points = 5; test_node_->declare_parameter( @@ -751,7 +762,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) // Test for checking edge cases in raytracing algorithm. // All points are lie on the edge lines parallel to OX, where the raytracing takes place. setCommonParameters(POLYGON_NAME, "stop"); - setPolygonParameters(ARBITRARY_POLYGON, true); + setPolygonParameters(ARBITRARY_POLYGON_STR, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -884,7 +895,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); - setPolygonParameters(SQUARE_POLYGON, true); + setPolygonParameters(SQUARE_POLYGON_STR, true); // Create new polygon polygon_ = std::make_shared( @@ -900,6 +911,22 @@ TEST_F(Tester, testPolygonDefaultVisualize) ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr); } +TEST_F(Tester, testPolygonInvalidPointsString) +{ + setCommonParameters(POLYGON_NAME, "stop"); + + // Invalid points + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(INVALID_POINTS_STR)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", INVALID_POINTS_STR)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index c28dc88cc0..11aa77227e 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -20,6 +20,7 @@ class DictItemReference: + def __init__(self, dictionary, key): self.dictionary = dictionary self.dictKey = key diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 305f4141f3..406ca377c3 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -132,6 +132,7 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat // Smooth plan auto costmap = costmap_sub_->getCostmap(); + std::lock_guard lock(*(costmap->getMutex())); if (!smoother_->smooth(path_world, start_dir, end_dir, costmap.get(), smoother_params_)) { RCLCPP_WARN( logger_, diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 1ccae77f77..6687b3452d 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -82,7 +82,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 544155ebb3..992c8fab04 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -265,6 +265,7 @@ class ControllerServer : public nav2_util::LifecycleNode double min_theta_velocity_threshold_; double failure_tolerance_; + bool use_realtime_priority_; // Whether we've published the single controller warning yet geometry_msgs::msg::PoseStamped end_pose_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 0386c06fc7..76754c5815 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -62,6 +62,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit")); declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); + declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false)); // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( @@ -126,6 +127,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) std::string speed_limit_topic; get_parameter("speed_limit_topic", speed_limit_topic); get_parameter("failure_tolerance", failure_tolerance_); + get_parameter("use_realtime_priority", use_realtime_priority_); costmap_ros_->configure(); // Launch a thread to run the costmap node @@ -221,13 +223,19 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) 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(), - "follow_path", - std::bind(&ControllerServer::computeControl, this), - nullptr, - std::chrono::milliseconds(500), - true, server_options); + // This may throw due to real-time prioritzation if user doesn't have real-time permissions + try { + action_server_ = std::make_unique( + shared_from_this(), + "follow_path", + std::bind(&ControllerServer::computeControl, this), + nullptr, + std::chrono::milliseconds(500), + true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } // Set subscribtion to the speed limiting topic speed_limit_sub_ = create_subscription( @@ -282,11 +290,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -312,11 +316,9 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) goal_checkers_.clear(); progress_checkers_.clear(); - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + + costmap_ros_->cleanup(); + // Release any allocated resources action_server_.reset(); diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index f039cf5d42..df0ddb2dc3 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -38,7 +38,6 @@ include_directories( add_definitions(${EIGEN3_DEFINITIONS}) add_library(nav2_costmap_2d_core SHARED - src/array_parser.cpp src/costmap_2d.cpp src/layer.cpp src/layered_costmap.cpp diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 2074ad5ea6..08b09504e5 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -48,6 +48,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "map_msgs/msg/occupancy_grid_update.hpp" #include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_msgs/srv/get_costmap.hpp" #include "tf2/transform_datatypes.h" #include "nav2_util/lifecycle_node.hpp" @@ -91,6 +92,7 @@ class Costmap2DPublisher costmap_pub_->on_activate(); costmap_update_pub_->on_activate(); costmap_raw_pub_->on_activate(); + costmap_raw_update_pub_->on_activate(); } /** @@ -101,6 +103,7 @@ class Costmap2DPublisher costmap_pub_->on_deactivate(); costmap_update_pub_->on_deactivate(); costmap_raw_pub_->on_deactivate(); + costmap_raw_update_pub_->on_deactivate(); } /** @@ -136,9 +139,16 @@ class Costmap2DPublisher void prepareGrid(); void prepareCostmap(); + /** @brief Prepare OccupancyGridUpdate msg for publication. */ + std::unique_ptr createGridUpdateMsg(); + /** @brief Prepare CostmapUpdate msg for publication. */ + std::unique_ptr createCostmapUpdateMsg(); + /** @brief Publish the latest full costmap to the new subscriber. */ // void onNewSubscription(const ros::SingleSubscriberPublisher& pub); + void updateGridParams(); + /** @brief GetCostmap callback service */ void costmap_service_callback( const std::shared_ptr request_header, @@ -164,12 +174,14 @@ class Costmap2DPublisher // Publisher for raw costmap values as msg::Costmap from layered costmap rclcpp_lifecycle::LifecyclePublisher::SharedPtr costmap_raw_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + costmap_raw_update_pub_; // Service for getting the costmaps rclcpp::Service::SharedPtr costmap_service_; - float grid_resolution; - unsigned int grid_width, grid_height; + float grid_resolution_; + unsigned int grid_width_, grid_height_; std::unique_ptr grid_; std::unique_ptr costmap_raw_; // Translate from 0-255 values in costmap to -1 to 100 values in message. 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 f319e52f33..69f2cfefa1 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 @@ -75,8 +75,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode public: /** * @brief Constructor for the wrapper + * @param options Additional options to control creation of the node. */ - Costmap2DROS(); + explicit Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Constructor for the wrapper, the node will @@ -134,6 +135,28 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief as a child-LifecycleNode : + * Costmap2DROS may be launched by another Lifecycle Node as a composed module + * If composed, its parents will handle the shutdown, which includes this module + */ + void on_rcl_preshutdown() override + { + if (is_lifecycle_follower_) { + // Transitioning handled by parent node + return; + } + + // Else, if this is an independent node, this node needs to handle itself. + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)", + this->get_name()); + + runCleanups(); + + destroyBond(); + } + /** * @brief Subscribes to sensor topics if necessary and starts costmap * updates, can be called to restart the costmap after calls to either @@ -376,11 +399,13 @@ class Costmap2DROS : public nav2_util::LifecycleNode double resolution_{0}; 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 initial_transform_timeout_{0}; ///< The timeout before activation of the node errors + bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node + // Derived parameters bool use_radius_{false}; std::vector unpadded_footprint_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index e4ea745015..4f50614876 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_update.hpp" #include "nav2_util/lifecycle_node.hpp" namespace nav2_costmap_2d @@ -52,25 +53,36 @@ class CostmapSubscriber ~CostmapSubscriber() {} /** - * @brief A Get the costmap from topic + * @brief Get current costmap */ std::shared_ptr getCostmap(); - - /** - * @brief Convert an occ grid message into a costmap object - */ - void toCostmap2D(); /** * @brief Callback for the costmap topic */ void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg); + /** + * @brief Callback for the costmap's update topic + */ + void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg); protected: + bool isCostmapReceived() {return costmap_ != nullptr;} + void processCurrentCostmapMsg(); + + bool haveCostmapParametersChanged(); + bool hasCostmapSizeChanged(); + bool hasCostmapResolutionChanged(); + bool hasCostmapOriginPositionChanged(); + + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Subscription::SharedPtr costmap_update_sub_; + std::shared_ptr costmap_; nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; + std::string topic_name_; - bool costmap_received_{false}; - rclcpp::Subscription::SharedPtr costmap_sub_; + std::mutex costmap_msg_mutex_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; }; } // namespace nav2_costmap_2d 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 99d24240d2..3a2f6a77d2 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 @@ -81,7 +81,7 @@ void morphologyOperation( using ShapeBuffer3x3 = std::array; // NOLINT inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity); -} // namespace imgproc_impl +} // namespace imgproc_impl /** * @brief Perform morphological dilation diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp index 8a5fc206f1..6590a4682e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index aa0e0f13c2..0ce4b008d9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -131,7 +131,7 @@ class InflationLayer : public Layer /** * @brief If clearing operations should be processed on this layer or not */ - virtual bool isClearable() override {return false;} + bool isClearable() override {return false;} /** * @brief Reset this costmap diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e72807da59..8914166a5e 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -49,13 +49,13 @@ namespace nav2_costmap_2d Costmap2D::Costmap2D( unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value) -: size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), +: resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), costmap_(NULL), default_value_(default_value) { access_ = new mutex_t(); // create the costmap - initMaps(size_x_, size_y_); + initMaps(cells_size_x, cells_size_y); resetMaps(); } @@ -102,6 +102,8 @@ void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y) { std::unique_lock lock(*access_); delete[] costmap_; + size_x_ = size_x; + size_y_ = size_y; costmap_ = new unsigned char[size_x * size_y]; } @@ -109,8 +111,6 @@ void Costmap2D::resizeMap( unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y) { - size_x_ = size_x; - size_y_ = size_y; resolution_ = resolution; origin_x_ = origin_x; origin_y_ = origin_y; @@ -166,15 +166,12 @@ bool Costmap2D::copyCostmapWindow( // ROS_ERROR("Cannot window a map that the window bounds don't fit inside of"); return false; } - - size_x_ = upper_right_x - lower_left_x; - size_y_ = upper_right_y - lower_left_y; resolution_ = map.resolution_; origin_x_ = win_origin_x; origin_y_ = win_origin_y; // initialize our various maps and reset markers for inflation - initMaps(size_x_, size_y_); + initMaps(upper_right_x - lower_left_x, upper_right_y - lower_left_y); // copy the window of the static map and the costmap that we're taking copyMapRegion( diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 87881bc7ca..58c7d43d56 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -76,6 +76,8 @@ Costmap2DPublisher::Costmap2DPublisher( custom_qos); costmap_update_pub_ = node->create_publisher( topic_name + "_updates", custom_qos); + costmap_raw_update_pub_ = node->create_publisher( + topic_name + "_raw_updates", custom_qos); // Create a service that will use the callback function to handle requests. costmap_service_ = node->create_service( @@ -115,32 +117,37 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub.publish(grid_); } */ + +void Costmap2DPublisher::updateGridParams() +{ + saved_origin_x_ = costmap_->getOriginX(); + saved_origin_y_ = costmap_->getOriginY(); + grid_resolution_ = costmap_->getResolution(); + grid_width_ = costmap_->getSizeInCellsX(); + grid_height_ = costmap_->getSizeInCellsY(); +} + // prepare grid_ message for publication. void Costmap2DPublisher::prepareGrid() { std::unique_lock lock(*(costmap_->getMutex())); - grid_resolution = costmap_->getResolution(); - grid_width = costmap_->getSizeInCellsX(); - grid_height = costmap_->getSizeInCellsY(); grid_ = std::make_unique(); grid_->header.frame_id = global_frame_; grid_->header.stamp = clock_->now(); - grid_->info.resolution = grid_resolution; + grid_->info.resolution = grid_resolution_; - grid_->info.width = grid_width; - grid_->info.height = grid_height; + grid_->info.width = grid_width_; + grid_->info.height = grid_height_; double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); - grid_->info.origin.position.x = wx - grid_resolution / 2; - grid_->info.origin.position.y = wy - grid_resolution / 2; + grid_->info.origin.position.x = wx - grid_resolution_ / 2; + grid_->info.origin.position.y = wy - grid_resolution_ / 2; grid_->info.origin.position.z = 0.0; grid_->info.origin.orientation.w = 1.0; - saved_origin_x_ = costmap_->getOriginX(); - saved_origin_y_ = costmap_->getOriginY(); grid_->data.resize(grid_->info.width * grid_->info.height); @@ -181,44 +188,74 @@ void Costmap2DPublisher::prepareCostmap() } } -void Costmap2DPublisher::publishCostmap() +std::unique_ptr Costmap2DPublisher::createGridUpdateMsg() +{ + auto update = std::make_unique(); + + update->header.stamp = clock_->now(); + update->header.frame_id = global_frame_; + update->x = x0_; + update->y = y0_; + update->width = xn_ - x0_; + update->height = yn_ - y0_; + update->data.resize(update->width * update->height); + + std::uint32_t i = 0; + for (std::uint32_t y = y0_; y < yn_; y++) { + for (std::uint32_t x = x0_; x < xn_; x++) { + update->data[i++] = cost_translation_table_[costmap_->getCost(x, y)]; + } + } + return update; +} + +std::unique_ptr Costmap2DPublisher::createCostmapUpdateMsg() { - if (costmap_raw_pub_->get_subscription_count() > 0) { - prepareCostmap(); - costmap_raw_pub_->publish(std::move(costmap_raw_)); + auto msg = std::make_unique(); + + msg->header.stamp = clock_->now(); + msg->header.frame_id = global_frame_; + msg->x = x0_; + msg->y = y0_; + msg->size_x = xn_ - x0_; + msg->size_y = yn_ - y0_; + msg->data.resize(msg->size_x * msg->size_y); + + std::uint32_t i = 0; + for (std::uint32_t y = y0_; y < yn_; y++) { + for (std::uint32_t x = x0_; x < xn_; x++) { + msg->data[i++] = costmap_->getCost(x, y); + } } + return msg; +} +void Costmap2DPublisher::publishCostmap() +{ float resolution = costmap_->getResolution(); - if (always_send_full_costmap_ || grid_resolution != resolution || - grid_width != costmap_->getSizeInCellsX() || - grid_height != costmap_->getSizeInCellsY() || + if (always_send_full_costmap_ || grid_resolution_ != resolution || + grid_width_ != costmap_->getSizeInCellsX() || + grid_height_ != costmap_->getSizeInCellsY() || saved_origin_x_ != costmap_->getOriginX() || saved_origin_y_ != costmap_->getOriginY()) { + updateGridParams(); if (costmap_pub_->get_subscription_count() > 0) { prepareGrid(); costmap_pub_->publish(std::move(grid_)); } + if (costmap_raw_pub_->get_subscription_count() > 0) { + prepareCostmap(); + costmap_raw_pub_->publish(std::move(costmap_raw_)); + } } else if (x0_ < xn_) { + // Publish just update msgs + std::unique_lock lock(*(costmap_->getMutex())); if (costmap_update_pub_->get_subscription_count() > 0) { - std::unique_lock lock(*(costmap_->getMutex())); - // Publish Just an Update - auto update = std::make_unique(); - update->header.stamp = rclcpp::Time(); - update->header.frame_id = global_frame_; - update->x = x0_; - update->y = y0_; - update->width = xn_ - x0_; - update->height = yn_ - y0_; - update->data.resize(update->width * update->height); - unsigned int i = 0; - for (unsigned int y = y0_; y < yn_; y++) { - for (unsigned int x = x0_; x < xn_; x++) { - unsigned char cost = costmap_->getCost(x, y); - update->data[i++] = cost_translation_table_[cost]; - } - } - costmap_update_pub_->publish(std::move(update)); + costmap_update_pub_->publish(createGridUpdateMsg()); + } + if (costmap_raw_update_pub_->get_subscription_count() > 0) { + costmap_raw_update_pub_->publish(createCostmapUpdateMsg()); } } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index c35933c908..2a36bab959 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -61,8 +61,8 @@ namespace nav2_costmap_2d Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) : Costmap2DROS(name, "/", name, use_sim_time) {} -Costmap2DROS::Costmap2DROS() -: nav2_util::LifecycleNode("costmap", ""), +Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("costmap", "", options), name_("costmap"), default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, default_types_{ @@ -71,6 +71,7 @@ Costmap2DROS::Costmap2DROS() "nav2_costmap_2d::InflationLayer"} { declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + is_lifecycle_follower_ = false; init(); } @@ -288,7 +289,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) // 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_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; @@ -448,6 +451,18 @@ Costmap2DROS::getParameters() footprint_.c_str(), robot_radius_); } } + + // 4. The width and height of map cannot be negative or 0 (to avoid abnoram memory usage) + if (map_width_meters_ <= 0) { + RCLCPP_ERROR( + get_logger(), "You try to set width of map to be negative or zero," + " this isn't allowed, please give a positive value."); + } + if (map_height_meters_ <= 0) { + RCLCPP_ERROR( + get_logger(), "You try to set height of map to be negative or zero," + " this isn't allowed, please give a positive value."); + } } void @@ -510,7 +525,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - for (auto & layer_pub: layer_publishers_) { + for (auto & layer_pub : layer_publishers_) { layer_pub->updateBounds(x0, xn, y0, yn); } @@ -522,7 +537,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); - for (auto & layer_pub: layer_publishers_) { + for (auto & layer_pub : layer_publishers_) { layer_pub->publishCostmap(); } diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index d56ac942e9..0426b87be3 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -14,22 +14,30 @@ #include #include +#include #include "nav2_costmap_2d/costmap_subscriber.hpp" namespace nav2_costmap_2d { +constexpr int costmapUpdateQueueDepth = 10; + CostmapSubscriber::CostmapSubscriber( const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name) : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); + costmap_update_sub_ = node->create_subscription( + topic_name_ + "_updates", + rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1)); } CostmapSubscriber::CostmapSubscriber( @@ -38,60 +46,119 @@ CostmapSubscriber::CostmapSubscriber( : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); + costmap_update_sub_ = node->create_subscription( + topic_name_ + "_updates", + rclcpp::QoS(rclcpp::KeepLast(costmapUpdateQueueDepth)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1)); } std::shared_ptr CostmapSubscriber::getCostmap() { - if (!costmap_received_) { + if (!isCostmapReceived()) { throw std::runtime_error("Costmap is not available"); } - toCostmap2D(); + if (costmap_msg_) { + processCurrentCostmapMsg(); + } return costmap_; } -void CostmapSubscriber::toCostmap2D() +void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) { - auto current_costmap_msg = std::atomic_load(&costmap_msg_); - - if (costmap_ == nullptr) { - costmap_ = std::make_shared( - current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, - current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x, - current_costmap_msg->metadata.origin.position.y); - } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x || // NOLINT - costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y || - costmap_->getResolution() != current_costmap_msg->metadata.resolution || - costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x || - costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y) { - // Update the size of the costmap - costmap_->resizeMap( - current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, - current_costmap_msg->metadata.resolution, - current_costmap_msg->metadata.origin.position.x, - current_costmap_msg->metadata.origin.position.y); + std::lock_guard lock(costmap_msg_mutex_); + costmap_msg_ = msg; } + if (!isCostmapReceived()) { + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); - unsigned char * master_array = costmap_->getCharMap(); - unsigned int index = 0; - for (unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i) { - for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) { - master_array[index] = current_costmap_msg->data[index]; - ++index; + processCurrentCostmapMsg(); + } +} + +void CostmapSubscriber::costmapUpdateCallback( + const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg) +{ + if (isCostmapReceived()) { + if (costmap_msg_) { + processCurrentCostmapMsg(); + } + + std::lock_guard lock(*(costmap_->getMutex())); + + auto map_cell_size_x = costmap_->getSizeInCellsX(); + auto map_call_size_y = costmap_->getSizeInCellsY(); + + if (map_cell_size_x < update_msg->x + update_msg->size_x || + map_call_size_y < update_msg->y + update_msg->size_y) + { + RCLCPP_WARN( + logger_, "Update area outside of original map area. Costmap bounds: %d X %d, " + "Update origin: %d, %d bounds: %d X %d", map_cell_size_x, map_call_size_y, + update_msg->x, update_msg->y, update_msg->size_x, update_msg->size_y); + return; + } + unsigned char * master_array = costmap_->getCharMap(); + // copy update msg row-wise + for (size_t y = 0; y < update_msg->size_y; ++y) { + auto starting_index_of_row_update_in_costmap = (y + update_msg->y) * map_cell_size_x + + update_msg->x; + + std::copy_n( + update_msg->data.begin() + (y * update_msg->size_x), + update_msg->size_x, &master_array[starting_index_of_row_update_in_costmap]); } + } else { + RCLCPP_WARN(logger_, "No costmap received."); } } -void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) +void CostmapSubscriber::processCurrentCostmapMsg() { - std::atomic_store(&costmap_msg_, msg); - if (!costmap_received_) { - costmap_received_ = true; + std::scoped_lock lock(*(costmap_->getMutex()), costmap_msg_mutex_); + if (haveCostmapParametersChanged()) { + costmap_->resizeMap( + costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y, + costmap_msg_->metadata.resolution, + costmap_msg_->metadata.origin.position.x, + costmap_msg_->metadata.origin.position.y); } + + unsigned char * master_array = costmap_->getCharMap(); + std::copy(costmap_msg_->data.begin(), costmap_msg_->data.end(), master_array); + costmap_msg_.reset(); +} + +bool CostmapSubscriber::haveCostmapParametersChanged() +{ + return hasCostmapSizeChanged() || + hasCostmapResolutionChanged() || + hasCostmapOriginPositionChanged(); +} + +bool CostmapSubscriber::hasCostmapSizeChanged() +{ + return costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x || + costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y; +} + +bool CostmapSubscriber::hasCostmapResolutionChanged() +{ + return costmap_->getResolution() != costmap_msg_->metadata.resolution; +} + +bool CostmapSubscriber::hasCostmapOriginPositionChanged() +{ + return costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x || + costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y; } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp index c925973038..75de1feb91 100644 --- a/nav2_costmap_2d/src/footprint.cpp +++ b/nav2_costmap_2d/src/footprint.cpp @@ -34,7 +34,7 @@ #include #include "geometry_msgs/msg/point32.hpp" -#include "nav2_costmap_2d/array_parser.hpp" +#include "nav2_util/array_parser.hpp" #include "nav2_costmap_2d/costmap_math.hpp" namespace nav2_costmap_2d @@ -177,7 +177,7 @@ bool makeFootprintFromString( std::vector & footprint) { std::string error; - std::vector> vvf = parseVVF(footprint_string, error); + std::vector> vvf = nav2_util::parseVVF(footprint_string, error); if (error != "") { RCLCPP_ERROR( diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 0768972a0b..868b5081a1 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -76,6 +76,17 @@ target_link_libraries(test_costmap_publisher_exec layers ) +ament_add_gtest_executable(test_costmap_subscriber_exec +test_costmap_subscriber.cpp +) +ament_target_dependencies(test_costmap_subscriber_exec + ${dependencies} +) +target_link_libraries(test_costmap_subscriber_exec + nav2_costmap_2d_core + nav2_costmap_2d_client +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -136,6 +147,16 @@ ament_add_test(test_costmap_publisher_exec TEST_EXECUTABLE=$ ) +ament_add_test(test_costmap_subscriber_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/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 790466f38b..68a9875ef6 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -162,7 +162,7 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test) auto costmap_raw = future.get(); // Check first 20 cells of the 10by10 map - ASSERT_TRUE(costmap_raw->data.size() == 100); + ASSERT_EQ(costmap_raw->data.size(), 100u); unsigned int i = 0; for (; i < 7; ++i) { EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE); diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp new file mode 100644 index 0000000000..d674ee7669 --- /dev/null +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2020 Samsung Research +// +// 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 "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; +class TestCostmapSubscriberShould : public ::testing::Test +{ +public: + TestCostmapSubscriberShould() + : topicName("/costmap"), node(nav2_util::LifecycleNode::make_shared("test_subscriber")) + { + dummyCostmapMsgSubscriber = node->create_subscription( + topicName + "_raw", 10, + std::bind(&TestCostmapSubscriberShould::costmapCallback, this, std::placeholders::_1)); + + dummyCostmapUpdateMsgSubscriber = + node->create_subscription( + topicName + "_raw_updates", 10, std::bind( + &TestCostmapSubscriberShould::costmapUpdateCallback, this, + std::placeholders::_1)); + } + + void SetUp() override + { + fullCostmapMsgCount = 0; + updateCostmapMsgCount = 0; + + costmapSubscriber = + std::make_unique(node, topicName + "_raw"); + + costmapToSend = std::make_shared(10, 10, 1.0, 0.0, 0.0); + } + + void TearDown() + { + costmapSubscriber.reset(); + costmapToSend.reset(); + } + + void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr) + { + this->fullCostmapMsgCount++; + } + void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr) + { + this->updateCostmapMsgCount++; + } + + struct CostmapObservation + { + std::uint32_t x; + std::uint32_t y; + std::uint8_t cost; + }; + + struct MapChange + { + std::vector observations; + std::uint32_t x0; + std::uint32_t xn; + std::uint32_t y0; + std::uint32_t yn; + }; + +/* *INDENT-OFF* */ + const std::vector mapChanges {{{{2, 2, 255}, {2, 3, 255}, {3, 2, 255}}, 2, 4, 2, 4}, + {{{7, 7, 255}, {7, 8, 255}}, 7, 8, 7, 9}, + {{{2, 2, 0}, {2, 3, 0}, {3, 2, 0}}, 2, 4, 2, 4}}; +/* *INDENT-ON* */ + +protected: + std::vector getCurrentCharMapFromSubscriber() + { + auto currentSubscriberCostmap = costmapSubscriber->getCostmap(); + return + std::vector( + currentSubscriberCostmap->getCharMap(), + currentSubscriberCostmap->getCharMap() + currentSubscriberCostmap->getSizeInCellsX() * + currentSubscriberCostmap->getSizeInCellsX()); + } + + std::vector getCurrentCharMapToSend() + { + return std::vector( + costmapToSend->getCharMap(), + costmapToSend->getCharMap() + costmapToSend->getSizeInCellsX() * + costmapToSend->getSizeInCellsX()); + } + + int fullCostmapMsgCount; + int updateCostmapMsgCount; + std::string topicName; + + nav2_util::LifecycleNode::SharedPtr node; + rclcpp::Logger logger {rclcpp::get_logger("test_costmap_subscriber_should")}; + + std::unique_ptr costmapSubscriber; + std::shared_ptr costmapToSend; + rclcpp::Subscription::SharedPtr dummyCostmapMsgSubscriber; + rclcpp::Subscription::SharedPtr dummyCostmapUpdateMsgSubscriber; +}; + +TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) +{ + bool always_send_full_costmap = true; + + std::vector> expectedCostmaps; + std::vector> recievedCostmaps; + + auto costmapPublisher = std::make_shared( + node, costmapToSend.get(), "", topicName, always_send_full_costmap); + costmapPublisher->on_activate(); + + for (const auto & mapChange : mapChanges) { + for (const auto & observation : mapChange.observations) { + costmapToSend->setCost(observation.x, observation.y, observation.cost); + } + + expectedCostmaps.emplace_back(getCurrentCharMapToSend()); + + costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); + costmapPublisher->publishCostmap(); + + rclcpp::spin_some(node->get_node_base_interface()); + + recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + } + + ASSERT_EQ(fullCostmapMsgCount, mapChanges.size()); + ASSERT_EQ(updateCostmapMsgCount, 0); + + ASSERT_EQ(expectedCostmaps, recievedCostmaps); + + costmapPublisher->on_deactivate(); +} + +TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) +{ + bool always_send_full_costmap = false; + + std::vector> expectedCostmaps; + std::vector> recievedCostmaps; + + auto costmapPublisher = std::make_shared( + node, costmapToSend.get(), "", topicName, always_send_full_costmap); + costmapPublisher->on_activate(); + + for (const auto & mapChange : mapChanges) { + for (const auto & observation : mapChange.observations) { + costmapToSend->setCost(observation.x, observation.y, observation.cost); + } + + expectedCostmaps.emplace_back(getCurrentCharMapToSend()); + + costmapPublisher->updateBounds(mapChange.x0, mapChange.xn, mapChange.y0, mapChange.yn); + costmapPublisher->publishCostmap(); + + rclcpp::spin_some(node->get_node_base_interface()); + + recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + } + + ASSERT_EQ(fullCostmapMsgCount, 1); + ASSERT_EQ(updateCostmapMsgCount, mapChanges.size() - 1); + + ASSERT_EQ(expectedCostmaps, recievedCostmaps); + + costmapPublisher->on_deactivate(); +} + +TEST_F( + TestCostmapSubscriberShould, + throwExceptionIfGetCostmapMethodIsCalledBeforeAnyCostmapMsgReceived) +{ + ASSERT_ANY_THROW(costmapSubscriber->getCostmap()); +} diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 466fd015c8..ec6748f980 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -63,7 +63,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_costmap_2d/test/regression/order_layer.hpp b/nav2_costmap_2d/test/regression/order_layer.hpp index 35bb2aeabb..01955f0ba9 100644 --- a/nav2_costmap_2d/test/regression/order_layer.hpp +++ b/nav2_costmap_2d/test/regression/order_layer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ -#define NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ +#ifndef REGRESSION__ORDER_LAYER_HPP_ +#define REGRESSION__ORDER_LAYER_HPP_ #include "nav2_costmap_2d/layer.hpp" @@ -43,4 +43,4 @@ class OrderLayer : public nav2_costmap_2d::Layer } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ +#endif // REGRESSION__ORDER_LAYER_HPP_ diff --git a/nav2_costmap_2d/test/regression/plugin_api_order.cpp b/nav2_costmap_2d/test/regression/plugin_api_order.cpp index c7baf34453..f7a00e90d2 100644 --- a/nav2_costmap_2d/test/regression/plugin_api_order.cpp +++ b/nav2_costmap_2d/test/regression/plugin_api_order.cpp @@ -16,8 +16,8 @@ #include #include -#include -#include +#include "gtest/gtest.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" TEST(CostmapPluginsTester, checkPluginAPIOrder) { diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index b9480fdffa..e69501277e 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -1,8 +1,3 @@ -ament_add_gtest(array_parser_test array_parser_test.cpp) -target_link_libraries(array_parser_test - nav2_costmap_2d_core -) - ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test nav2_costmap_2d_core diff --git a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp index 37c277f900..558fb26361 100644 --- a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp +++ b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp @@ -102,9 +102,9 @@ class DenoiseLayerTester : public ::testing::Test nav2_costmap_2d::DenoiseLayer denoise_; }; -} +} // namespace nav2_costmap_2d -using namespace nav2_costmap_2d; +using namespace nav2_costmap_2d; // NOLINT TEST_F(DenoiseLayerTester, removeSinglePixels4way) { const auto in = imageFromString( diff --git a/nav2_costmap_2d/test/unit/image_processing_test.cpp b/nav2_costmap_2d/test/unit/image_processing_test.cpp index cc177d699a..f56ca58149 100644 --- a/nav2_costmap_2d/test/unit/image_processing_test.cpp +++ b/nav2_costmap_2d/test/unit/image_processing_test.cpp @@ -18,8 +18,8 @@ #include "nav2_costmap_2d/denoise/image_processing.hpp" #include "image_tests_helper.hpp" -using namespace nav2_costmap_2d; -using namespace imgproc_impl; +using namespace nav2_costmap_2d; // NOLINT +using namespace imgproc_impl; // NOLINT struct ImageProcTester : public ::testing::Test { diff --git a/nav2_costmap_2d/test/unit/image_test.cpp b/nav2_costmap_2d/test/unit/image_test.cpp index 83912d0622..7ab796e155 100644 --- a/nav2_costmap_2d/test/unit/image_test.cpp +++ b/nav2_costmap_2d/test/unit/image_test.cpp @@ -17,7 +17,7 @@ #include "nav2_costmap_2d/denoise/image.hpp" #include "image_tests_helper.hpp" -using namespace nav2_costmap_2d; +using namespace nav2_costmap_2d; // NOLINT struct ImageTester : public ::testing::Test { diff --git a/nav2_costmap_2d/test/unit/image_tests_helper.hpp b/nav2_costmap_2d/test/unit/image_tests_helper.hpp index 3eee735f7e..2ffe8196ba 100644 --- a/nav2_costmap_2d/test/unit/image_tests_helper.hpp +++ b/nav2_costmap_2d/test/unit/image_tests_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ -#define NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ +#ifndef UNIT__IMAGE_TESTS_HELPER_HPP_ +#define UNIT__IMAGE_TESTS_HELPER_HPP_ #include "nav2_costmap_2d/denoise/image.hpp" #include @@ -121,4 +121,4 @@ std::ostream & operator<<(std::ostream & out, const Image & image) } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ +#endif // UNIT__IMAGE_TESTS_HELPER_HPP_ diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp index 8a657d81ad..e54264ebea 100644 --- a/nav2_costmap_2d/test/unit/lifecycle_test.cpp +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -1,3 +1,5 @@ +// Copyright (c) 2020 Samsung Research +// // 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 @@ -8,7 +10,7 @@ // 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. +// limitations under the License. Reserved. #include @@ -22,7 +24,7 @@ TEST(LifecylceTest, CheckInitialTfTimeout) { rclcpp::init(0, nullptr); - auto costmap = std::make_shared("test_costmap"); + auto costmap = std::make_shared(rclcpp::NodeOptions()); costmap->set_parameter({"initial_transform_timeout", 0.0}); std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index ed3cabc3d0..bfed0c2ec5 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -35,12 +35,13 @@ #ifndef DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_ #define DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_ +#include #include #include #include -#include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" namespace dwb_plugins { @@ -75,6 +76,36 @@ struct KinematicParameters inline double getMinSpeedXY_SQ() {return min_speed_xy_sq_;} inline double getMaxSpeedXY_SQ() {return max_speed_xy_sq_;} + /** + * @brief Check to see whether the combined x/y/theta velocities are valid + * @return True if the magnitude hypot(x,y) and theta are within the robot's + * absolute limits + * + * This is based on three parameters: min_speed_xy, max_speed_xy and + * min_speed_theta. The speed is valid if 1) The combined magnitude hypot(x,y) + * is less than max_speed_xy (or max_speed_xy is negative) AND 2) min_speed_xy + * is negative or min_speed_theta is negative or hypot(x,y) is greater than + * min_speed_xy or fabs(theta) is greater than min_speed_theta. + */ + inline bool isValidSpeed(double x, double y, double theta) + { + double vmag_sq = x * x + y * y; + if (max_speed_xy_ >= 0.0 && + vmag_sq > max_speed_xy_sq_ + std::numeric_limits::epsilon()) + { + return false; + } + if ( + min_speed_xy_ >= 0.0 && vmag_sq + std::numeric_limits::epsilon() < min_speed_xy_sq_ && + min_speed_theta_ >= 0.0 && + fabs(theta) + std::numeric_limits::epsilon() < min_speed_theta_) + { + return false; + } + if (vmag_sq == 0.0 && theta == 0.0) {return false;} + return true; + } + protected: // For parameter descriptions, see cfg/KinematicParams.cfg double min_vel_x_{0}; @@ -127,8 +158,8 @@ class KinematicsHandler * @brief Callback executed when a paramter change is detected * @param parameters list of changed parameters */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); void update_kinematics(KinematicParameters kinematics); std::string plugin_name_; }; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp index 88c18046ed..68ac9405c2 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp @@ -58,18 +58,6 @@ class XYThetaIterator : public VelocityIterator nav_2d_msgs::msg::Twist2D nextTwist() override; protected: - /** - * @brief Check to see whether the combined x/y/theta velocities are valid - * @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits - * - * This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta. - * The speed is valid if - * 1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative) - * AND - * 2) min_speed_xy is negative or min_speed_theta is negative or - * hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta. - */ - bool isValidSpeed(double x, double y, double theta); virtual bool isValidVelocity(); void iterateToValidVelocity(); int vx_samples_, vy_samples_, vtheta_samples_; diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp index 50b19aa337..5ca1992c6b 100644 --- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp @@ -41,8 +41,6 @@ #include "nav_2d_utils/parameters.hpp" #include "nav2_util/node_utils.hpp" -#define EPSILON 1E-5 - namespace dwb_plugins { void XYThetaIterator::initialize( @@ -92,29 +90,10 @@ void XYThetaIterator::startNewIteration( } } -bool XYThetaIterator::isValidSpeed(double x, double y, double theta) -{ - KinematicParameters kinematics = kinematics_handler_->getKinematics(); - double vmag_sq = x * x + y * y; - if (kinematics.getMaxSpeedXY() >= 0.0 && vmag_sq > kinematics.getMaxSpeedXY_SQ() + EPSILON) { - return false; - } - if (kinematics.getMinSpeedXY() >= 0.0 && vmag_sq + EPSILON < kinematics.getMinSpeedXY_SQ() && - kinematics.getMinSpeedTheta() >= 0.0 && fabs(theta) + EPSILON < kinematics.getMinSpeedTheta()) - { - return false; - } - if (vmag_sq == 0.0 && th_it_->getVelocity() == 0.0) { - return false; - } - return true; -} - bool XYThetaIterator::isValidVelocity() { - return isValidSpeed( - x_it_->getVelocity(), y_it_->getVelocity(), - th_it_->getVelocity()); + return kinematics_handler_->getKinematics().isValidSpeed( + x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity()); } bool XYThetaIterator::hasMoreTwists() diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 13c314205b..fd7ad80214 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -104,9 +104,10 @@ This process is then repeated a number of times and returns a converged solution | critical_weight | double | Default 20.0. Weight to apply to critic for near collisions closer than `collision_margin_distance` to prevent near collisions **only** as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from critical collisions. | | repulsion_weight | double | Default 1.5. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the `inflation_radius` distance from obstacles. | | cost_power | int | Default 1. Power order to apply to term. | - | collision_cost | double | Default 10000.0. Cost to apply to a true collision in a trajectory. | + | collision_cost | double | Default 100000.0. Cost to apply to a true collision in a trajectory. | | collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. | | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | #### Path Align Critic | Parameter | Type | Definition | @@ -124,11 +125,11 @@ Note: There is a "Legacy" version of this critic also available with the same pa #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | cost_weight | double | Default 2.2. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered | | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | - | max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered | + | max_angle_to_furthest | double | Default 0.785398. Angular distance between robot and goal above which path angle cost starts being considered | | mode | int | Default 0 (Forward Preference). Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires. 0: Forward Preference, penalizes high path angles relative to the robot's orientation to incentivize turning towards the path. 1: No directional preference, penalizes high path angles relative to the robot's orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable. 2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path's requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction. | diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index fd6eeab012..358994c238 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -16,9 +16,10 @@ #define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_ #include +#include + #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" - #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" @@ -96,6 +97,7 @@ class ObstaclesCritic : public CriticFunction unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; + std::string inflation_layer_name_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index b8d6c89870..ecb22b295b 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -25,9 +26,10 @@ void ObstaclesCritic::initialize() getParam(power_, "cost_power", 1); getParam(repulsion_weight_, "repulsion_weight", 1.5); getParam(critical_weight_, "critical_weight", 20.0); - getParam(collision_cost_, "collision_cost", 10000.0); + getParam(collision_cost_, "collision_cost", 100000.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.10); getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); @@ -68,7 +70,10 @@ float ObstaclesCritic::findCircumscribedCost( ++layer) { auto inflation_layer = std::dynamic_pointer_cast(*layer); - if (!inflation_layer) { + if (!inflation_layer || + (!inflation_layer_name_.empty() && + inflation_layer->getName() != inflation_layer_name_)) + { continue; } @@ -160,8 +165,11 @@ void ObstaclesCritic::score(CriticData & data) // Let near-collision trajectory points be punished severely if (dist_to_obj < collision_margin_distance_) { traj_cost += (collision_margin_distance_ - dist_to_obj); - } else if (!near_goal) { // Generally prefer trajectories further from obstacles - repulsive_cost[i] += (inflation_radius_ - dist_to_obj); + } + + // Generally prefer trajectories further from obstacles + if (!near_goal) { + repulsive_cost[i] += inflation_radius_ - dist_to_obj; } } @@ -169,9 +177,14 @@ void ObstaclesCritic::score(CriticData & data) raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost; } + // Normalize repulsive cost by trajectory length & lowest score to not overweight importance + // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors + auto && repulsive_cost_normalized = + (repulsive_cost - xt::amin(repulsive_cost, immediate)) / traj_len; + data.costs += xt::pow( (critical_weight_ * raw_cost) + - (repulsion_weight_ * repulsive_cost / traj_len), + (repulsion_weight_ * repulsive_cost_normalized), power_); data.fail_flag = all_trajectories_collide; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index cd701ea093..d334b81673 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -36,13 +36,13 @@ void PathAngleCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(offset_from_furthest_, "offset_from_furthest", 4); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 2.0); + getParam(weight_, "cost_weight", 2.2); getParam( threshold_to_consider_, "threshold_to_consider", 0.5); getParam( max_angle_to_furthest_, - "max_angle_to_furthest", 1.2); + "max_angle_to_furthest", 0.785398); int mode = 0; getParam(mode, "mode", mode); diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3b053f4577..3c5d203289 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -123,6 +123,8 @@ void Optimizer::reset() control_history_[2] = {0.0, 0.0, 0.0}; control_history_[3] = {0.0, 0.0, 0.0}; + settings_.constraints = settings_.base_constraints; + costs_ = xt::zeros({settings_.batch_size}); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index f88249aed1..08f50945fd 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -282,7 +282,7 @@ TEST(CriticTests, PathAngleCritic) path.y(6) = 4.0; critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); - EXPECT_NEAR(costs(0), 3.6315, 1e-2); // atan2(4,-1) [1.81] * 2.0 weight + EXPECT_NEAR(costs(0), 3.9947, 1e-2); // atan2(4,-1) [1.81] * 2.2 weight // Set mode to no directional preferences + reset costs critic.setMode(1); @@ -306,7 +306,7 @@ TEST(CriticTests, PathAngleCritic) critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); // should use reverse orientation as the closer angle in no dir preference mode - EXPECT_NEAR(costs(0), 2.6516, 1e-2); + EXPECT_NEAR(costs(0), 2.9167, 1e-2); // Set mode to consider path directionality + reset costs critic.setMode(2); @@ -330,7 +330,7 @@ TEST(CriticTests, PathAngleCritic) critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); // should use reverse orientation as the closer angle in no dir preference mode - EXPECT_NEAR(costs(0), 2.6516, 1e-2); + EXPECT_NEAR(costs(0), 2.9167, 1e-2); PathAngleMode mode; mode = PathAngleMode::FORWARD_PREFERENCE; diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index e588ff0fa5..206ae86322 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/CollisionDetectorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" + "msg/CostmapUpdate.msg" "msg/CostmapFilterInfo.msg" "msg/SpeedLimit.msg" "msg/VoxelGrid.msg" diff --git a/nav2_msgs/msg/CostmapUpdate.msg b/nav2_msgs/msg/CostmapUpdate.msg new file mode 100644 index 0000000000..3c865d326b --- /dev/null +++ b/nav2_msgs/msg/CostmapUpdate.msg @@ -0,0 +1,11 @@ +# Update msg for Costmap containing the modified part of Costmap +std_msgs/Header header + +uint32 x +uint32 y + +uint32 size_x +uint32 size_y + +# The cost data, in row-major order, starting with (x,y) from 0-255 in Costmap format rather than OccupancyGrid 0-100. +uint8[] data diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index c65119698a..2ee8d69f53 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -147,12 +147,6 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( std::to_string(goal.pose.position.y) + ") was outside bounds"); } - if (costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was in lethal cost"); - } - if (tolerance_ == 0 && costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::GoalOccupied( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c54b9ae829..7d2c9d03f0 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -39,6 +39,7 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_core/planner_exceptions.hpp" namespace nav2_planner @@ -241,9 +242,6 @@ class PlannerServer : public nav2_util::LifecycleNode double max_planner_duration_; std::string planner_ids_concat_; - // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; - // TF buffer std::shared_ptr tf_; @@ -251,6 +249,8 @@ class PlannerServer : public nav2_util::LifecycleNode std::shared_ptr costmap_ros_; std::unique_ptr costmap_thread_; nav2_costmap_2d::Costmap2D * costmap_; + std::unique_ptr> + collision_checker_; // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 84edc6e4df..9a7b7912ae 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,7 +52,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); - declare_parameter("action_server_result_timeout", 10.0); get_parameter("planner_plugins", planner_ids_); @@ -86,6 +85,12 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); + if (!costmap_ros_->getUseRadius()) { + collision_checker_ = + std::make_unique>( + costmap_); + } + // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); @@ -218,11 +223,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -247,15 +248,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) plan_publisher_.reset(); tf_.reset(); - /* - * Double check whether something else transitioned it to INACTIVE - * already, e.g. the rcl preshutdown callback. - */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -373,7 +366,7 @@ void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = steady_clock_.now(); + auto start_time = this->now(); // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); @@ -433,7 +426,7 @@ void PlannerServer::computePlanThroughPoses() result->path = concat_path; publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; + auto cycle_duration = this->now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { @@ -492,7 +485,7 @@ PlannerServer::computePlan() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = steady_clock_.now(); + auto start_time = this->now(); // Initialize the ComputePathToPose goal and result auto goal = action_server_pose_->get_current_goal(); @@ -529,7 +522,7 @@ PlannerServer::computePlan() // Publish the plan for visualization purposes publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; + auto cycle_duration = this->now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { @@ -651,21 +644,40 @@ void PlannerServer::isPathValid( /** * The lethal check starts at the closest point to avoid points that have already been passed - * and may have become occupied + * and may have become occupied. The method for collision detection is based on the shape of + * the footprint. */ std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; unsigned int my = 0; + + bool use_radius = costmap_ros_->getUseRadius(); + + unsigned int cost = nav2_costmap_2d::FREE_SPACE; for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { - costmap_->worldToMap( - request->path.poses[i].pose.position.x, - request->path.poses[i].pose.position.y, mx, my); - unsigned int cost = costmap_->getCost(mx, my); + auto & position = request->path.poses[i].pose.position; + if (use_radius) { + if (costmap_->worldToMap(position.x, position.y, mx, my)) { + cost = costmap_->getCost(mx, my); + } else { + cost = nav2_costmap_2d::LETHAL_OBSTACLE; + } + } else { + nav2_costmap_2d::Footprint footprint = costmap_ros_->getRobotFootprint(); + auto theta = tf2::getYaw(request->path.poses[i].pose.orientation); + cost = static_cast(collision_checker_->footprintCostAtPose( + position.x, position.y, theta, footprint)); + } - if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + if (use_radius && + (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) { response->is_valid = false; + break; + } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) { + response->is_valid = false; + break; } } } diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 8c332d4969..76837754e8 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -419,13 +419,30 @@ double RegulatedPurePursuitController::findVelocitySignChange( /* Checking for the existance of cusp, in the path, using the dot product and determine it's distance from the robot. If there is no cusp in the path, then just determine the distance to the goal location. */ - if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) { + const double dot_prod = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_prod < 0.0) { // returning the distance if there is a cusp // The transformed path is in the robots frame, so robot is at the origin return hypot( transformed_plan.poses[pose_id].pose.position.x, transformed_plan.poses[pose_id].pose.position.y); } + + if ( + (hypot(oa_x, oa_y) == 0.0 && + transformed_plan.poses[pose_id - 1].pose.orientation != + transformed_plan.poses[pose_id].pose.orientation) + || + (hypot(ab_x, ab_y) == 0.0 && + transformed_plan.poses[pose_id].pose.orientation != + transformed_plan.poses[pose_id + 1].pose.orientation)) + { + // returning the distance since the points overlap + // but are not simply duplicate points (e.g. in place rotation) + return hypot( + transformed_plan.poses[pose_id].pose.position.x, + transformed_plan.poses[pose_id].pose.position.y); + } } return std::numeric_limits::max(); diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 3de0d3c9f5..b4d0a8ed01 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -25,6 +25,7 @@ rviz_rendering std_msgs tf2_geometry_msgs + urdf visualization_msgs yaml_cpp_vendor diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py index 1ae7e808b6..ad4014522a 100644 --- a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -37,10 +37,14 @@ def __init__(self, occupancy_map): """ Initialize costmap2D. - Args: + Args ---- occupancy_map (OccupancyGrid): 2D OccupancyGrid Map + Returns + ------- + None + """ self.size_x = occupancy_map.info.width self.size_y = occupancy_map.info.height diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index a8ded664cd..5f9e355a2f 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -23,7 +23,7 @@ from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import AssistedTeleop, BackUp, Spin +from nav2_msgs.action import AssistedTeleop, BackUp, DriveOnHeading, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose from nav2_msgs.action import ( FollowGPSWaypoints, @@ -51,6 +51,7 @@ class TaskResult(Enum): class BasicNavigator(Node): + def __init__(self, node_name='basic_navigator', namespace=''): super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() @@ -88,6 +89,9 @@ def __init__(self, node_name='basic_navigator', namespace=''): self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') + self.drive_on_heading_client = ActionClient( + self, DriveOnHeading, 'drive_on_heading' + ) self.assisted_teleop_client = ActionClient( self, AssistedTeleop, 'assisted_teleop' ) @@ -127,6 +131,7 @@ def destroy_node(self): self.smoother_client.destroy() self.spin_client.destroy() self.backup_client.destroy() + self.drive_on_heading_client.destroy() super().destroy_node() def setInitialPose(self, initial_pose): @@ -288,6 +293,29 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True + def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): + self.debug("Waiting for 'DriveOnHeading' action server") + while not self.backup_client.wait_for_server(timeout_sec=1.0): + self.info("'DriveOnHeading' action server not available, waiting...") + goal_msg = DriveOnHeading.Goal() + goal_msg.target = Point(x=float(dist)) + goal_msg.speed = speed + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Drive {goal_msg.target.x} m on heading at {goal_msg.speed} m/s....') + send_goal_future = self.drive_on_heading_client.send_goal_async( + goal_msg, self._feedbackCallback + ) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Drive On Heading request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def assistedTeleop(self, time_allowance=30): self.debug("Wainting for 'assisted_teleop' action server") while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0): diff --git a/nav2_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py index 070a7bc6e4..8ffc4b653b 100644 --- a/nav2_simple_commander/test/test_footprint_collision_checker.py +++ b/nav2_simple_commander/test/test_footprint_collision_checker.py @@ -23,6 +23,7 @@ class TestFootprintCollisionChecker(unittest.TestCase): + def test_no_costmap(self): # Test if a type error raised when costmap is not specified yet fcc_ = FootprintCollisionChecker() diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py index 1360c183e7..8bfb91588b 100644 --- a/nav2_simple_commander/test/test_line_iterator.py +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -19,6 +19,7 @@ class TestLineIterator(unittest.TestCase): + def test_type_error(self): # Test if a type error raised when passing invalid arguements types self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 5f437ce546..5eee580bc8 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -126,7 +126,7 @@ planner_server: cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. - debug_visualizations: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. smoother: max_iterations: 1000 w_smooth: 0.3 diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 31dae78968..e4ecf38f03 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -72,8 +72,7 @@ class AStarAlgorithm typedef std::priority_queue, NodeComparator> NodeQueue; /** - * @brief A constructor for nav2_smac_planner::PlannerServer - * @param neighborhood The type of neighborhood to use for search (4 or 8 connected) + * @brief A constructor for nav2_smac_planner::AStarAlgorithm */ explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); @@ -196,7 +195,7 @@ class AStarAlgorithm inline NodePtr getNextNode(); /** - * @brief Get pointer to next goal in open set + * @brief Add a node to the open set * @param cost The cost to sort into the open set of the node * @param node Node pointer reference to add to open set */ @@ -204,8 +203,7 @@ class AStarAlgorithm /** * @brief Adds node to graph - * @param cost The cost to sort into the open set of the node - * @param node Node pointer reference to add to open set + * @param index Node index to add */ inline NodePtr addToGraph(const unsigned int & index); @@ -218,9 +216,8 @@ class AStarAlgorithm /** * @brief Get cost of heuristic of node - * @param node Node index current - * @param node Node index of new - * @return Heuristic cost between the nodes + * @param node Node pointer to get heuristic for + * @return Heuristic cost for node */ inline float getHeuristicCost(const NodePtr & node); @@ -248,6 +245,11 @@ class AStarAlgorithm inline void populateExpansionsLog( const NodePtr & node, std::vector> * expansions_log); + /** + * @brief Clear Start + */ + void clearStart(); + int _timing_interval = 5000; bool _traverse_unknown; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index eb3680ec1c..0fbace4360 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -47,7 +47,6 @@ class NodeBasic public: /** * @brief A constructor for nav2_smac_planner::NodeBasic - * @param cost_in The costmap cost at this node * @param index The index of this node for self-reference */ explicit NodeBasic(const unsigned int index) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index c74c3a8d17..807e068c88 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -58,8 +58,6 @@ struct LatticeMotionTable /** * @brief Initializing state lattice planner's motion model * @param size_x_in Size of costmap in X - * @param size_y_in Size of costmap in Y - * @param angle_quantization_in Size of costmap in bin sizes * @param search_info Parameters for searching */ void initMotionModel( @@ -69,9 +67,12 @@ struct LatticeMotionTable /** * @brief Get projections of motion models * @param node Ptr to NodeLattice + * @param Reference direction change index * @return A set of motion poses */ - MotionPrimitivePtrs getMotionPrimitives(const NodeLattice * node); + MotionPrimitivePtrs getMotionPrimitives( + const NodeLattice * node, + unsigned int & direction_change_index); /** * @brief Get file metadata needed diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index f5207cd45c..ce8bafd2fd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -27,6 +27,7 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" @@ -111,6 +112,11 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; double _lookup_table_size; + bool _debug_visualizations; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _planned_footprints_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _expansions_publisher; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index c2a3815457..e57db9952a 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -234,10 +234,8 @@ bool AStarAlgorithm::areInputsValid() throw nav2_core::GoalOccupied("Goal was in lethal cost"); } - // Check if starting point is valid - if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::StartOccupied("Start was in lethal cost"); - } + // Note: We do not check the if the start is valid because it is cleared + clearStart(); return true; } @@ -465,6 +463,20 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } +template<> +void AStarAlgorithm::clearStart() +{ + auto coords = Node2D::getCoords(_start->getIndex()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + +template +void AStarAlgorithm::clearStart() +{ + auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index f630b51eca..0076979c44 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -114,7 +114,9 @@ void LatticeMotionTable::initMotionModel( } } -MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * node) +MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives( + const NodeLattice * node, + unsigned int & direction_change_index) { MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta]; MotionPrimitivePtrs primitive_projection_list; @@ -122,6 +124,9 @@ MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * primitive_projection_list.push_back(&prims_at_heading[i]); } + // direction change index + direction_change_index = static_cast(primitive_projection_list.size()); + if (allow_reverse_expansion) { // Find normalized heading bin of the reverse expansion double reserve_heading = node->pose.theta - (num_angle_quantization / 2); @@ -475,17 +480,12 @@ void NodeLattice::getNeighbors( bool backwards = false; NodePtr neighbor = nullptr; Coordinates initial_node_coords, motion_projection; - MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(this); + unsigned int direction_change_index = 0; + MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives( + this, + direction_change_index); const float & grid_resolution = motion_table.lattice_metadata.grid_resolution; - unsigned int direction_change_idx = 1e9; - for (unsigned int i = 0; i != motion_primitives.size(); i++) { - if (motion_primitives[0]->start_angle != motion_primitives[i]->start_angle) { - direction_change_idx = i; - break; - } - } - for (unsigned int i = 0; i != motion_primitives.size(); i++) { const MotionPose & end_pose = motion_primitives[i]->poses.back(); motion_projection.x = this->pose.x + (end_pose._x / grid_resolution); @@ -507,7 +507,7 @@ void NodeLattice::getNeighbors( // account in case the robot base footprint is asymmetric. backwards = false; angle = motion_projection.theta; - if (i >= direction_change_idx) { + if (i >= direction_change_index) { backwards = true; angle = motion_projection.theta - (motion_table.num_angle_quantization / 2); if (angle < 0) { diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 94b8ee1306..77650a61f4 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -240,9 +240,6 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Corner case of start and goal beeing on the same cell if (mx_start == mx_goal && my_start == my_goal) { - if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied("Start was in lethal cost"); - } pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal // orientation, unless use_final_approach_orientation=true where we need it to be the start @@ -262,6 +259,11 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) { + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 440dcb47cd..446c5cbc2a 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -408,6 +408,12 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } _expansions_publisher->publish(msg); } + + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 65847d4b3b..61c64b23b3 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -135,6 +135,9 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".allow_reverse_expansion", rclcpp::ParameterValue(false)); node->get_parameter(name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion); + nav2_util::declare_parameter_if_not_declared( + node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".debug_visualizations", _debug_visualizations); _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = @@ -202,6 +205,12 @@ void SmacPlannerLattice::configure( _smoother->initialize(_metadata.min_turning_radius); } + if (_debug_visualizations) { + _expansions_publisher = node->create_publisher("expansions", 1); + _planned_footprints_publisher = node->create_publisher( + "planned_footprints", 1); + } + RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerLattice with " "maximum iterations %i, max on approach iterations %i, " @@ -217,6 +226,10 @@ void SmacPlannerLattice::activate() _logger, "Activating plugin %s of type SmacPlannerLattice", _name.c_str()); _raw_plan_publisher->on_activate(); + if (_debug_visualizations) { + _expansions_publisher->on_activate(); + _planned_footprints_publisher->on_activate(); + } auto node = _node.lock(); // Add callback for dynamic parameters _dyn_params_handler = node->add_on_set_parameters_callback( @@ -229,6 +242,10 @@ void SmacPlannerLattice::deactivate() _logger, "Deactivating plugin %s of type SmacPlannerLattice", _name.c_str()); _raw_plan_publisher->on_deactivate(); + if (_debug_visualizations) { + _expansions_publisher->on_deactivate(); + _planned_footprints_publisher->on_deactivate(); + } _dyn_params_handler.reset(); } @@ -291,11 +308,35 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::CoordinateVector path; int num_iterations = 0; std::string error; + std::unique_ptr>> expansions = nullptr; + if (_debug_visualizations) { + expansions = std::make_unique>>(); + } // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + path, num_iterations, + _tolerance / static_cast(_costmap->getResolution()), expansions.get())) { + if (_debug_visualizations) { + geometry_msgs::msg::PoseArray msg; + geometry_msgs::msg::Pose msg_pose; + msg.header.stamp = _clock->now(); + msg.header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg.poses.push_back(msg_pose); + } + _expansions_publisher->publish(msg); + } + + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { @@ -328,6 +369,38 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( _raw_plan_publisher->publish(plan); } + if (_debug_visualizations) { + // Publish expansions for debug + geometry_msgs::msg::PoseArray msg; + geometry_msgs::msg::Pose msg_pose; + msg.header.stamp = _clock->now(); + msg.header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg.poses.push_back(msg_pose); + } + _expansions_publisher->publish(msg); + + // plot footprint path planned for debug + if (_planned_footprints_publisher->get_subscription_count() > 0) { + auto marker_array = std::make_unique(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); + } + + if (marker_array->markers.empty()) { + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + } + _planned_footprints_publisher->publish(std::move(marker_array)); + } + } + // Find how much time we have left to do smoothing steady_clock::time_point b = steady_clock::now(); duration time_span = duration_cast>(b - a); diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 873446a7a1..730193f802 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -92,10 +92,6 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setCollisionChecker(checker.get()); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); - a_star_2.setStart(50, 50, 0); // invalid - a_star_2.setGoal(0, 0, 0); // valid - num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); a_star_2.setStart(0, 0, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid num_it = 0; @@ -234,7 +230,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 22); - EXPECT_GT(path.size(), 46u); + EXPECT_GT(path.size(), 45u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 16674d9dad..f64f022f71 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -120,9 +120,12 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info); nav2_smac_planner::NodeLattice aNode(0); + unsigned int direction_change_index = 0; aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0)); nav2_smac_planner::MotionPrimitivePtrs projections = - nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&aNode); + nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives( + &aNode, + direction_change_index); EXPECT_NEAR(projections[0]->poses.back()._x, 0.5, 0.01); EXPECT_NEAR(projections[0]->poses.back()._y, -0.35, 0.01); @@ -130,7 +133,9 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) EXPECT_NEAR( nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata( - filePath).grid_resolution, 0.05, 0.005); + filePath) + .grid_resolution, + 0.05, 0.005); } TEST(NodeLatticeTest, test_node_lattice_conversions) @@ -248,7 +253,6 @@ TEST(NodeLatticeTest, test_node_lattice) delete costmapA; } - TEST(NodeLatticeTest, test_get_neighbors) { auto lnode = std::make_shared("test"); @@ -356,9 +360,12 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint) node.pose.x = 20; node.pose.y = 20; node.pose.theta = 0; + + // initialize direction change index + unsigned int direction_change_index = 0; // Test that the node is valid though all motion primitives poses for custom footprint nav2_smac_planner::MotionPrimitivePtrs motion_primitives = - nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node); + nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node, direction_change_index); EXPECT_GT(motion_primitives.size(), 0u); for (unsigned int i = 0; i < motion_primitives.size(); i++) { EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], false), true); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 3be1a9e897..aae4666edc 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -45,6 +45,7 @@ TEST(SmacTest, test_smac_se2) { rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = std::make_shared("SmacSE2Test"); + nodeSE2->declare_parameter("test.debug_visualizations", rclcpp::ParameterValue(true)); std::shared_ptr costmap_ros = std::make_shared("global_costmap"); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 9ce99a46b1..b791f9961c 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -54,6 +54,7 @@ TEST(SmacTest, test_smac_lattice) { rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice = std::make_shared("SmacLatticeTest"); + nodeLattice->declare_parameter("test.debug_visualizations", rclcpp::ParameterValue(true)); std::shared_ptr costmap_ros = std::make_shared("global_costmap"); diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index 35eaadb861..fac9ffe3af 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -166,8 +166,6 @@ class SmootherServer : public nav2_util::LifecycleNode std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; std::shared_ptr collision_checker_; - - rclcpp::Clock steady_clock_; }; } // namespace nav2_smoother diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 92d567a057..4e7f7242de 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -259,7 +259,7 @@ bool SmootherServer::findSmootherId( void SmootherServer::smoothPlan() { - auto start_time = steady_clock_.now(); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a path to smooth."); @@ -283,7 +283,7 @@ void SmootherServer::smoothPlan() result->was_completed = smoothers_[current_smoother_]->smooth( result->path, goal->max_smoothing_duration); - result->smoothing_duration = steady_clock_.now() - start_time; + result->smoothing_duration = this->now() - start_time; if (!result->was_completed) { RCLCPP_INFO( diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 29f3b89a82..0b494d96c6 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -72,6 +72,8 @@ bool SimpleSmoother::smooth( std::vector path_segments = findDirectionalPathSegments(path); + std::lock_guard lock(*(costmap->getMutex())); + for (unsigned int i = 0; i != path_segments.size(); i++) { if (path_segments[i].end - path_segments[i].start > 9) { // Populate path segment diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 67472eb46b..8fb1b6ad0d 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -146,7 +146,12 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) { costmap_msg_ = msg; - costmap_received_ = true; + costmap_ = std::make_shared( + msg->metadata.size_x, msg->metadata.size_y, + msg->metadata.resolution, msg->metadata.origin.position.x, + msg->metadata.origin.position.y); + + processCurrentCostmapMsg(); } }; diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 48ba35038c..48b97848f8 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -138,6 +138,8 @@ class BehaviorTreeHandler "server_timeout", std::chrono::milliseconds(20)); // NOLINT blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT + blackboard->set( + "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp index a270c8ab08..974c2d60b2 100644 --- a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp @@ -35,7 +35,9 @@ BackupBehaviorTester::BackupBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("backup_behavior_test"); + rclcpp::NodeOptions options; + options.parameter_overrides({{"use_sim_time", true}}); + node_ = rclcpp::Node::make_shared("backup_behavior_test", options); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index 6f4a6c3c16..bd4c5432fe 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_backup_behavior_node', output='screen' + cmd=[testExecutable], name='test_backup_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp index 8db6ef6406..967c6beb04 100644 --- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp @@ -35,7 +35,9 @@ DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test"); + rclcpp::NodeOptions options; + options.parameter_overrides({{"use_sim_time", true}}); + node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test", options); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index 4552f51768..c9df3d3243 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_spin_behavior_node', output='screen' + cmd=[testExecutable], name='test_spin_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index f4c4e5bc16..80f1529820 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -102,7 +102,7 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_wait_behavior_node', output='screen' + cmd=[testExecutable], name='test_wait_behavior_node', output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index ea6e58afb7..dffd52f911 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -164,7 +164,6 @@ bool WaitBehaviorTester::behaviorTest( RCLCPP_INFO(node_->get_logger(), "result received"); - if ((node_->now() - start_time).seconds() < static_cast(wait_time)) { return false; } diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 7667d3081f..f5d79153ec 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -307,3 +307,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "keepout_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 58b92e608f..bf65fdf9ba 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -298,3 +298,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "speed_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index a3be1396bc..3799062cf8 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -298,3 +298,34 @@ filter_mask_server: frame_id: "map" topic_name: "/filter_mask" yaml_filename: "speed_mask.yaml" + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index da8ab82a20..3b3f723095 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -47,6 +47,7 @@ class TestType(Enum): class FilterMask: + def __init__(self, filter_mask: OccupancyGrid): self.filter_mask = filter_mask diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 2539cef7b5..b8bc078103 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -5,6 +5,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" @@ -305,3 +306,34 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_link" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 5e765981ab..b9fb4725ef 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -30,6 +30,7 @@ class GpsWaypointFollowerTest(Node): + def __init__(self): super().__init__(node_name='nav2_gps_waypoint_tester', namespace='') self.waypoints = None diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index 5e587ad8fa..2bcea31931 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -37,6 +37,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index cbb2ac20e6..8fd1bc6034 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -38,6 +38,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index 765535c6e5..9cea2f6118 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -37,6 +37,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 7eb4645706..2a3ef3df53 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -31,6 +31,7 @@ class WaypointFollowerTest(Node): + def __init__(self): super().__init__(node_name='nav2_waypoint_tester', namespace='') self.waypoints = None diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index d1ddf7354c..a58a4f87ed 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -116,6 +116,11 @@ class ThetaStar return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); } + /** + * @brief Clear Start + */ + void clearStart(); + int nodes_opened = 0; protected: diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 9432f867a4..f83b745b22 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -258,4 +258,12 @@ void ThetaStar::initializePosn(int size_inc) node_position_.push_back(nullptr); } } + +void ThetaStar::clearStart() +{ + unsigned int mx_start = static_cast(src_.x); + unsigned int my_start = static_cast(src_.y); + costmap_->setCost(mx_start, my_start, nav2_costmap_2d::FREE_SPACE); +} + } // namespace theta_star diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index f93eaa44d7..ca9391111b 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -111,12 +111,6 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( std::to_string(goal.pose.position.y) + ") was outside bounds"); } - if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was in lethal cost"); - } - if (planner_->costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::GoalOccupied( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + @@ -141,6 +135,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( return global_path; } + planner_->clearStart(); planner_->setStartAndGoal(start, goal); RCLCPP_DEBUG( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 4635bab0b0..1c8b9b1d64 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -50,6 +50,8 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_pytest REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + # skip copyright linting + set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp b/nav2_util/include/nav2_util/array_parser.hpp similarity index 91% rename from nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp rename to nav2_util/include/nav2_util/array_parser.hpp index ca2dd879b6..f232cf3027 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.hpp +++ b/nav2_util/include/nav2_util/array_parser.hpp @@ -28,13 +28,13 @@ * * author: Dave Hershberger */ -#ifndef NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ -#define NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ +#ifndef NAV2_UTIL__ARRAY_PARSER_HPP_ +#define NAV2_UTIL__ARRAY_PARSER_HPP_ #include #include -namespace nav2_costmap_2d +namespace nav2_util { /** @brief Parse a vector of vectors of floats from a string. @@ -46,6 +46,6 @@ namespace nav2_costmap_2d * anything, like part of a successful parse. */ std::vector> parseVVF(const std::string & input, std::string & error_return); -} // end namespace nav2_costmap_2d +} // end namespace nav2_util -#endif // NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_ +#endif // NAV2_UTIL__ARRAY_PARSER_HPP_ diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 884b7b366e..67cc87243d 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -132,7 +132,7 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal) Iter lowest_it = begin; for (Iter it = ++begin; it != end; ++it) { auto comp = getCompareVal(*it); - if (comp < lowest) { + if (comp <= lowest) { lowest = comp; lowest_it = it; } diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp index 78d84fb8db..e0c1692792 100644 --- a/nav2_util/include/nav2_util/node_thread.hpp +++ b/nav2_util/include/nav2_util/node_thread.hpp @@ -32,13 +32,15 @@ class NodeThread * @brief A background thread to process node callbacks constructor * @param node_base Interface to Node to spin in thread */ - explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); + explicit NodeThread( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); /** * @brief A background thread to process executor's callbacks constructor * @param executor Interface to executor to spin in thread */ - explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); + explicit NodeThread( + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); /** * @brief A background thread to process node callbacks constructor diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index d0bb65cf41..99570224ad 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -153,23 +154,11 @@ std::string get_plugin_type_param( } /** - * @brief A method to copy all parameters from one node (parent) to another (child). - * May throw parameter exceptions in error conditions - * @param parent Node to copy parameters from - * @param child Node to copy parameters to + * @brief Sets the caller thread to have a soft-realtime prioritization by + * increasing the priority level of the host thread. + * May throw exception if unable to set prioritization successfully */ -template -void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child) -{ - using Parameters = std::vector; - std::vector param_names = parent->list_parameters({}, 0).names; - Parameters params = parent->get_parameters(param_names); - for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) { - if (!child->has_parameter(iter->get_name())) { - child->declare_parameter(iter->get_name(), iter->get_parameter_value()); - } - } -} +void setSoftRealTimePriority(); } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index f05a4602ac..71828321f0 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -25,6 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_util/node_thread.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_util { @@ -57,6 +58,8 @@ class SimpleActionServer * @param server_timeout Timeout to to react to stop or preemption requests * @param spin_thread Whether to spin with a dedicated thread internally * @param options Options to pass to the underlying rcl_action_server_t + * @param realtime Whether the action server's worker thread should have elevated + * prioritization (soft realtime) */ template explicit SimpleActionServer( @@ -66,13 +69,15 @@ class SimpleActionServer CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), bool spin_thread = false, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + const bool realtime = false) : SimpleActionServer( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - action_name, execute_callback, completion_callback, server_timeout, spin_thread, options) + action_name, execute_callback, completion_callback, + server_timeout, spin_thread, options, realtime) {} /** @@ -83,6 +88,8 @@ class SimpleActionServer * @param server_timeout Timeout to to react to stop or preemption requests * @param spin_thread Whether to spin with a dedicated thread internally * @param options Options to pass to the underlying rcl_action_server_t + * @param realtime Whether the action server's worker thread should have elevated + * prioritization (soft realtime) */ explicit SimpleActionServer( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, @@ -94,7 +101,8 @@ class SimpleActionServer CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), bool spin_thread = false, - const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + const bool realtime = false) : node_base_interface_(node_base_interface), node_clock_interface_(node_clock_interface), node_logging_interface_(node_logging_interface), @@ -106,6 +114,7 @@ class SimpleActionServer spin_thread_(spin_thread) { using namespace std::placeholders; // NOLINT + use_realtime_prioritization_ = realtime; if (spin_thread_) { callback_group_ = node_base_interface->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -170,6 +179,17 @@ class SimpleActionServer return rclcpp_action::CancelResponse::ACCEPT; } + /** + * @brief Sets thread priority level + */ + void setSoftRealTimePriority() + { + if (use_realtime_prioritization_) { + nav2_util::setSoftRealTimePriority(); + debug_msg("Soft realtime prioritization successfully set!"); + } + } + /** * @brief Handles accepted goals and adds to preempted queue to switch to * @param Goal A server goal handle to cancel @@ -202,7 +222,11 @@ class SimpleActionServer // Return quickly to avoid blocking the executor, so spin up a new thread debug_msg("Executing goal asynchronously."); - execution_future_ = std::async(std::launch::async, [this]() {work();}); + execution_future_ = std::async( + std::launch::async, [this]() { + setSoftRealTimePriority(); + work(); + }); } } @@ -220,7 +244,7 @@ class SimpleActionServer node_logging_interface_->get_logger(), "Action server failed while executing action callback: \"%s\"", ex.what()); terminate_all(); - completion_callback_(); + if (completion_callback_) {completion_callback_();} return; } @@ -230,14 +254,14 @@ class SimpleActionServer if (stop_execution_) { warn_msg("Stopping the thread per request."); terminate_all(); - completion_callback_(); + if (completion_callback_) {completion_callback_();} break; } if (is_active(current_handle_)) { warn_msg("Current goal was not completed successfully."); terminate(current_handle_); - completion_callback_(); + if (completion_callback_) {completion_callback_();} } if (is_active(pending_handle_)) { @@ -290,7 +314,7 @@ class SimpleActionServer info_msg("Waiting for async process to finish."); if (steady_clock::now() - start_time >= server_timeout_) { terminate_all(); - completion_callback_(); + if (completion_callback_) {completion_callback_();} throw std::runtime_error("Action callback is still running and missed deadline to stop"); } } @@ -509,6 +533,7 @@ class SimpleActionServer CompletionCallback completion_callback_; std::future execution_future_; bool stop_execution_{false}; + bool use_realtime_prioritization_{false}; mutable std::recursive_mutex update_mutex_; bool server_active_{false}; diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index a639a0e59e..104966e219 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -8,6 +8,7 @@ add_library(${library_name} SHARED robot_utils.cpp node_thread.cpp odometry_utils.cpp + array_parser.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_costmap_2d/src/array_parser.cpp b/nav2_util/src/array_parser.cpp similarity index 98% rename from nav2_costmap_2d/src/array_parser.cpp rename to nav2_util/src/array_parser.cpp index eecd4eebc8..cfe2f69914 100644 --- a/nav2_costmap_2d/src/array_parser.cpp +++ b/nav2_util/src/array_parser.cpp @@ -34,7 +34,7 @@ #include #include -namespace nav2_costmap_2d +namespace nav2_util { /** @brief Parse a vector of vector of floats from a string. @@ -102,4 +102,4 @@ std::vector> parseVVF(const std::string & input, std::string return result; } -} // end namespace nav2_costmap_2d +} // end namespace nav2_util diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp index c19fbd9847..adcbe50b78 100644 --- a/nav2_util/src/node_thread.cpp +++ b/nav2_util/src/node_thread.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include - #include "nav2_util/node_thread.hpp" namespace nav2_util { -NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) +NodeThread::NodeThread( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_(node_base) { executor_ = std::make_shared(); @@ -35,7 +35,10 @@ NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nod NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor) : executor_(executor) { - thread_ = std::make_unique([&]() {executor_->spin();}); + thread_ = std::make_unique( + [&]() { + executor_->spin(); + }); } NodeThread::~NodeThread() diff --git a/nav2_util/src/node_utils.cpp b/nav2_util/src/node_utils.cpp index e5415b81ad..993eaf53b6 100644 --- a/nav2_util/src/node_utils.cpp +++ b/nav2_util/src/node_utils.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -89,4 +90,17 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix) return rclcpp::Node::make_shared("_", options); } +void setSoftRealTimePriority() +{ + sched_param sch; + sch.sched_priority = 49; + if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + std::string errmsg( + "Cannot set as real-time thread. Users must set: hard rtprio 99 and " + " soft rtprio 99 in /etc/security/limits.conf to enable " + "realtime prioritization! Error: "); + throw std::runtime_error(errmsg + std::strerror(errno)); + } +} + } // namespace nav2_util diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 9f1ae99bbc..cfd747b9f1 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,3 +41,6 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) + +ament_add_gtest(test_array_parser test_array_parser.cpp) +target_link_libraries(test_array_parser ${library_name}) diff --git a/nav2_costmap_2d/test/unit/array_parser_test.cpp b/nav2_util/test/test_array_parser.cpp similarity index 88% rename from nav2_costmap_2d/test/unit/array_parser_test.cpp rename to nav2_util/test/test_array_parser.cpp index 3706f0cb77..76a691b700 100644 --- a/nav2_costmap_2d/test/unit/array_parser_test.cpp +++ b/nav2_util/test/test_array_parser.cpp @@ -31,13 +31,13 @@ #include #include "gtest/gtest.h" -#include "nav2_costmap_2d/array_parser.hpp" +#include "nav2_util/array_parser.hpp" TEST(array_parser, basic_operation) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[[1, 2.2], [.3, -4e4]]", error); + vvf = nav2_util::parseVVF("[[1, 2.2], [.3, -4e4]]", error); EXPECT_EQ(2u, vvf.size() ); EXPECT_EQ(2u, vvf[0].size() ); EXPECT_EQ(2u, vvf[1].size() ); @@ -52,7 +52,7 @@ TEST(array_parser, missing_open) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[1, 2.2], [.3, -4e4]]", error); + vvf = nav2_util::parseVVF("[1, 2.2], [.3, -4e4]]", error); EXPECT_NE(error, ""); } @@ -60,7 +60,7 @@ TEST(array_parser, missing_close) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[[1, 2.2], [.3, -4e4]", error); + vvf = nav2_util::parseVVF("[[1, 2.2], [.3, -4e4]", error); EXPECT_NE(error, ""); } @@ -68,7 +68,7 @@ TEST(array_parser, wrong_depth) { std::string error; std::vector> vvf; - vvf = nav2_costmap_2d::parseVVF("[1, 2.2], [.3, -4e4]", error); + vvf = nav2_util::parseVVF("[1, 2.2], [.3, -4e4]", error); EXPECT_NE(error, ""); } diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index ff85d2bd41..bf94527635 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -94,35 +94,3 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error); } - -TEST(TestParamCopying, TestParamCopying) -{ - auto node1 = std::make_shared("test_node1"); - auto node2 = std::make_shared("test_node2"); - - // Tests for (1) multiple types, (2) recursion, (3) overriding values - node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); - node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); - node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); - node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); - node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); - - // Show Node2 is empty of Node1's parameters, but contains its own - EXPECT_FALSE(node2->has_parameter("Foo1")); - EXPECT_FALSE(node2->has_parameter("Foo2")); - EXPECT_FALSE(node2->has_parameter("Foo.bar")); - EXPECT_TRUE(node2->has_parameter("Foo")); - EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); - - nav2_util::copy_all_parameters(node1, node2); - - // Test new parameters exist, of expected value, and original param is not overridden - EXPECT_TRUE(node2->has_parameter("Foo1")); - EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); - EXPECT_TRUE(node2->has_parameter("Foo2")); - EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); - EXPECT_TRUE(node2->has_parameter("Foo.bar")); - EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); - EXPECT_TRUE(node2->has_parameter("Foo")); - EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); -} diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 1c86659aae..d2dad65913 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -133,6 +133,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) "cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1)); + declare_parameter_if_not_declared(node, "use_realtime_priority", rclcpp::ParameterValue(false)); + bool use_realtime_priority = false; + node->get_parameter("use_realtime_priority", use_realtime_priority); + if (use_realtime_priority) { + try { + nav2_util::setSoftRealTimePriority(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + } + return nav2_util::CallbackReturn::SUCCESS; } @@ -142,7 +154,7 @@ VelocitySmoother::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Activating"); smoothed_cmd_pub_->on_activate(); double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); @@ -350,7 +362,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); } else if (name == "velocity_timeout") { diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp index 911ae441a2..cbd2700246 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -73,6 +73,7 @@ class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor int waypoint_pause_duration_; bool is_enabled_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp index 917655e21a..77e20bcf76 100644 --- a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp @@ -42,6 +42,7 @@ void WaitAtWaypoint::initialize( throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"}; } logger_ = node->get_logger(); + clock_ = node->get_clock(); nav2_util::declare_parameter_if_not_declared( node, plugin_name + ".waypoint_pause_duration", @@ -77,7 +78,7 @@ bool WaitAtWaypoint::processAtWaypoint( logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds", curr_waypoint_index, waypoint_pause_duration_); - rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); + clock_->sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); return true; } } // namespace nav2_waypoint_follower diff --git a/tools/underlay.repos b/tools/underlay.repos index 2e092f2517..96772eafd1 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -14,7 +14,7 @@ repositories: # ros-perception/vision_opencv: # type: git # url: https://github.com/ros-perception/vision_opencv.git - # version: ros2 + # version: rolling # ros/bond_core: # type: git # url: https://github.com/ros/bond_core.git