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 06fc6dd8103..90c7adda745 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 @@ -97,7 +97,9 @@ class BtActionNode : public BT::ActionNodeBase RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); - throw std::runtime_error(std::string("Action server %s not available", action_name.c_str())); + throw std::runtime_error( + std::string("Action server ") + action_name + + std::string(" not available")); } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index 504df9157a1..e87ffbfac48 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -93,7 +93,9 @@ class BtCancelActionNode : public BT::ActionNodeBase RCLCPP_ERROR( node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", action_name.c_str()); - throw std::runtime_error(std::string("Action server %s not available", action_name.c_str())); + throw std::runtime_error( + std::string("Action server ") + action_name + + std::string(" not available")); } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 15322ef85b5..8150eb31098 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -57,7 +57,6 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), - BT::InputPort("filter_duration", 0.3, "Duration (secs) for velocity smoothing filter") }; } diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 67b97662afb..49cfeffb4a3 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -11,7 +11,7 @@ Distance to backup Speed at which to backup Allowed time for reversing - Service name + Server name Server timeout @@ -19,27 +19,27 @@ Distance to travel Speed at which to travel Allowed time for reversing - Service name + Server name Server timeout - Service name to cancel the controller server + Server name to cancel the controller server Server timeout - Service name to cancel the backup behavior + Server name to cancel the backup behavior Server timeout - Service name to cancel the drive on heading behavior + Service name to cancel the drive on heading behavior Server timeout - Service name to cancel the spin behavior + Server name to cancel the spin behavior Server timeout @@ -49,7 +49,7 @@ - Service name to cancel the wait behavior + Server name to cancel the wait behavior Server timeout @@ -74,7 +74,7 @@ Destination to plan to Start pose of the path if overriding current robot pose Mapped name to the planner plugin type to use - Service name + Server name Server timeout Path created by ComputePathToPose node @@ -82,7 +82,7 @@ Destinations to plan through Start pose of the path if overriding current robot pose - Service name + Server name Server timeout Mapped name to the planner plugin type to use Path created by ComputePathToPose node @@ -110,20 +110,20 @@ Path to follow Goal checker - Service name + Server name Server timeout Goal - Service name + Server name Server timeout Behavior tree to run Goals - Service name + Server name Server timeout Behavior tree to run @@ -177,13 +177,13 @@ Spin distance Allowed time for spinning - Service name + Server name Server timeout Wait time - Service name + Server name Server timeout @@ -242,6 +242,24 @@ Server timeout + + Error code + + + + Error code + + + + Error code + + + + Error code + Error codes to check, user defined + + + @@ -275,7 +293,6 @@ Maximum rate Minimum speed Maximum speed - Duration (secs) for velocity smoothing filter diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 26d96de77a7..e5c96eba2c2 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -50,18 +50,28 @@ SpeedController::SpeedController( d_rate_ = max_rate_ - min_rate_; d_speed_ = max_speed_ - min_speed_; - double duration = 0.3; - getInput("filter_duration", duration); std::string odom_topic; node_->get_parameter_or("odom_topic", odom_topic, std::string("odom")); - odom_smoother_ = std::make_shared(node_, duration, odom_topic); + odom_smoother_ = config().blackboard->get>( + "odom_smoother"); } inline BT::NodeStatus SpeedController::tick() { - auto current_goal = config().blackboard->get("goal"); - auto current_goals = - config().blackboard->get>("goals"); + if (status() == BT::NodeStatus::IDLE) { + // Reset since we're starting a new iteration of + // the speed controller (moving from IDLE to RUNNING) + config().blackboard->get>("goals", goals_); + config().blackboard->get("goal", goal_); + period_ = 1.0 / max_rate_; + start_ = node_->now(); + first_tick_ = true; + } + + std::vector current_goals; + config().blackboard->get>("goals", current_goals); + geometry_msgs::msg::PoseStamped current_goal; + config().blackboard->get("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { // Reset state and set period to max since we have a new goal diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 7e9d50c9c36..7ece3dfe695 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -34,6 +34,10 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi public: void SetUp() { + odom_smoother_ = std::make_shared(node_); + config_->blackboard->set>( + "odom_smoother", odom_smoother_); // NOLINT + geometry_msgs::msg::PoseStamped goal; goal.header.stamp = node_->now(); config_->blackboard->set("goal", goal); @@ -50,13 +54,17 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi { dummy_node_.reset(); bt_node_.reset(); + odom_smoother_.reset(); } protected: + static std::shared_ptr odom_smoother_; static std::shared_ptr bt_node_; static std::shared_ptr dummy_node_; }; +std::shared_ptr +SpeedControllerTestFixture::odom_smoother_ = nullptr; std::shared_ptr SpeedControllerTestFixture::bt_node_ = nullptr; std::shared_ptr diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 9eee3f06a9d..2d631c43900 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -37,6 +37,7 @@ def generate_launch_description(): params_file = LaunchConfiguration('params_file') use_composition = LaunchConfiguration('use_composition') container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') @@ -141,7 +142,7 @@ def generate_launch_description(): load_composable_nodes = LoadComposableNodes( condition=IfCondition(use_composition), - target_container=container_name, + target_container=container_name_full, composable_node_descriptions=[ ComposableNode( package='nav2_map_server', diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 2b391a024c1..8e04a040158 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -37,6 +37,7 @@ def generate_launch_description(): params_file = LaunchConfiguration('params_file') use_composition = LaunchConfiguration('use_composition') container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml') @@ -199,7 +200,7 @@ def generate_launch_description(): load_composable_nodes = LoadComposableNodes( condition=IfCondition(use_composition), - target_container=container_name, + target_container=container_name_full, composable_node_descriptions=[ ComposableNode( package='nav2_controller', diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 41e149be7f2..8d5bde45477 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -52,52 +52,59 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: @@ -179,19 +186,27 @@ local_costmap: height: 3 resolution: 0.05 robot_radius: 0.22 - plugins: ["obstacle_layer", "inflation_layer"] + plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.55 - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan scan: topic: /robot1/scan max_obstacle_height: 2.0 clearing: True marking: True + data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 obstacle_max_range: 2.5 @@ -199,28 +214,35 @@ local_costmap: static_layer: map_subscribe_transient_local: True always_send_full_costmap: True - observation_sources: scan global_costmap: global_costmap: ros__parameters: use_sim_time: True robot_radius: 0.22 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True + observation_sources: scan scan: topic: /robot1/scan max_obstacle_height: 2.0 clearing: True marking: True + data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 static_layer: + plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 always_send_full_costmap: True - observation_sources: scan map_server: ros__parameters: diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 0b22b7146bd..1a31306f677 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -52,52 +52,59 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: @@ -179,14 +186,21 @@ local_costmap: height: 3 resolution: 0.05 robot_radius: 0.22 - plugins: ["obstacle_layer", "inflation_layer"] + plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.55 - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan scan: topic: /robot2/scan max_obstacle_height: 2.0 @@ -199,28 +213,35 @@ local_costmap: static_layer: map_subscribe_transient_local: True always_send_full_costmap: True - observation_sources: scan global_costmap: global_costmap: ros__parameters: use_sim_time: True robot_radius: 0.22 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True + observation_sources: scan scan: topic: /robot2/scan max_obstacle_height: 2.0 clearing: True marking: True + data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 static_layer: + plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 always_send_full_costmap: True - observation_sources: scan map_server: ros__parameters: diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 049e78794b8..f0f47c496a0 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -5,7 +5,7 @@ - + diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp index e53bdce6abd..019e9ca6b05 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp @@ -175,6 +175,7 @@ class Navigator blackboard->set>("tf_buffer", feedback_utils.tf); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set>("odom_smoother", odom_smoother); // NOLINT return configure(parent_node, odom_smoother) && ok; } diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index f42f08f9d27..a58b529ef05 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -23,7 +23,7 @@ smoother_server: minimum_turning_radius: 0.40 # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots w_curve: 30.0 # weight to enforce minimum_turning_radius w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight - w_smooth: 15000.0 # weight to maximize smoothness of path + w_smooth: 2000000.0 # weight to maximize smoothness of path w_cost: 0.015 # weight to steer robot away from collision and cost # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp index 36c313cb092..637443ebdd0 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp @@ -72,7 +72,7 @@ struct SmootherParams node, local_name + "w_dist", rclcpp::ParameterValue(0.0)); node->get_parameter(local_name + "w_dist", distance_weight); nav2_util::declare_parameter_if_not_declared( - node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0)); + node, local_name + "w_smooth", rclcpp::ParameterValue(2000000.0)); node->get_parameter(local_name + "w_smooth", smooth_weight); nav2_util::declare_parameter_if_not_declared( node, local_name + "cost_check_points", rclcpp::ParameterValue(std::vector())); diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index f50d2fb780b..463af33fd08 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -59,7 +59,7 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber for (int j = 40; j < 100; ++j) { int dist_x = std::max(0, std::max(60 - j, j - 80)); int dist_y = std::max(0, std::max(30 - i, i - 40)); - double dist = sqrt(dist_x * dist_x + dist_y * dist_y); + double dist = sqrt(dist_x * dist_x + dist_y * dist_y) * costmap->metadata.resolution; unsigned char cost; if (dist == 0) { cost = nav2_costmap_2d::LETHAL_OBSTACLE; @@ -68,7 +68,7 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber } else { double factor = exp( - -1.0 * cost_scaling_factor * (dist * costmap->metadata.resolution - inscribed_radius)); + -1.0 * cost_scaling_factor * (dist - inscribed_radius)); cost = static_cast((nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1) * factor); } @@ -141,7 +141,7 @@ class SmootherTest : public ::testing::Test std::shared_ptr()); smoother_->activate(); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 2000000.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.minimum_turning_radius", 0.4)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 30.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_dist", 0.0)); @@ -614,7 +614,7 @@ TEST_F(SmootherTest, testingObstacleAvoidance) footprint.push_back(pointMsg(-0.4, -0.25)); footprint.push_back(pointMsg(0.4, -0.25)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 2000000.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); reloadParams(); @@ -644,7 +644,7 @@ TEST_F(SmootherTest, testingObstacleAvoidance) straight_near_obstacle, smoothed_path, cost_avoidance_criterion); EXPECT_GT(cost_avoidance_improvement, 0.0); - EXPECT_NEAR(cost_avoidance_improvement, 12.9, 1.0); + EXPECT_NEAR(cost_avoidance_improvement, 9.4, 1.0); } TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) @@ -723,6 +723,7 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) footprint.push_back(pointMsg(0.4, -0.2)); // first smooth with homogeneous w_cost to compare + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); // higher w_curve significantly decreases convergence speed here // path feasibility can be restored by subsequent resmoothing with higher w_curve @@ -809,7 +810,7 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) footprint.push_back(pointMsg(0.15, -0.2)); // reset parameters back to homogeneous and shift cost check point to the center of the footprint - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 1.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", -1.0)); @@ -995,7 +996,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) &mvmt_smoothness_criterion_out); // more poses -> smoother path EXPECT_GT(smoothness_improvement, 0.0); - EXPECT_NEAR(smoothness_improvement, 65.7, 1.0); + EXPECT_NEAR(smoothness_improvement, 63.9, 1.0); // upsample above original size node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 2)); @@ -1010,7 +1011,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) &mvmt_smoothness_criterion_out); // even more poses -> even smoother path EXPECT_GT(smoothness_improvement, 0.0); - EXPECT_NEAR(smoothness_improvement, 83.7, 1.0); + EXPECT_NEAR(smoothness_improvement, 82.2, 1.0); } TEST_F(SmootherTest, testingStartGoalOrientations) @@ -1032,7 +1033,7 @@ TEST_F(SmootherTest, testingStartGoalOrientations) double mvmt_smoothness_improvement = assessPathImprovement(sharp_turn_90, smoothed_path, mvmt_smoothness_criterion_); EXPECT_GT(mvmt_smoothness_improvement, 0.0); - EXPECT_NEAR(mvmt_smoothness_improvement, 53.3, 1.0); + EXPECT_NEAR(mvmt_smoothness_improvement, 55.2, 1.0); // no change in orientations EXPECT_NEAR(smoothed_path.front()[2], 0, 0.001); EXPECT_NEAR(smoothed_path.back()[2], M_PI / 2, 0.001); @@ -1049,10 +1050,10 @@ TEST_F(SmootherTest, testingStartGoalOrientations) mvmt_smoothness_improvement = assessPathImprovement(smoothed_path, smoothed_path_sg_overwritten, mvmt_smoothness_criterion_); EXPECT_GT(mvmt_smoothness_improvement, 0.0); - EXPECT_NEAR(mvmt_smoothness_improvement, 98.3, 1.0); + EXPECT_NEAR(mvmt_smoothness_improvement, 58.9, 1.0); // orientations adjusted to follow the path - EXPECT_NEAR(smoothed_path_sg_overwritten.front()[2], M_PI / 4, 0.1); - EXPECT_NEAR(smoothed_path_sg_overwritten.back()[2], M_PI / 4, 0.1); + EXPECT_NEAR(smoothed_path_sg_overwritten.front()[2], M_PI / 8, 0.1); + EXPECT_NEAR(smoothed_path_sg_overwritten.back()[2], 3 * M_PI / 8, 0.1); // test short paths std::vector short_screwed_path = diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 32899585199..2a52e296dad 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -410,6 +410,12 @@ void ControllerServer::computeControl() publishZeroVelocity(); action_server_->terminate_current(); return; + } catch (std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + action_server_->terminate_current(result); + return; } RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result"); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 72125143bae..fe3d7134ce3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -139,6 +139,12 @@ class Layer return current_; } + /**@brief Gets whether the layer is enabled. */ + bool isEnabled() const + { + return enabled_; + } + /** @brief Convenience function for layered_costmap_->getFootprint(). */ const std::vector & getFootprint() const; diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index bd3d37cd31e..3bfc03711ea 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -256,12 +256,12 @@ bool LayeredCostmap::isCurrent() for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - current_ = current_ && (*plugin)->isCurrent(); + current_ = current_ && ((*plugin)->isCurrent() || !(*plugin)->isEnabled()); } for (vector>::iterator filter = filters_.begin(); filter != filters_.end(); ++filter) { - current_ = current_ && (*filter)->isCurrent(); + current_ = current_ && ((*filter)->isCurrent() || !(*filter)->isEnabled()); } return current_; } diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index a844498ab83..b2a7f8df01f 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -46,8 +46,8 @@ class TaskResult(Enum): class BasicNavigator(Node): - def __init__(self): - super().__init__(node_name='basic_navigator') + def __init__(self, node_name='basic_navigator'): + super().__init__(node_name=node_name) self.initial_pose = PoseStamped() self.initial_pose.header.frame_id = 'map' self.goal_handle = None diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 79a77ce0823..4bbe5c55ca7 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -158,7 +158,7 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include ${OMPL_INCLUDE_DIRS}) ament_export_libraries(${library_name} ${library_name}_2d ${library_name}_lattice) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 83590304bff..1fdf154d6fa 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -317,6 +317,10 @@ void SmootherServer::smoothPlan() RCLCPP_ERROR(this->get_logger(), e.what()); action_server_->terminate_current(); return; + } catch (std::exception & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + action_server_->terminate_current(result); + return; } } diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index d7ede55cd18..34627223108 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -29,6 +29,8 @@ #include "tf2_ros/transform_listener.h" #include "tf2_ros/create_timer_ros.h" +#include "nav2_util/odometry_utils.hpp" + #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -51,6 +53,8 @@ class BehaviorTreeHandler tf_->setUsingDedicatedThread(true); tf_listener_ = std::make_shared(*tf_, node_, false); + odom_smoother_ = std::make_shared(node_); + const std::vector plugin_libs = { "nav2_compute_path_to_pose_action_bt_node", "nav2_compute_path_through_poses_action_bt_node", @@ -130,6 +134,7 @@ class BehaviorTreeHandler blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set>("odom_smoother", odom_smoother_); // NOLINT // set dummy goal on blackboard geometry_msgs::msg::PoseStamped goal; @@ -166,6 +171,8 @@ class BehaviorTreeHandler std::shared_ptr tf_; std::shared_ptr tf_listener_; + std::shared_ptr odom_smoother_; + BT::BehaviorTreeFactory factory_; }; diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 54706c92a64..fed748f0731 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -95,7 +95,6 @@ TEST(ThetaStarTest, test_theta_star) { planner_->setStartAndGoal(start, goal); EXPECT_TRUE(planner_->src_.x == s.x && planner_->src_.y == s.y); EXPECT_TRUE(planner_->dst_.x == g.x && planner_->dst_.y == g.y); - /// Check if the initializePosn function works properly planner_->uinitializePosn(size_x * size_y); EXPECT_EQ(planner_->getSizeOfNodePosition(), (size_x * size_y)); @@ -121,7 +120,7 @@ TEST(ThetaStarTest, test_theta_star) { EXPECT_FALSE(planner_->isSafe(10, 10)); // cost at this point is 253 (>LETHAL_COST) /// Check if the functions addIndex & getIndex work properly - coordsM c = {20, 30}; + coordsM c = {18, 18}; planner_->uaddToNodesData(0); planner_->uaddIndex(c.x, c.y); tree_node * c_node = planner_->ugetIndex(c.x, c.y); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 6573d74684f..17c9ee718d1 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -209,6 +209,11 @@ double VelocitySmoother::applyConstraints( void VelocitySmoother::smootherTimer() { + // Wait until the first command is received + if (!command_) { + return; + } + auto cmd_vel = std::make_unique(); // Check for velocity timeout. If nothing received, publish zeros to stop robot