diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp index e53d6e4c35..729271a8c5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp @@ -18,6 +18,7 @@ #include #include "nav2_msgs/action/compute_path_to_pose.hpp" +#include "nav2_msgs/action/compute_path_through_poses.hpp" #include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp" namespace nav2_behavior_tree @@ -27,6 +28,8 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent { using Action = nav2_msgs::action::ComputePathToPose; using ActionResult = Action::Result; + using ThroughAction = nav2_msgs::action::ComputePathThroughPoses; + using ThroughActionResult = Action::Result; public: WouldAPlannerRecoveryHelp( diff --git a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp index 6a8d3f33ca..91de9c2e22 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp @@ -26,7 +26,10 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp( error_codes_to_check_ = { ActionResult::UNKNOWN, ActionResult::NO_VALID_PATH, - ActionResult::TIMEOUT + ActionResult::TIMEOUT, + ThroughActionResult::UNKNOWN, + ThroughActionResult::TIMEOUT, + ThroughActionResult::NO_VALID_PATH }; } diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 39d72a5297..5f0e6e775d 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -199,7 +199,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -256,7 +256,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True # The yaml_filename does not need to be specified since it going to be set by defaults in launch. @@ -362,7 +362,7 @@ collision_monitor: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 2.0 + time_before_collision: 1.2 simulation_time_step: 0.1 min_points: 6 visualize: False diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 9257836be8..ebbe3cbbe5 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -13,26 +13,38 @@ - + + + + - + + + + - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 13c314205b..4f5a9398d1 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -104,7 +104,7 @@ This process is then repeated a number of times and returns a converged solution | critical_weight | double | Default 20.0. Weight to apply to critic for near collisions closer than `collision_margin_distance` to prevent near collisions **only** as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from critical collisions. | | repulsion_weight | double | Default 1.5. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the `inflation_radius` distance from obstacles. | | cost_power | int | Default 1. Power order to apply to term. | - | collision_cost | double | Default 10000.0. Cost to apply to a true collision in a trajectory. | + | collision_cost | double | Default 100000.0. Cost to apply to a true collision in a trajectory. | | collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. | | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. @@ -124,11 +124,11 @@ Note: There is a "Legacy" version of this critic also available with the same pa #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | - | cost_weight | double | Default 2.0. Weight to apply to critic term. | + | cost_weight | double | Default 2.2. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered | | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | - | max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered | + | max_angle_to_furthest | double | Default 0.785398. Angular distance between robot and goal above which path angle cost starts being considered | | mode | int | Default 0 (Forward Preference). Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires. 0: Forward Preference, penalizes high path angles relative to the robot's orientation to incentivize turning towards the path. 1: No directional preference, penalizes high path angles relative to the robot's orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable. 2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path's requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction. | diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index b8d6c89870..274315a06b 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -25,7 +26,7 @@ void ObstaclesCritic::initialize() getParam(power_, "cost_power", 1); getParam(repulsion_weight_, "repulsion_weight", 1.5); getParam(critical_weight_, "critical_weight", 20.0); - getParam(collision_cost_, "collision_cost", 10000.0); + getParam(collision_cost_, "collision_cost", 100000.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.10); getParam(near_goal_distance_, "near_goal_distance", 0.5); @@ -160,8 +161,11 @@ void ObstaclesCritic::score(CriticData & data) // Let near-collision trajectory points be punished severely if (dist_to_obj < collision_margin_distance_) { traj_cost += (collision_margin_distance_ - dist_to_obj); - } else if (!near_goal) { // Generally prefer trajectories further from obstacles - repulsive_cost[i] += (inflation_radius_ - dist_to_obj); + } + + // Generally prefer trajectories further from obstacles + if (!near_goal) { + repulsive_cost[i] += inflation_radius_ - dist_to_obj; } } @@ -169,9 +173,14 @@ void ObstaclesCritic::score(CriticData & data) raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost; } + // Normalize repulsive cost by trajectory length & lowest score to not overweight importance + // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors + auto && repulsive_cost_normalized = + (repulsive_cost - xt::amin(repulsive_cost, immediate)) / traj_len; + data.costs += xt::pow( (critical_weight_ * raw_cost) + - (repulsion_weight_ * repulsive_cost / traj_len), + (repulsion_weight_ * repulsive_cost_normalized), power_); data.fail_flag = all_trajectories_collide; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index cd701ea093..d334b81673 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -36,13 +36,13 @@ void PathAngleCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(offset_from_furthest_, "offset_from_furthest", 4); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 2.0); + getParam(weight_, "cost_weight", 2.2); getParam( threshold_to_consider_, "threshold_to_consider", 0.5); getParam( max_angle_to_furthest_, - "max_angle_to_furthest", 1.2); + "max_angle_to_furthest", 0.785398); int mode = 0; getParam(mode, "mode", mode); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index f88249aed1..08f50945fd 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -282,7 +282,7 @@ TEST(CriticTests, PathAngleCritic) path.y(6) = 4.0; critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); - EXPECT_NEAR(costs(0), 3.6315, 1e-2); // atan2(4,-1) [1.81] * 2.0 weight + EXPECT_NEAR(costs(0), 3.9947, 1e-2); // atan2(4,-1) [1.81] * 2.2 weight // Set mode to no directional preferences + reset costs critic.setMode(1); @@ -306,7 +306,7 @@ TEST(CriticTests, PathAngleCritic) critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); // should use reverse orientation as the closer angle in no dir preference mode - EXPECT_NEAR(costs(0), 2.6516, 1e-2); + EXPECT_NEAR(costs(0), 2.9167, 1e-2); // Set mode to consider path directionality + reset costs critic.setMode(2); @@ -330,7 +330,7 @@ TEST(CriticTests, PathAngleCritic) critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); // should use reverse orientation as the closer angle in no dir preference mode - EXPECT_NEAR(costs(0), 2.6516, 1e-2); + EXPECT_NEAR(costs(0), 2.9167, 1e-2); PathAngleMode mode; mode = PathAngleMode::FORWARD_PREFERENCE;