diff --git a/.circleci/config.yml b/.circleci/config.yml index 9d3e0f490d8..190ba5d7f12 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/README.md b/README.md index 1d5b83ab57b..e910e77b013 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_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 596fda97911..daf609d067c 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/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index fb3f683b462..97511608d50 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 7e5b27af9ce..dd1a6f29086 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 2e442c0e112..4f9b300e445 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 5143f11351e..fdc320c16b0 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 d29f40e66d8..3c87c7370c7 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 5c4753cf9a1..ff479105807 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 a04b1263f4b..36890f2a8a0 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 9f8c47afab0..356a79857db 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 64572e21b7a..5fdefa9b8bf 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 e53d6e4c351..729271a8c5a 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 201f4868cfd..24a0207e3bd 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 1d2bad9f376..64eb61d788e 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 cb15fc93336..09d79c7e230 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 be062357b16..7e0b613f62f 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 7a5816973dc..bf8b95c84f6 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 78fa1311752..cab9c8f8211 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 7f919a30f3a..b394a804c73 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 f08fed31476..b9317076737 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 5840b8882df..64394ee3782 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 d81df60a9ec..2697680415b 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 b8630261e6e..9d221cf8c81 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 4a02dede9aa..59d593a1475 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 e018e025352..1347998602f 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 4dc7e588937..64afd1a34be 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 8daca5afede..33e94178deb 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 6a8d3f33cad..91de9c2e22a 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 42fc8076be6..44e56dc55ec 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 13e98e0d29d..b710ec30091 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 947778da5d6..429368ea96a 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_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index de4aec7532e..cd0a1674285 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 6af6490b618..d01e14179ff 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 4628010a3de..a0fc9729991 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 1ec9e83264d..7a194874c7e 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 1e3b07fbf1c..55a7872247a 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/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 39d72a5297c..5f0e6e775d1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -199,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 @@ -256,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. @@ -362,7 +362,7 @@ collision_monitor: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 2.0 + time_before_collision: 1.2 simulation_time_step: 0.1 min_points: 6 visualize: False 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 9257836be88..ebbe3cbbe56 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 7667ec860cf..f02759815bc 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -224,6 +224,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 68b4cd7b507..eb3ee0a8739 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 ec7aa6d6b19..db36fbe1c6b 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" , "FootprintApproach"] 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/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 8491ec31b61..2566aea7991 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -23,6 +23,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" @@ -440,19 +441,14 @@ 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(); + 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. - if (!setPolygonShape(poly_row, poly_)) { - RCLCPP_ERROR( - logger_, - "[%s]: Polygon has incorrect points description", - polygon_name_.c_str()); - return false; - } - return true; + return getPolygonFromString(poly_string, poly_); + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, @@ -612,6 +608,46 @@ 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; +} + bool Polygon::setPolygonShape(std::vector & poly_points, std::vector & poly) { // Check for points format correctness diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index b0b926fde05..1ac519111f1 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -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 94801a3d2a3..242b9f72db9 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -301,8 +301,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 c616544469c..849b6278686 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, @@ -547,7 +560,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, @@ -559,12 +572,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, @@ -576,12 +588,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, @@ -593,7 +604,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( @@ -753,7 +764,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, @@ -886,7 +897,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( @@ -902,6 +913,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_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index f039cf5d42b..df0ddb2dc3a 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/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e72807da59b..8914166a5e1 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_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index bc887eeeb3b..501f675736e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -449,6 +449,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 diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp index c9259730382..75de1feb917 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/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index b9480fdffab..e69501277ea 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_mppi_controller/README.md b/nav2_mppi_controller/README.md index 13c314205bd..4f5a9398d1e 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -104,7 +104,7 @@ 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. @@ -124,11 +124,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/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index b8d6c898709..274315a06b7 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,7 +26,7 @@ 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); @@ -160,8 +161,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 +173,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 cd701ea093d..d334b816735 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/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index f88249aed14..08f50945fd5 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_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index c65119698a2..2ee8d69f53e 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 c54b9ae829b..2413e2eb3f0 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -241,9 +241,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_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index fea40ea4053..d087cbef758 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -361,7 +361,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(); @@ -421,7 +421,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_) { @@ -480,7 +480,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(); @@ -517,7 +517,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_) { diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index a8ded664cd5..17d2f130e84 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, @@ -88,6 +88,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 +130,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 +292,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_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 650d8d4adff..e4ecf38f035 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -245,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/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d6c9db69bb3..55fe35e5311 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/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 94b8ee1306e..77650a61f4f 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 d623ab61107..fc016ac5457 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -399,6 +399,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 8b46fc3ae74..d3ddf5bdef2 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -287,6 +287,11 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( if (!_a_star->createPath( 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/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index b6107b141db..f048cc55503 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; diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index 35eaadb8614..fac9ffe3af6 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 92d567a0573..4e7f7242deb 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_system_tests/src/behaviors/backup/backup_behavior_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp index a270c8ab082..974c2d60b2a 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 6f4a6c3c16e..bd4c5432fe8 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 8db6ef6406e..967c6beb046 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 4552f51768d..c9df3d32432 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 f4c4e5bc168..80f15298209 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 ea6e58afb7d..dffd52f9115 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_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 d1ddf7354ce..a58a4f87eda 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 9432f867a45..f83b745b22e 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 f93eaa44d73..ca9391111b3 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 4635bab0b0d..1c8b9b1d64b 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 ca2dd879b6f..f232cf30274 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/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index c74dcf55f29..71828321f00 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -244,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; } @@ -254,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_)) { @@ -314,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"); } } diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index a639a0e59e9..104966e2198 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 eecd4eebc8c..cfe2f699145 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/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 9f1ae99bbcb..cfd747b9f1b 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 3706f0cb775..76a691b7000 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_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index d3dd5c2a997..d2dad659130 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -154,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)); @@ -362,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 911ae441a23..cbd27002464 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 917655e21a0..77e20bcf768 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