From 3bb9441241420d4f88b1d6eedf243b672734a374 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 18 Mar 2024 16:50:47 +0900 Subject: [PATCH 1/5] Add param reconfig to goal planner Signed-off-by: Daniel Sanchez --- .../src/manager.cpp | 441 +++++++++++++++++- 1 file changed, 440 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index b27c604536e8c..29ea4c6b5b0de 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -427,7 +427,446 @@ void GoalPlannerModuleManager::updateModuleParams( auto & p = parameters_; - [[maybe_unused]] const std::string ns = name_ + "."; + const std::string base_ns = "goal_planner."; + // general params + + { + updateParam(parameters, base_ns + "th_stopped_velocity", p->th_stopped_velocity); + updateParam(parameters, base_ns + "th_arrived_distance", p->th_arrived_distance); + updateParam( + parameters, base_ns + "center_line_path_interval", p->center_line_path_interval); + } + // goal search + + { + const std::string ns = base_ns + "goal_search."; + updateParam( + parameters, ns + "minimum_weighted_distance.lateral_weight", + p->minimum_weighted_distance_lateral_weight); + updateParam( + parameters, ns + "prioritize_goals_before_objects", p->prioritize_goals_before_objects); + updateParam( + parameters, ns + "forward_goal_search_length", p->forward_goal_search_length); + updateParam( + parameters, ns + "backward_goal_search_length", p->backward_goal_search_length); + updateParam(parameters, ns + "goal_search_interval", p->goal_search_interval); + updateParam(parameters, ns + "longitudinal_margin", p->longitudinal_margin); + updateParam(parameters, ns + "max_lateral_offset", p->max_lateral_offset); + updateParam(parameters, ns + "lateral_offset_interval", p->lateral_offset_interval); + updateParam( + parameters, ns + "ignore_distance_from_lane_start", p->ignore_distance_from_lane_start); + updateParam(parameters, ns + "margin_from_boundary", p->margin_from_boundary); + + std::string parking_policy_name; + updateParam(parameters, ns + "parking_policy", parking_policy_name); + if (parking_policy_name == "left_side") { + p->parking_policy = ParkingPolicy::LEFT_SIDE; + } else if (parking_policy_name == "right_side") { + p->parking_policy = ParkingPolicy::RIGHT_SIDE; + } else { + RCLCPP_WARN_STREAM( + node_->get_logger().get_child(name()), + "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); + } + } + + // occupancy grid map + { + const std::string ns = base_ns + "occupancy_grid."; + updateParam( + parameters, ns + "use_occupancy_grid_for_goal_search", p->use_occupancy_grid_for_goal_search); + updateParam( + parameters, ns + "use_occupancy_grid_for_path_collision_check", + p->use_occupancy_grid_for_path_collision_check); + updateParam( + parameters, ns + "use_occupancy_grid_for_goal_longitudinal_margin", + p->use_occupancy_grid_for_goal_longitudinal_margin); + updateParam( + parameters, ns + "occupancy_grid_collision_check_margin", + p->occupancy_grid_collision_check_margin); + updateParam(parameters, ns + "theta_size", p->theta_size); + updateParam(parameters, ns + "obstacle_threshold", p->obstacle_threshold); + } + + // object recognition + { + const std::string ns = base_ns + "object_recognition."; + + updateParam(parameters, ns + "use_object_recognition", p->use_object_recognition); + updateParam( + parameters, ns + "object_recognition_collision_check_max_extra_stopping_margin", + p->object_recognition_collision_check_max_extra_stopping_margin); + updateParam(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); + updateParam(parameters, ns + "detection_bound_offset", p->detection_bound_offset); + updateParam(parameters, ns + "outer_road_detection_offset", p->outer_road_detection_offset); + updateParam(parameters, ns + "inner_road_detection_offset", p->inner_road_detection_offset); + + std::vector temp_object_recognition_collision_check_soft_margins; + std::vector temp_object_recognition_collision_check_hard_margins; + updateParam>( + parameters, ns + "collision_check_soft_margins", + temp_object_recognition_collision_check_soft_margins); + updateParam>( + parameters, ns + "collision_check_hard_margins", + temp_object_recognition_collision_check_hard_margins); + + // validate object recognition collision check margins + if ( + temp_object_recognition_collision_check_soft_margins.empty() || + temp_object_recognition_collision_check_hard_margins.empty()) { + RCLCPP_WARN_STREAM( + node_->get_logger().get_child(name()), + "object_recognition.collision_check_soft_margins and " + << "object_recognition.collision_check_hard_margins must not be empty. " << std::endl); + } else { + p->object_recognition_collision_check_soft_margins = + temp_object_recognition_collision_check_soft_margins; + p->object_recognition_collision_check_hard_margins = + temp_object_recognition_collision_check_hard_margins; + } + } + + // pull over general params + { + const std::string ns = base_ns + "pull_over."; + + updateParam( + parameters, ns + "minimum_request_length", p->pull_over_minimum_request_length); + updateParam(parameters, ns + "pull_over_velocity", p->pull_over_velocity); + updateParam( + parameters, ns + "pull_over_minimum_velocity", p->pull_over_minimum_velocity); + updateParam(parameters, ns + "decide_path_distance", p->decide_path_distance); + + double temp_maximum_deceleration; + updateParam(parameters, ns + "maximum_deceleration", temp_maximum_deceleration); + if (temp_maximum_deceleration < 0.0) { + RCLCPP_WARN_STREAM( + node_->get_logger().get_child(name()), + "maximum_deceleration cannot be negative value. Given parameter: " + << temp_maximum_deceleration << std::endl); + } else { + p->maximum_deceleration = temp_maximum_deceleration; + } + + updateParam(parameters, ns + "maximum_jerk", p->maximum_jerk); + } + + // shift parking + { + const std::string ns = base_ns + "pull_over.shift_parking."; + updateParam(parameters, ns + "enable_shift_parking", p->enable_shift_parking); + int temp_shift_sampling_num; + updateParam(parameters, ns + "shift_sampling_num", temp_shift_sampling_num); + // validation of parameters + if (temp_shift_sampling_num < 1) { + RCLCPP_WARN_STREAM( + node_->get_logger().get_child(name()), + "shift_sampling_num must be positive integer. Given parameter: " << temp_shift_sampling_num + << std::endl); + } else { + p->shift_sampling_num = temp_shift_sampling_num; + } + + updateParam(parameters, ns + "maximum_lateral_jerk", p->maximum_lateral_jerk); + updateParam(parameters, ns + "minimum_lateral_jerk", p->minimum_lateral_jerk); + updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); + updateParam( + parameters, ns + "after_shift_straight_distance", p->after_shift_straight_distance); + } + + // parallel parking common + { + const std::string ns = base_ns + "pull_over.parallel_parking."; + p->parallel_parking_parameters.center_line_path_interval = + p->center_line_path_interval; // for geometric parallel parking + } + + // forward parallel parking forward + { + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; + updateParam(parameters, ns + "enable_arc_forward_parking", p->enable_arc_forward_parking); + updateParam( + parameters, ns + "after_forward_parking_straight_distance", + p->parallel_parking_parameters.after_forward_parking_straight_distance); + updateParam( + parameters, ns + "forward_parking_velocity", + p->parallel_parking_parameters.forward_parking_velocity); + updateParam( + parameters, ns + "forward_parking_lane_departure_margin", + p->parallel_parking_parameters.forward_parking_lane_departure_margin); + updateParam( + parameters, ns + "forward_parking_path_interval", + p->parallel_parking_parameters.forward_parking_path_interval); + updateParam( + parameters, ns + "forward_parking_max_steer_angle", + p->parallel_parking_parameters.forward_parking_max_steer_angle); + } + + // forward parallel parking backward + { + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; + updateParam( + parameters, ns + "enable_arc_backward_parking", p->enable_arc_backward_parking); + updateParam( + parameters, ns + "after_backward_parking_straight_distance", + p->parallel_parking_parameters.after_backward_parking_straight_distance); + updateParam( + parameters, ns + "backward_parking_velocity", + p->parallel_parking_parameters.backward_parking_velocity); + updateParam( + parameters, ns + "backward_parking_lane_departure_margin", + p->parallel_parking_parameters.backward_parking_lane_departure_margin); + updateParam( + parameters, ns + "backward_parking_path_interval", + p->parallel_parking_parameters.backward_parking_path_interval); + updateParam( + parameters, ns + "backward_parking_max_steer_angle", + p->parallel_parking_parameters.backward_parking_max_steer_angle); + } + + // freespace parking general params + { + const std::string ns = base_ns + "pull_over.freespace_parking."; + updateParam(parameters, ns + "enable_freespace_parking", p->enable_freespace_parking); + updateParam(parameters, ns + "velocity", p->freespace_parking_velocity); + + updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); + updateParam( + parameters, ns + "time_limit", p->freespace_parking_common_parameters.time_limit); + updateParam( + parameters, ns + "minimum_turning_radius", + p->freespace_parking_common_parameters.minimum_turning_radius); + updateParam( + parameters, ns + "maximum_turning_radius", + p->freespace_parking_common_parameters.maximum_turning_radius); + updateParam( + parameters, ns + "turning_radius_size", + p->freespace_parking_common_parameters.turning_radius_size); + p->freespace_parking_common_parameters.maximum_turning_radius = std::max( + p->freespace_parking_common_parameters.maximum_turning_radius, + p->freespace_parking_common_parameters.minimum_turning_radius); + p->freespace_parking_common_parameters.turning_radius_size = + std::max(p->freespace_parking_common_parameters.turning_radius_size, 1); + } + + // freespace parking search config + { + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; + updateParam( + parameters, ns + "theta_size", p->freespace_parking_common_parameters.theta_size); + updateParam( + parameters, ns + "angle_goal_range", p->freespace_parking_common_parameters.angle_goal_range); + updateParam( + parameters, ns + "curve_weight", p->freespace_parking_common_parameters.curve_weight); + updateParam( + parameters, ns + "reverse_weight", p->freespace_parking_common_parameters.reverse_weight); + updateParam( + parameters, ns + "lateral_goal_range", + p->freespace_parking_common_parameters.lateral_goal_range); + updateParam( + parameters, ns + "longitudinal_goal_range", + p->freespace_parking_common_parameters.longitudinal_goal_range); + } + + // freespace parking costmap configs + { + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; + updateParam( + parameters, ns + "obstacle_threshold", + p->freespace_parking_common_parameters.obstacle_threshold); + } + + // freespace parking astar + + { + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; + updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); + updateParam( + parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); + updateParam( + parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time); + updateParam(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); + updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); + } + + // stop condition + { + updateParam( + parameters, base_ns + "stop_condition.maximum_deceleration_for_stop", + p->maximum_deceleration_for_stop); + updateParam( + parameters, base_ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); + } + + const std::string path_safety_check_ns = "goal_planner.path_safety_check."; + const std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + + // EgoPredictedPath + { + updateParam( + parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity); + updateParam( + parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration); + updateParam( + parameters, ego_path_ns + "time_horizon_for_front_object", + p->ego_predicted_path_params.time_horizon_for_front_object); + updateParam( + parameters, ego_path_ns + "time_horizon_for_rear_object", + p->ego_predicted_path_params.time_horizon_for_rear_object); + updateParam( + parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution); + updateParam( + parameters, ego_path_ns + "delay_until_departure", + p->ego_predicted_path_params.delay_until_departure); + } + + // ObjectFilteringParams + const std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + updateParam( + parameters, obj_filter_ns + "safety_check_time_horizon", + p->objects_filtering_params.safety_check_time_horizon); + updateParam( + parameters, obj_filter_ns + "safety_check_time_resolution", + p->objects_filtering_params.safety_check_time_resolution); + updateParam( + parameters, obj_filter_ns + "object_check_forward_distance", + p->objects_filtering_params.object_check_forward_distance); + updateParam( + parameters, obj_filter_ns + "object_check_backward_distance", + p->objects_filtering_params.object_check_backward_distance); + updateParam( + parameters, obj_filter_ns + "ignore_object_velocity_threshold", + p->objects_filtering_params.ignore_object_velocity_threshold); + updateParam( + parameters, obj_filter_ns + "include_opposite_lane", + p->objects_filtering_params.include_opposite_lane); + updateParam( + parameters, obj_filter_ns + "invert_opposite_lane", + p->objects_filtering_params.invert_opposite_lane); + updateParam( + parameters, obj_filter_ns + "check_all_predicted_path", + p->objects_filtering_params.check_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_all_predicted_path", + p->objects_filtering_params.use_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_predicted_path_outside_lanelet", + p->objects_filtering_params.use_predicted_path_outside_lanelet); + } + + // ObjectTypesToCheck + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->objects_filtering_params.object_types_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->objects_filtering_params.object_types_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->objects_filtering_params.object_types_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->objects_filtering_params.object_types_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->objects_filtering_params.object_types_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->objects_filtering_params.object_types_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->objects_filtering_params.object_types_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->objects_filtering_params.object_types_to_check.check_pedestrian); + } + // ObjectLaneConfiguration + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + updateParam( + parameters, obj_lane_ns + "check_current_lane", + p->objects_filtering_params.object_lane_configuration.check_current_lane); + updateParam( + parameters, obj_lane_ns + "check_right_side_lane", + p->objects_filtering_params.object_lane_configuration.check_right_lane); + updateParam( + parameters, obj_lane_ns + "check_left_side_lane", + p->objects_filtering_params.object_lane_configuration.check_left_lane); + updateParam( + parameters, obj_lane_ns + "check_shoulder_lane", + p->objects_filtering_params.object_lane_configuration.check_shoulder_lane); + updateParam( + parameters, obj_lane_ns + "check_other_lane", + p->objects_filtering_params.object_lane_configuration.check_other_lane); + } + + // SafetyCheckParams + const std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + updateParam( + parameters, safety_check_ns + "enable_safety_check", + p->safety_check_params.enable_safety_check); + updateParam( + parameters, safety_check_ns + "keep_unsafe_time", p->safety_check_params.keep_unsafe_time); + updateParam( + parameters, safety_check_ns + "hysteresis_factor_expand_rate", + p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "backward_path_length", + p->safety_check_params.backward_path_length); + updateParam( + parameters, safety_check_ns + "forward_path_length", + p->safety_check_params.forward_path_length); + updateParam( + parameters, safety_check_ns + "publish_debug_marker", + p->safety_check_params.publish_debug_marker); + } + + // RSSparams + const std::string rss_ns = safety_check_ns + "rss_params."; + { + updateParam( + parameters, rss_ns + "rear_vehicle_reaction_time", + p->safety_check_params.rss_params.rear_vehicle_reaction_time); + updateParam( + parameters, rss_ns + "rear_vehicle_safety_time_margin", + p->safety_check_params.rss_params.rear_vehicle_safety_time_margin); + updateParam( + parameters, rss_ns + "lateral_distance_max_threshold", + p->safety_check_params.rss_params.lateral_distance_max_threshold); + updateParam( + parameters, rss_ns + "longitudinal_distance_min_threshold", + p->safety_check_params.rss_params.longitudinal_distance_min_threshold); + updateParam( + parameters, rss_ns + "longitudinal_velocity_delta_time", + p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + } + + // IntegralPredictedPolygonParams + const std::string integral_ns = safety_check_ns + "integral_predicted_polygon_params."; + { + updateParam( + parameters, integral_ns + "forward_margin", + p->safety_check_params.integral_predicted_polygon_params.forward_margin); + updateParam( + parameters, integral_ns + "backward_margin", + p->safety_check_params.integral_predicted_polygon_params.backward_margin); + updateParam( + parameters, integral_ns + "lat_margin", + p->safety_check_params.integral_predicted_polygon_params.lat_margin); + updateParam( + parameters, integral_ns + "time_horizon", + p->safety_check_params.integral_predicted_polygon_params.time_horizon); + } + + // debug + { + const std::string ns = base_ns + "debug."; + updateParam(parameters, ns + "print_debug_info", p->print_debug_info); + } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); From 804f8f88622c20e33c9f852cdb50f125e99f34ba Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 18 Mar 2024 17:13:53 +0900 Subject: [PATCH 2/5] Using the Warn params is bugged, wont fix Signed-off-by: Daniel Sanchez --- .../src/manager.cpp | 60 ------------------- 1 file changed, 60 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 29ea4c6b5b0de..20f5938a2ee2e 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -456,18 +456,6 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "ignore_distance_from_lane_start", p->ignore_distance_from_lane_start); updateParam(parameters, ns + "margin_from_boundary", p->margin_from_boundary); - - std::string parking_policy_name; - updateParam(parameters, ns + "parking_policy", parking_policy_name); - if (parking_policy_name == "left_side") { - p->parking_policy = ParkingPolicy::LEFT_SIDE; - } else if (parking_policy_name == "right_side") { - p->parking_policy = ParkingPolicy::RIGHT_SIDE; - } else { - RCLCPP_WARN_STREAM( - node_->get_logger().get_child(name()), - "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); - } } // occupancy grid map @@ -500,30 +488,6 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "detection_bound_offset", p->detection_bound_offset); updateParam(parameters, ns + "outer_road_detection_offset", p->outer_road_detection_offset); updateParam(parameters, ns + "inner_road_detection_offset", p->inner_road_detection_offset); - - std::vector temp_object_recognition_collision_check_soft_margins; - std::vector temp_object_recognition_collision_check_hard_margins; - updateParam>( - parameters, ns + "collision_check_soft_margins", - temp_object_recognition_collision_check_soft_margins); - updateParam>( - parameters, ns + "collision_check_hard_margins", - temp_object_recognition_collision_check_hard_margins); - - // validate object recognition collision check margins - if ( - temp_object_recognition_collision_check_soft_margins.empty() || - temp_object_recognition_collision_check_hard_margins.empty()) { - RCLCPP_WARN_STREAM( - node_->get_logger().get_child(name()), - "object_recognition.collision_check_soft_margins and " - << "object_recognition.collision_check_hard_margins must not be empty. " << std::endl); - } else { - p->object_recognition_collision_check_soft_margins = - temp_object_recognition_collision_check_soft_margins; - p->object_recognition_collision_check_hard_margins = - temp_object_recognition_collision_check_hard_margins; - } } // pull over general params @@ -536,18 +500,6 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "pull_over_minimum_velocity", p->pull_over_minimum_velocity); updateParam(parameters, ns + "decide_path_distance", p->decide_path_distance); - - double temp_maximum_deceleration; - updateParam(parameters, ns + "maximum_deceleration", temp_maximum_deceleration); - if (temp_maximum_deceleration < 0.0) { - RCLCPP_WARN_STREAM( - node_->get_logger().get_child(name()), - "maximum_deceleration cannot be negative value. Given parameter: " - << temp_maximum_deceleration << std::endl); - } else { - p->maximum_deceleration = temp_maximum_deceleration; - } - updateParam(parameters, ns + "maximum_jerk", p->maximum_jerk); } @@ -555,18 +507,6 @@ void GoalPlannerModuleManager::updateModuleParams( { const std::string ns = base_ns + "pull_over.shift_parking."; updateParam(parameters, ns + "enable_shift_parking", p->enable_shift_parking); - int temp_shift_sampling_num; - updateParam(parameters, ns + "shift_sampling_num", temp_shift_sampling_num); - // validation of parameters - if (temp_shift_sampling_num < 1) { - RCLCPP_WARN_STREAM( - node_->get_logger().get_child(name()), - "shift_sampling_num must be positive integer. Given parameter: " << temp_shift_sampling_num - << std::endl); - } else { - p->shift_sampling_num = temp_shift_sampling_num; - } - updateParam(parameters, ns + "maximum_lateral_jerk", p->maximum_lateral_jerk); updateParam(parameters, ns + "minimum_lateral_jerk", p->minimum_lateral_jerk); updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); From 7745c635ae16f17a7c87925acb4dfe1a1a485dee Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 18 Mar 2024 17:41:54 +0900 Subject: [PATCH 3/5] add comments Signed-off-by: Daniel Sanchez --- planning/behavior_path_goal_planner_module/src/manager.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 20f5938a2ee2e..046a4b8b4949b 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -423,6 +423,11 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) void GoalPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { + // TODO(someone): This function does not check OR update + // object_recognition_collision_check_soft_margins.empty(), + // object_recognition_collision_check_hard_margins, maximum_deceleration, shift_sampling_num or + // parking_policy, there seems to be a problem when we use a temp value to check these values. + using tier4_autoware_utils::updateParam; auto & p = parameters_; From 75d7eb7c79585e07301f1c4fb8e82487f15a8efc Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 26 Mar 2024 18:30:38 +0900 Subject: [PATCH 4/5] add missing params Signed-off-by: Daniel Sanchez --- .../src/manager.cpp | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 046a4b8b4949b..767978e17c632 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -424,7 +424,7 @@ void GoalPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { // TODO(someone): This function does not check OR update - // object_recognition_collision_check_soft_margins.empty(), + // object_recognition_collision_check_soft_margins, // object_recognition_collision_check_hard_margins, maximum_deceleration, shift_sampling_num or // parking_policy, there seems to be a problem when we use a temp value to check these values. @@ -438,6 +438,8 @@ void GoalPlannerModuleManager::updateModuleParams( { updateParam(parameters, base_ns + "th_stopped_velocity", p->th_stopped_velocity); updateParam(parameters, base_ns + "th_arrived_distance", p->th_arrived_distance); + updateParam(parameters, base_ns + "th_stopped_time", p->th_stopped_time); + updateParam( parameters, base_ns + "center_line_path_interval", p->center_line_path_interval); } @@ -445,6 +447,7 @@ void GoalPlannerModuleManager::updateModuleParams( { const std::string ns = base_ns + "goal_search."; + updateParam(parameters, ns + "goal_priority", p->goal_priority); updateParam( parameters, ns + "minimum_weighted_distance.lateral_weight", p->minimum_weighted_distance_lateral_weight); @@ -506,6 +509,9 @@ void GoalPlannerModuleManager::updateModuleParams( parameters, ns + "pull_over_minimum_velocity", p->pull_over_minimum_velocity); updateParam(parameters, ns + "decide_path_distance", p->decide_path_distance); updateParam(parameters, ns + "maximum_jerk", p->maximum_jerk); + updateParam(parameters, ns + "path_priority", p->path_priority); + updateParam>( + parameters, ns + "efficient_path_order", p->efficient_path_order); } // shift parking @@ -573,6 +579,8 @@ void GoalPlannerModuleManager::updateModuleParams( { const std::string ns = base_ns + "pull_over.freespace_parking."; updateParam(parameters, ns + "enable_freespace_parking", p->enable_freespace_parking); + updateParam( + parameters, ns + "freespace_parking_algorithm", p->freespace_parking_algorithm); updateParam(parameters, ns + "velocity", p->freespace_parking_velocity); updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); @@ -622,9 +630,19 @@ void GoalPlannerModuleManager::updateModuleParams( } // freespace parking astar - { const std::string ns = base_ns + "pull_over.freespace_parking.astar."; + updateParam( + parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); + updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); + updateParam( + parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); + } + + // freespace parking rrtstar + + { + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); updateParam( parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); From a340ac211e33be7413e36c38e33c59ba7b8e7e2c Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 26 Mar 2024 18:39:01 +0900 Subject: [PATCH 5/5] add missing param Signed-off-by: Daniel Sanchez --- planning/behavior_path_goal_planner_module/src/manager.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 767978e17c632..bca5bb1230f38 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -774,6 +774,7 @@ void GoalPlannerModuleManager::updateModuleParams( p->safety_check_params.enable_safety_check); updateParam( parameters, safety_check_ns + "keep_unsafe_time", p->safety_check_params.keep_unsafe_time); + updateParam(parameters, safety_check_ns + "method", p->safety_check_params.method); updateParam( parameters, safety_check_ns + "hysteresis_factor_expand_rate", p->safety_check_params.hysteresis_factor_expand_rate);