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 4e723f1ac5f..505a7895c51 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 @@ -191,15 +191,15 @@ class BtActionNode : public BT::ActionNodeBase { // first step to be done only at the beginning of the Action if (!BT::isStatusActive(status())) { - // setting the status to RUNNING to notify the BT Loggers (if any) - setStatus(BT::NodeStatus::RUNNING); - // reset the flag to send the goal or not, allowing the user the option to set it in on_tick should_send_goal_ = true; // user defined callback, may modify "should_send_goal_". on_tick(); + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + if (!should_send_goal_) { return BT::NodeStatus::FAILURE; } 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 97511608d50..6466948b313 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 @@ -86,7 +86,6 @@ class AssistedTeleopAction : 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/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index 33552a24f06..fcfade6d17b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -61,7 +61,6 @@ class RemovePassedGoals : public BT::ActionNodeBase double transform_tolerance_; std::shared_ptr 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 4f9b300e445..cad5e8ce4a8 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 @@ -86,7 +86,6 @@ 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 fdc320c16b0..1749a032cb1 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 @@ -61,9 +61,6 @@ 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 77a80728ddf..dd200f318b3 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 @@ -80,7 +80,6 @@ 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/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index 44e582c5f53..ff65da09a18 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -90,7 +90,6 @@ class GoalReachedCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; std::shared_ptr tf_; - bool initialized_; double goal_reached_tol_; double transform_tolerance_; std::string robot_base_frame_; 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 59a023ff3c2..40f0ac5804b 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 @@ -86,7 +86,6 @@ 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 5a9f255a9b7..a22942f9a56 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 @@ -73,7 +73,6 @@ 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 26a3431b5db..2e34689c56a 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 @@ -68,7 +68,6 @@ 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 511c381321b..732e862ff4c 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 @@ -80,7 +80,6 @@ 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/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index c390357b342..ccc346eb16d 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 @@ -64,7 +64,6 @@ 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 e876d2ce40b..7354b05f370 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -24,8 +24,7 @@ AssistedTeleopAction::AssistedTeleopAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) {} void AssistedTeleopAction::initialize() @@ -36,12 +35,11 @@ void AssistedTeleopAction::initialize() // Populate the input message goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); - initialized_ = true; } void AssistedTeleopAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index e17580fe71c..3df77f98d7c 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -24,8 +24,7 @@ BackUpAction::BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -44,12 +43,11 @@ void nav2_behavior_tree::BackUpAction::initialize() 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_) { + if (!BT::isStatusActive(status())) { initialize(); } 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 665bf81bc3f..723e704e437 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -28,8 +28,7 @@ RemovePassedGoals::RemovePassedGoals( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), - viapoint_achieved_radius_(0.5), - initialized_(false) + viapoint_achieved_radius_(0.5) {} void RemovePassedGoals::initialize() @@ -42,14 +41,11 @@ void RemovePassedGoals::initialize() robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); - initialized_ = true; } inline BT::NodeStatus RemovePassedGoals::tick() { - setStatus(BT::NodeStatus::RUNNING); - - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index d10bb83f32b..cb3459006fe 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -23,10 +23,7 @@ SpinAction::SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) -{ -} +: BtActionNode(xml_tag_name, action_name, conf) {} void SpinAction::initialize() { @@ -37,13 +34,11 @@ void SpinAction::initialize() 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_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index b607d026e59..03a18c9f947 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -25,8 +25,7 @@ WaitAction::WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -42,12 +41,11 @@ void WaitAction::initialize() } goal_.time = rclcpp::Duration::from_seconds(duration); - initialized_ = true; } void WaitAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 991e0ab7cf2..2ebf9c486ea 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -29,8 +29,7 @@ DistanceTraveledCondition::DistanceTraveledCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), distance_(1.0), - transform_tolerance_(0.1), - initialized_(false) + transform_tolerance_(0.1) { } @@ -46,12 +45,11 @@ void DistanceTraveledCondition::initialize() node_, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); - initialized_ = true; } BT::NodeStatus DistanceTraveledCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e93ba5cc360..4b5e20a733e 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -27,8 +27,7 @@ namespace nav2_behavior_tree GoalReachedCondition::GoalReachedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf), - initialized_(false) +: BT::ConditionNode(condition_name, conf) { auto node = config().blackboard->get("node"); @@ -52,13 +51,11 @@ void GoalReachedCondition::initialize() tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - - initialized_ = true; } BT::NodeStatus GoalReachedCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } 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 a0e3761f28c..eb0a8be4301 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -27,35 +27,40 @@ IsBatteryLowCondition::IsBatteryLowCondition( battery_topic_("/battery_status"), min_battery_(0.0), is_voltage_(false), - is_battery_low_(false), - initialized_(false) + is_battery_low_(false) { } void IsBatteryLowCondition::initialize() { getInput("min_battery", min_battery_); - getInput("battery_topic", battery_topic_); + std::string battery_topic_new; + getInput("battery_topic", battery_topic_new); getInput("is_voltage", is_voltage_); - node_ = config().blackboard->get("node"); - callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - rclcpp::SubscriptionOptions sub_option; - sub_option.callback_group = callback_group_; - battery_sub_ = node_->create_subscription( - battery_topic_, - rclcpp::SystemDefaultsQoS(), - std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), - sub_option); - initialized_ = true; + // Only create a new subscriber if the topic has changed or subscriber is empty + if (battery_topic_new != battery_topic_ || !battery_sub_) { + battery_topic_ = battery_topic_new; + + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + battery_sub_ = node_->create_subscription( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), + sub_option); + } } BT::NodeStatus IsBatteryLowCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } 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 7274743e1e9..cd76df97495 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -23,8 +23,7 @@ namespace nav2_behavior_tree IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf), - initialized_(false) +: BT::ConditionNode(condition_name, conf) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); @@ -35,12 +34,11 @@ IsPathValidCondition::IsPathValidCondition( void IsPathValidCondition::initialize() { getInput("server_timeout", server_timeout_); - initialized_ = true; } BT::NodeStatus IsPathValidCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index f03c9711aa8..23fd614cb7c 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -27,8 +27,7 @@ TimeExpiredCondition::TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - period_(1.0), - initialized_(false) + period_(1.0) { } @@ -37,12 +36,11 @@ void TimeExpiredCondition::initialize() getInput("seconds", period_); node_ = config().blackboard->get("node"); start_ = node_->now(); - initialized_ = true; } BT::NodeStatus TimeExpiredCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 1afe0e14331..ab589048c82 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -31,8 +31,7 @@ TransformAvailableCondition::TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - was_found_(false), - initialized_(false) + was_found_(false) { node_ = config().blackboard->get("node"); tf_ = config().blackboard->get>("tf_buffer"); @@ -56,12 +55,11 @@ void TransformAvailableCondition::initialize() } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); - initialized_ = true; } BT::NodeStatus TransformAvailableCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index 9592d119c96..5263da09eb2 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -24,8 +24,7 @@ RateController::RateController( const std::string & name, const BT::NodeConfiguration & conf) : BT::DecoratorNode(name, conf), - first_time_(false), - initialized_(false) + first_time_(false) { } @@ -34,12 +33,11 @@ void RateController::initialize() double hz = 1.0; getInput("hz", hz); period_ = 1.0 / hz; - initialized_ = true; } BT::NodeStatus RateController::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 8a96b0bad17..76725b8d47f 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(geometry_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_common REQUIRED) +find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav_2d_msgs REQUIRED) diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index b83ae3a214c..7eb51ca459c 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -14,6 +14,7 @@ geometry_msgs lifecycle_msgs nav2_core + nav2_costmap_2d nav2_msgs nav2_util nav_2d_msgs diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 005dd205d80..927e5806122 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -150,8 +150,7 @@ class Costmap2D /** * @brief Get the cost of a cell in the costmap - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell + * @param index The cell index * @return The cost of the cell */ unsigned char getCost(unsigned int index) const; diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 6321305dd79..3ba1c97f08a 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -225,8 +225,7 @@ void ObstacleLayer::onInitialize() source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data; - custom_qos_profile.depth = 50; + const auto custom_qos_profile = rclcpp::SensorDataQoS().keep_last(50); // create a callback for the topic if (data_type == "LaserScan") { diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index 73399040902..6c8c599aa94 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -8,7 +8,7 @@ This was created by Steve Macenski of [Open Navigation LLC](https://opennav.org) It is drop-in replacable with AMR simulators and global localization by providing: - Map -> Odom transform -- Odom -> Base Link transform, `nav_msgs/Odometry` odometry +- Odom -> Base Link transform, `nav_msgs/Odometry` odometry - Accepts the standard `/initialpose` topic for transporting the robot to another location Note: This does not provide sensor data, so it is required that the global (and probably local) costmap contain the `StaticLayer` to avoid obstacles. @@ -33,12 +33,15 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na ### Parameters -- `update_duration`: the duration between updates (default 0.01 -- 100hz) +- `update_duration`: The duration between updates (default 0.01 -- 100hz) - `base_frame_id`: The base frame to use (default `base_link`) - `odom_frame_id`: The odom frame to use (default `odom`) - `map_frame_id`: The map frame to use (default `map`) - `scan_frame_id`: The can frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4) - `enable_stamped_cmd_vel`: Whether cmd_vel is stamped or unstamped (i.e. Twist or TwistStamped). Default `false` for `Twist`. +- `scan_publish_dur`: : The duration between publishing scan (default 0.1s -- 10hz) +- `publish_map_odom_tf`: Whether or not to publish tf from `map_frame_id` to `odom_frame_id` (default `true`) +- `publish_clock`: Whether or not to publish simulated clock to `/clock` (default `true`) ### Topics @@ -47,6 +50,7 @@ This node subscribes to: - `cmd_vel`: Nav2's output twist to get the commanded velocity This node publishes: +- `clock`: To publish a simulation clock for all other nodes with `use_sim_time=True` - `odom`: To publish odometry from twist - `tf`: To publish map->odom and odom->base_link transforms -- `scan`: To publish a bogus max range laser scan sensor to make the collision monitor happy +- `scan`: To publish a range laser scan sensor based on the static map diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 7902affb13b..71560c5aa80 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -24,6 +24,7 @@ from rclpy.duration import Duration from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from rosgraph_msgs.msg import Clock from sensor_msgs.msg import LaserScan from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations @@ -84,6 +85,9 @@ def __init__(self): self.publish_map_odom_tf = self.get_parameter( 'publish_map_odom_tf').get_parameter_value().bool_value + self.declare_parameter('publish_clock', True) + self.publish_clock = self.get_parameter('publish_clock').get_parameter_value().bool_value + self.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id self.t_map_to_odom.child_frame_id = self.odom_frame_id @@ -112,6 +116,10 @@ def __init__(self): depth=10) self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) + if self.publish_clock: + self.clock_timer = self.create_timer(0.1, self.clockTimerCallback) + self.clock_pub = self.create_publisher(Clock, '/clock', 10) + self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) self.map_client = self.create_client(GetMap, '/map_server/map') @@ -139,6 +147,11 @@ def setupTimerCallback(self): if self.mat_base_to_laser is None: self.getBaseToLaserTf() + def clockTimerCallback(self): + msg = Clock() + msg.clock = self.get_clock().now().to_msg() + self.clock_pub.publish(msg) + def cmdVelCallback(self, msg): self.debug('Received cmd_vel') if self.initial_pose is None: diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index a2a253def98..9376cfb50c7 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -12,7 +12,6 @@ nav_msgs tf_transformations tf2_ros - python3-transforms3d ament_copyright ament_flake8 diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 6fb82c1af2a..b97342cf820 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -133,8 +133,10 @@ std::string expand_user_home_dir_if_needed( return yaml_filename; } if (home_variable_value.empty()) { - std::cout << "[INFO] [map_io]: Map yaml file name starts with '~/' but no HOME variable set. \n" - << "[INFO] [map_io] User home dir will be not expanded \n"; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Map yaml file name starts with '~/' but no HOME variable set. \n" + << "[INFO] [map_io] User home dir will be not expanded \n"); return yaml_filename; } const std::string prefix{home_variable_value}; @@ -182,15 +184,18 @@ LoadParameters loadMapYaml(const std::string & yaml_filename) load_parameters.negate = yaml_get_value(doc, "negate"); } - std::cout << "[DEBUG] [map_io]: resolution: " << load_parameters.resolution << std::endl; - std::cout << "[DEBUG] [map_io]: origin[0]: " << load_parameters.origin[0] << std::endl; - std::cout << "[DEBUG] [map_io]: origin[1]: " << load_parameters.origin[1] << std::endl; - std::cout << "[DEBUG] [map_io]: origin[2]: " << load_parameters.origin[2] << std::endl; - std::cout << "[DEBUG] [map_io]: free_thresh: " << load_parameters.free_thresh << std::endl; - std::cout << "[DEBUG] [map_io]: occupied_thresh: " << load_parameters.occupied_thresh << - std::endl; - std::cout << "[DEBUG] [map_io]: mode: " << map_mode_to_string(load_parameters.mode) << std::endl; - std::cout << "[DEBUG] [map_io]: negate: " << load_parameters.negate << std::endl; //NOLINT + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "resolution: " << load_parameters.resolution); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[0]: " << load_parameters.origin[0]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[1]: " << load_parameters.origin[1]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[2]: " << load_parameters.origin[2]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "free_thresh: " << load_parameters.free_thresh); + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "occupied_thresh: " << load_parameters.occupied_thresh); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), + "mode: " << map_mode_to_string(load_parameters.mode)); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "negate: " << load_parameters.negate); return load_parameters; } @@ -202,8 +207,9 @@ void loadMapFromFile( Magick::InitializeMagick(nullptr); nav_msgs::msg::OccupancyGrid msg; - std::cout << "[INFO] [map_io]: Loading image_file: " << - load_parameters.image_file_name << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), "Loading image_file: " << + load_parameters.image_file_name); Magick::Image img(load_parameters.image_file_name); // Copy the image data into the map structure @@ -291,9 +297,11 @@ void loadMapFromFile( msg.header.frame_id = "map"; msg.header.stamp = clock.now(); - std::cout << - "[DEBUG] [map_io]: Read map " << load_parameters.image_file_name << ": " << msg.info.width << - " X " << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Read map " << load_parameters.image_file_name + << ": " << msg.info.width << " X " << msg.info.height << " map @ " + << msg.info.resolution << " m/cell"); map = msg; } @@ -303,30 +311,32 @@ LOAD_MAP_STATUS loadMapFromYaml( nav_msgs::msg::OccupancyGrid & map) { if (yaml_file.empty()) { - std::cerr << "[ERROR] [map_io]: YAML file name is empty, can't load!" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "YAML file name is empty, can't load!"); return MAP_DOES_NOT_EXIST; } - std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Loading yaml file: " << yaml_file); LoadParameters load_parameters; try { load_parameters = loadMapYaml(yaml_file); } catch (YAML::Exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position (" << - e.mark.line << ":" << e.mark.column << ") for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Failed processing YAML file " << yaml_file << " at position (" << + e.mark.line << ":" << e.mark.column << ") for reason: " << e.what()); return INVALID_MAP_METADATA; } catch (std::exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file << - " for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), "Failed to parse map YAML loaded from file " << yaml_file << + " for reason: " << e.what()); return INVALID_MAP_METADATA; } try { loadMapFromFile(load_parameters, map); } catch (std::exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name << - " for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Failed to load image file " << load_parameters.image_file_name << + " for reason: " << e.what()); return INVALID_MAP_DATA; } @@ -351,40 +361,46 @@ void checkSaveParameters(SaveParameters & save_parameters) rclcpp::Clock clock(RCL_SYSTEM_TIME); save_parameters.map_file_name = "map_" + std::to_string(static_cast(clock.now().seconds())); - std::cout << "[WARN] [map_io]: Map file unspecified. Map will be saved to " << - save_parameters.map_file_name << " file" << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Map file unspecified. Map will be saved to " << + save_parameters.map_file_name << " file"); } // Checking thresholds if (save_parameters.occupied_thresh == 0.0) { save_parameters.occupied_thresh = 0.65; - std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " << - save_parameters.occupied_thresh << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "map_io"), "Occupied threshold unspecified. Setting it to default value: " << + save_parameters.occupied_thresh); } if (save_parameters.free_thresh == 0.0) { save_parameters.free_thresh = 0.25; - std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " << - save_parameters.free_thresh << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Free threshold unspecified. Setting it to default value: " << + save_parameters.free_thresh); } if (1.0 < save_parameters.occupied_thresh) { - std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Threshold_occupied must be 1.0 or less"); throw std::runtime_error("Incorrect thresholds"); } if (save_parameters.free_thresh < 0.0) { - std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Free threshold must be 0.0 or greater"); throw std::runtime_error("Incorrect thresholds"); } if (save_parameters.occupied_thresh <= save_parameters.free_thresh) { - std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" << - std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Threshold_free must be smaller than threshold_occupied"); throw std::runtime_error("Incorrect thresholds"); } // Checking image format if (save_parameters.image_format == "") { save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm"; - std::cout << "[WARN] [map_io]: Image format unspecified. Setting it to: " << - save_parameters.image_format << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Image format unspecified. Setting it to: " << + save_parameters.image_format); } std::transform( @@ -407,24 +423,25 @@ void checkSaveParameters(SaveParameters & save_parameters) ss << "'" << format_name << "'"; first = false; } - std::cout << - "[WARN] [map_io]: Requested image format '" << save_parameters.image_format << - "' is not one of the recommended formats: " << ss.str() << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Requested image format '" << save_parameters.image_format << + "' is not one of the recommended formats: " << ss.str()); } const std::string FALLBACK_FORMAT = "png"; try { Magick::CoderInfo info(save_parameters.image_format); if (!info.isWritable()) { - std::cout << - "[WARN] [map_io]: Format '" << save_parameters.image_format << - "' is not writable. Using '" << FALLBACK_FORMAT << "' instead" << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Format '" << save_parameters.image_format << + "' is not writable. Using '" << FALLBACK_FORMAT << "' instead"); save_parameters.image_format = FALLBACK_FORMAT; } } catch (Magick::ErrorOption & e) { - std::cout << - "[WARN] [map_io]: Format '" << save_parameters.image_format << "' is not usable. Using '" << - FALLBACK_FORMAT << "' instead:" << std::endl << e.what() << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "map_io"), "Format '" << save_parameters.image_format << "' is not usable. Using '" << + FALLBACK_FORMAT << "' instead:" << std::endl << e.what()); save_parameters.image_format = FALLBACK_FORMAT; } @@ -435,10 +452,10 @@ void checkSaveParameters(SaveParameters & save_parameters) save_parameters.image_format == "jpg" || save_parameters.image_format == "jpeg")) { - std::cout << - "[WARN] [map_io]: Map mode 'scale' requires transparency, but format '" << - save_parameters.image_format << - "' does not support it. Consider switching image format to 'png'." << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Map mode 'scale' requires transparency, but format '" << + save_parameters.image_format << + "' does not support it. Consider switching image format to 'png'."); } } @@ -452,9 +469,10 @@ void tryWriteMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters) { - std::cout << - "[INFO] [map_io]: Received a " << map.info.width << " X " << map.info.height << " map @ " << - map.info.resolution << " m/pix" << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Received a " << map.info.width << " X " << map.info.height << " map @ " << + map.info.resolution << " m/pix"); std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format; { @@ -510,14 +528,18 @@ void tryWriteMapToFile( pixel = Magick::Color(q, q, q); break; default: - std::cerr << "[ERROR] [map_io]: Map mode should be Trinary, Scale or Raw" << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Map mode should be Trinary, Scale or Raw"); throw std::runtime_error("Invalid map mode"); } image.pixelColor(x, y, pixel); } } - std::cout << "[INFO] [map_io]: Writing map occupancy data to " << mapdatafile << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), + "Writing map occupancy data to " << mapdatafile); image.write(mapdatafile); } @@ -546,15 +568,15 @@ void tryWriteMapToFile( e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh; if (!e.good()) { - std::cout << - "[WARN] [map_io]: YAML writer failed with an error " << e.GetLastError() << - ". The map metadata may be invalid." << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), "YAML writer failed with an error " << e.GetLastError() << + ". The map metadata may be invalid."); } - std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Writing map metadata to " << mapmetadatafile); std::ofstream(mapmetadatafile) << e.c_str(); } - std::cout << "[INFO] [map_io]: Map saved" << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Map saved"); } bool saveMapToFile( @@ -570,7 +592,9 @@ bool saveMapToFile( tryWriteMapToFile(map, save_parameters_loc); } catch (std::exception & e) { - std::cout << "[ERROR] [map_io]: Failed to write map for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), + "Failed to write map for reason: " << e.what()); return false; } return true; diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index ebb25bb52be..49ab250123f 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -43,7 +43,7 @@ The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots - Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint. -## Features +## Features We further improve on Hybrid-A\* in the following ways: - Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio). @@ -90,7 +90,7 @@ We provide 3 nodes by default currently. The 2D node template (`Node2D`) which d In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path (not available for State Lattice due to the lattices generated are dependent on costmap resolution). For the `SmacPlannerHybrid` and `SmacPlannerLattice` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. -We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. +We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. ## Parameters @@ -111,13 +111,13 @@ planner_server: max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance terminal_checking_interval: 5000 # number of iterations between checking if the goal has been cancelled or planner timed out max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. - motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp + motion_model_for_search: "DUBIN" # For Hybrid Dubin, Reeds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: 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 (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius - analytic_expansion_max_cost: true # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal) - analytic_expansion_max_cost_override: false # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + analytic_expansion_max_cost: 200 # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal) + analytic_expansion_max_cost_override: false # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required). If expansion is within 2*pi*min_r of the goal, then it will override the max cost if ``false``. minimum_turning_radius: 0.40 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 change_penalty: 0.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 @@ -126,10 +126,10 @@ planner_server: retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0 rotation_penalty: 5.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings. lookup_table_size: 20.0 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. - 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. + cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsequent 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/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. + 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 @@ -155,19 +155,19 @@ sudo apt-get install ros--nav2-smac-planner ### Potential Fields -Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. +Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. -Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. +Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be somewhat suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. -So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. +So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. ### Hybrid-A* and State Lattice Turning Radius' A very reasonable and logical assumption would be to set the `minimum_turning_radius` to the kinematic limits of your vehicle. For an ackermann car, that's a physical quantity; while for differential or omni robots, its a bit of a dance around what kind of turns you'd like your robot to be able to make. Obviously setting this to something insanely small (like 20 cm) means you have alot of options, but also probably means the raw output plans won't be very straight and smooth when you have 2+ meter wide aisles to work in. -I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. +I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. By default, `0.4m` is the setting which I think is "reasonable" for the smaller scale industrial grade robots (think Simbe, the small Fetch, or Locus robots) resulting in faster plans and less "wobbly" motions that do not require post-smoothing -- further improving CPU performance. I selected `0.4m` as a trade off between practical robots mentioned above and hobbyist users with a tiny-little-turtlebot-3 which might still need to navigate around some smaller cavities. @@ -177,11 +177,11 @@ We provide for the Hybrid-A\*, State Lattice, and 2D A\* implementations a costm I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. -Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. +Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. ### Penalty Function Tuning -The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. +The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. **However**, due to the nature of the State Lattice planner being able to use any number of custom generated minimum control sets, this planner may require more tuning to get good behavior. The defaults for State Lattice were generated using the 5cm Ackermann files you can find in this package as initial examples. After a change in formulation for the Hybrid-A* planner, the default of change penalty off seems to produce good results, but please tune to your application need and run-time speed requirements. @@ -197,13 +197,13 @@ Note that change penalty must be greater than 0.0. The non-straight, reverse, an Before addressing the section below, make sure you have an appropriately set max iterations parameter. If you have a 1 km2 sized warehouse, clearly 5000 expansions will be insufficient. Try increasing this value if you're unable to achieve goals or disable it with the `-1` value to see if you are now able to plan within a reasonable time period. If you still have issues, there is a secondary effect which could be happening that is good to be aware of. -In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. +In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. Seeing the figures below, you'll see an attempt to plan into a "U" shaped region across the map. The first figure shows the small gap in the map (from an imperfect SLAM session) which is nearly traversible, but not quite. From the starting location, that gap yeilds the shortest path to the goal, so the heuristics will attempt to route the paths in that direction. However, it is not possible to actually pass with a kinematically valid path with the footprint set. As a result, the planner expands all of its maximum 1,000,000 iterations attempting to fit through it (visualized in red). If an infinite number of iterations were allowed, eventually a valid path would be found, but might take significant time. By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassible), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now! -As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. +As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. ![](media/A.png) ![](media/B.png) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 385f484615d..5afb919662f 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -54,6 +54,16 @@ void GridCollisionChecker::setFootprint( const double & possible_collision_cost) { possible_collision_cost_ = static_cast(possible_collision_cost); + if (possible_collision_cost_ <= 0.0f) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + footprint_is_radius_ = radius; // Use radius, no caching required @@ -114,18 +124,8 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5f), static_cast(y + 0.5f))); - if (footprint_cost_ < possible_collision_cost_) { - if (possible_collision_cost_ > 0.0f) { - return false; - } else { - RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 1000, - "Inflation layer either not found or inflation is not set sufficiently for " - "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" - " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " - "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" - " for full instructions. This will substantially impact run-time performance."); - } + if (footprint_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { + return false; } // If its inscribed, in collision, or unknown in the middle,