From df791eb40f22b29b81328d37b2d57f7366022b3a Mon Sep 17 00:00:00 2001 From: song6-1 Date: Sun, 30 Nov 2025 13:37:55 +0900 Subject: [PATCH 1/2] synk fork and retry test Signed-off-by: song6-1 --- .../src/parameter_handler.cpp | 201 ++++++------------ 1 file changed, 65 insertions(+), 136 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 31bc7b7f19d..24bb2279011 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -24,7 +24,6 @@ namespace nav2_regulated_pure_pursuit_controller { -using nav2::declare_parameter_if_not_declared; using rcl_interfaces::msg::ParameterType; ParameterHandler::ParameterHandler( @@ -35,140 +34,71 @@ ParameterHandler::ParameterHandler( { plugin_name_ = plugin_name; - declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", - rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".approach_velocity_scaling_dist", - rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_distance_to_obstacle", - rclcpp::ParameterValue(-1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_fixed_curvature_lookahead", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".curvature_lookahead_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(costmap_size_x / 2.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".interpolate_curvature_after_goal", - rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_collision_detection", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); - - node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); + params_.desired_linear_vel = + node->declare_or_get_parameter(plugin_name_ + ".desired_linear_vel", 0.5); params_.base_desired_linear_vel = params_.desired_linear_vel; - node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); - node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); - node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); - node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time); - node->get_parameter( - plugin_name_ + ".rotate_to_heading_angular_vel", - params_.rotate_to_heading_angular_vel); - node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); - node->get_parameter( - plugin_name_ + ".use_velocity_scaled_lookahead_dist", - params_.use_velocity_scaled_lookahead_dist); - node->get_parameter( - plugin_name_ + ".min_approach_linear_velocity", - params_.min_approach_linear_velocity); - node->get_parameter( - plugin_name_ + ".approach_velocity_scaling_dist", - params_.approach_velocity_scaling_dist); + params_.lookahead_dist = + node->declare_or_get_parameter(plugin_name_ + ".lookahead_dist", 0.6); + params_.min_lookahead_dist = + node->declare_or_get_parameter(plugin_name_ + ".min_lookahead_dist", 0.3); + params_.max_lookahead_dist = + node->declare_or_get_parameter(plugin_name_ + ".max_lookahead_dist", 0.9); + params_.lookahead_time = + node->declare_or_get_parameter(plugin_name_ + ".lookahead_time", 1.5); + params_.rotate_to_heading_angular_vel = node->declare_or_get_parameter( + plugin_name_ + ".rotate_to_heading_angular_vel", 1.8); + params_.transform_tolerance = node->declare_or_get_parameter( + plugin_name_ + ".transform_tolerance", 0.1); + params_.use_velocity_scaled_lookahead_dist = node->declare_or_get_parameter( + plugin_name_ + ".use_velocity_scaled_lookahead_dist", false); + params_.min_approach_linear_velocity = node->declare_or_get_parameter( + plugin_name_ + ".min_approach_linear_velocity", 0.05); + params_.approach_velocity_scaling_dist = node->declare_or_get_parameter( + plugin_name_ + ".approach_velocity_scaling_dist", 0.6); if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) { RCLCPP_WARN( logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " "leading to permanent slowdown"); } - node->get_parameter( - plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - params_.max_allowed_time_to_collision_up_to_carrot); - node->get_parameter( - plugin_name_ + ".min_distance_to_obstacle", - params_.min_distance_to_obstacle); - node->get_parameter( - plugin_name_ + ".use_regulated_linear_velocity_scaling", - params_.use_regulated_linear_velocity_scaling); - node->get_parameter( - plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - params_.use_cost_regulated_linear_velocity_scaling); - node->get_parameter(plugin_name_ + ".cost_scaling_dist", params_.cost_scaling_dist); - node->get_parameter(plugin_name_ + ".cost_scaling_gain", params_.cost_scaling_gain); - node->get_parameter( - plugin_name_ + ".inflation_cost_scaling_factor", - params_.inflation_cost_scaling_factor); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_radius", - params_.regulated_linear_scaling_min_radius); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_speed", - params_.regulated_linear_scaling_min_speed); - node->get_parameter( - plugin_name_ + ".use_fixed_curvature_lookahead", - params_.use_fixed_curvature_lookahead); - node->get_parameter( - plugin_name_ + ".curvature_lookahead_dist", - params_.curvature_lookahead_dist); - node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading); - node->get_parameter( - plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); - node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); - node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); - node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); - node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); - node->get_parameter( - plugin_name_ + ".max_robot_pose_search_dist", - params_.max_robot_pose_search_dist); + params_.max_allowed_time_to_collision_up_to_carrot = + node->declare_or_get_parameter( + plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", 1.0); + params_.min_distance_to_obstacle = node->declare_or_get_parameter( + plugin_name_ + ".min_distance_to_obstacle", -1.0); + params_.use_regulated_linear_velocity_scaling = + node->declare_or_get_parameter( + plugin_name_ + ".use_regulated_linear_velocity_scaling", true); + params_.use_cost_regulated_linear_velocity_scaling = + node->declare_or_get_parameter( + plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", true); + params_.cost_scaling_dist = + node->declare_or_get_parameter(plugin_name_ + ".cost_scaling_dist", 0.6); + params_.cost_scaling_gain = + node->declare_or_get_parameter(plugin_name_ + ".cost_scaling_gain", 1.0); + params_.inflation_cost_scaling_factor = node->declare_or_get_parameter( + plugin_name_ + ".inflation_cost_scaling_factor", 3.0); + params_.regulated_linear_scaling_min_radius = node->declare_or_get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_radius", 0.90); + params_.regulated_linear_scaling_min_speed = node->declare_or_get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_speed", 0.25); + params_.use_fixed_curvature_lookahead = node->declare_or_get_parameter( + plugin_name_ + ".use_fixed_curvature_lookahead", false); + params_.curvature_lookahead_dist = node->declare_or_get_parameter( + plugin_name_ + ".curvature_lookahead_dist", 0.6); + params_.use_rotate_to_heading = node->declare_or_get_parameter( + plugin_name_ + ".use_rotate_to_heading", true); + params_.rotate_to_heading_min_angle = node->declare_or_get_parameter( + plugin_name_ + ".rotate_to_heading_min_angle", 0.785); + params_.max_angular_accel = + node->declare_or_get_parameter(plugin_name_ + ".max_angular_accel", 3.2); + params_.use_cancel_deceleration = node->declare_or_get_parameter( + plugin_name_ + ".use_cancel_deceleration", false); + params_.cancel_deceleration = node->declare_or_get_parameter( + plugin_name_ + ".cancel_deceleration", 3.2); + params_.allow_reversing = + node->declare_or_get_parameter(plugin_name_ + ".allow_reversing", false); + params_.max_robot_pose_search_dist = node->declare_or_get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", costmap_size_x / 2.0); if (params_.max_robot_pose_search_dist < 0.0) { RCLCPP_WARN( logger_, "Max robot search distance is negative, setting to max to search" @@ -176,19 +106,18 @@ ParameterHandler::ParameterHandler( params_.max_robot_pose_search_dist = std::numeric_limits::max(); } - node->get_parameter( - plugin_name_ + ".interpolate_curvature_after_goal", - params_.interpolate_curvature_after_goal); + params_.interpolate_curvature_after_goal = node->declare_or_get_parameter( + plugin_name_ + ".interpolate_curvature_after_goal", false); if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) { RCLCPP_WARN( logger_, "For interpolate_curvature_after_goal to be set to true, " "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling."); params_.interpolate_curvature_after_goal = false; } - node->get_parameter( - plugin_name_ + ".use_collision_detection", - params_.use_collision_detection); - node->get_parameter(plugin_name_ + ".stateful", params_.stateful); + params_.use_collision_detection = node->declare_or_get_parameter( + plugin_name_ + ".use_collision_detection", true); + params_.stateful = + node->declare_or_get_parameter(plugin_name_ + ".stateful", true); if (params_.inflation_cost_scaling_factor <= 0.0) { RCLCPP_WARN( From b1b9172304eb1e8fa566d59940c0f1a53c77a898 Mon Sep 17 00:00:00 2001 From: song6-1 Date: Sun, 30 Nov 2025 14:52:42 +0900 Subject: [PATCH 2/2] ci: retry tests Signed-off-by: song6-1