From 3dd2ec665e7d1b1ee5d89e5c539c646c7be48561 Mon Sep 17 00:00:00 2001 From: Sandeep Dutta Date: Sat, 14 Sep 2024 11:00:20 -0700 Subject: [PATCH 1/6] added disengage_threshold --- .../nav2_rotation_shim_controller.hpp | 3 ++- .../src/nav2_rotation_shim_controller.cpp | 7 +++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index e5ecbad9e58..15ce9cebb5a 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -172,7 +172,8 @@ class RotationShimController : public nav2_core::Controller nav2_core::Controller::Ptr primary_controller_; bool path_updated_; nav_msgs::msg::Path current_path_; - double forward_sampling_distance_, angular_dist_threshold_; + double forward_sampling_distance_, angular_dist_threshold_, angular_dist_threshold_param_; + double angular_disengage_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; bool rotate_to_goal_heading_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 9e2485af7c4..1e61812133d 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -52,6 +52,8 @@ void RotationShimController::configure( double control_frequency; nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue(0.785)); // 45 deg + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".angular_disengage_threshold", rclcpp::ParameterValue(0.785/3.0)); // 15 deg nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( @@ -66,6 +68,9 @@ void RotationShimController::configure( node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); + angular_dist_threshold_param_ = angular_dist_threshold_; + node->get_parameter( + plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_); node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_); node->get_parameter( plugin_name_ + ".rotate_to_heading_angular_vel", @@ -191,11 +196,13 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands double angular_distance_to_heading = std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); if (fabs(angular_distance_to_heading) > angular_dist_threshold_) { + angular_dist_threshold_ = angular_disengage_threshold_; RCLCPP_DEBUG( logger_, "Robot is not within the new path's rough heading, rotating to heading..."); return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); } else { + angular_dist_threshold_ = angular_dist_threshold_param_; RCLCPP_DEBUG( logger_, "Robot is at the new path's rough heading, passing to controller"); From 3118e9158d22183846758c8bf843499cce1965f8 Mon Sep 17 00:00:00 2001 From: Sandeep Dutta Date: Fri, 27 Sep 2024 15:56:04 -0700 Subject: [PATCH 2/6] added global costmap clearing once it reaches goal --- .../navigate_to_pose_w_replanning_and_recovery.xml | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 2396b844aed..31837e9d38c 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -15,7 +15,10 @@ - + + + + @@ -26,9 +29,9 @@ - - + + From f27417cd98ee266571944189bfcfde310c00e060 Mon Sep 17 00:00:00 2001 From: Sandeep Dutta Date: Wed, 9 Oct 2024 10:12:50 -0700 Subject: [PATCH 3/6] fixed merge error in rotaton_shim_controller --- .../src/nav2_rotation_shim_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index fb506dda10c..de8ce424e29 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -205,7 +205,6 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands in_rotation_ = true; return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); } else { - angular_dist_threshold_ = angular_dist_threshold_param_; RCLCPP_DEBUG( logger_, "Robot is at the new path's rough heading, passing to controller"); From e7d15567064fc9e328f8eb29dc2f9dc76b915605 Mon Sep 17 00:00:00 2001 From: Sandeep Dutta Date: Mon, 14 Apr 2025 14:59:27 -0700 Subject: [PATCH 4/6] fixed pose header frame_id in nav2_navfn_planner --- nav2_navfn_planner/src/navfn_planner.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index d15ae89622c..a78027ffe7f 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -418,6 +418,7 @@ NavfnPlanner::getPlanFromPotential( mapToWorld(x[i], y[i], world_x, world_y); geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; pose.pose.position.x = world_x; pose.pose.position.y = world_y; pose.pose.position.z = 0.0; From 0dda7d4789330c76e0bdfac4eeed3a77d99e3f78 Mon Sep 17 00:00:00 2001 From: Sandeep Dutta Date: Wed, 16 Apr 2025 07:36:59 -0700 Subject: [PATCH 5/6] reverted change to nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml --- .../navigate_to_pose_w_replanning_and_recovery.xml | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 31837e9d38c..2396b844aed 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -15,10 +15,7 @@ - - - - + @@ -29,9 +26,9 @@ - - + + From ed1c560d490e85dca22e3950ccba69251313d6a2 Mon Sep 17 00:00:00 2001 From: Sandeep Dutta Date: Wed, 16 Apr 2025 13:19:14 -0700 Subject: [PATCH 6/6] one more spot for the header update --- nav2_navfn_planner/src/navfn_planner.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index a78027ffe7f..8adca5617e9 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -363,6 +363,7 @@ NavfnPlanner::smoothApproachToGoal( } geometry_msgs::msg::PoseStamped goal_copy; goal_copy.pose = goal; + goal_copy.header = plan.header; plan.poses.push_back(goal_copy); }