diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 9dc88115a85d8..8647945c4caa5 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -42,8 +42,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); wheelbase_ = vehicle_info.wheel_base_m; initCommonParam(); - over_stop_velocity_warn_thr_ = - declare_parameter("over_stop_velocity_warn_thr", tier4_autoware_utils::kmph2mps(5.0)); + over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); // create smoother switch (node_param_.algorithm_type) { @@ -101,7 +100,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions std::bind(&MotionVelocitySmootherNode::onParameter, this, _1)); // debug - publish_debug_trajs_ = declare_parameter("publish_debug_trajs", true); + publish_debug_trajs_ = declare_parameter("publish_debug_trajs"); debug_closest_velocity_ = create_publisher("~/closest_velocity", 1); debug_closest_acc_ = create_publisher("~/closest_acceleration", 1); debug_closest_jerk_ = create_publisher("~/closest_jerk", 1); @@ -243,34 +242,33 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter void MotionVelocitySmootherNode::initCommonParam() { auto & p = node_param_; - p.max_velocity = declare_parameter("max_velocity", 20.0); // 72.0 kmph + p.max_velocity = declare_parameter("max_velocity"); // 72.0 kmph p.margin_to_insert_external_velocity_limit = - declare_parameter("margin_to_insert_external_velocity_limit", 0.3); - p.replan_vel_deviation = declare_parameter("replan_vel_deviation", 3.0); - p.engage_velocity = declare_parameter("engage_velocity", 0.3); - p.engage_acceleration = declare_parameter("engage_acceleration", 0.1); - p.engage_exit_ratio = declare_parameter("engage_exit_ratio", 0.5); + declare_parameter("margin_to_insert_external_velocity_limit"); + p.replan_vel_deviation = declare_parameter("replan_vel_deviation"); + p.engage_velocity = declare_parameter("engage_velocity"); + p.engage_acceleration = declare_parameter("engage_acceleration"); + p.engage_exit_ratio = declare_parameter("engage_exit_ratio"); p.engage_exit_ratio = std::min(std::max(p.engage_exit_ratio, 0.0), 1.0); - p.stopping_velocity = - declare_parameter("stopping_velocity", tier4_autoware_utils::kmph2mps(10.0)); - p.stopping_distance = declare_parameter("stopping_distance", 0.0); - p.extract_ahead_dist = declare_parameter("extract_ahead_dist", 200.0); - p.extract_behind_dist = declare_parameter("extract_behind_dist", 3.0); - p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage", 1.5); + p.stopping_velocity = declare_parameter("stopping_velocity"); + p.stopping_distance = declare_parameter("stopping_distance"); + p.extract_ahead_dist = declare_parameter("extract_ahead_dist"); + p.extract_behind_dist = declare_parameter("extract_behind_dist"); + p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage"); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); p.post_resample_param.max_trajectory_length = - declare_parameter("post_max_trajectory_length", 300.0); + declare_parameter("post_max_trajectory_length"); p.post_resample_param.min_trajectory_length = - declare_parameter("post_min_trajectory_length", 30.0); - p.post_resample_param.resample_time = declare_parameter("post_resample_time", 10.0); - p.post_resample_param.dense_resample_dt = declare_parameter("post_dense_resample_dt", 0.1); + declare_parameter("post_min_trajectory_length"); + p.post_resample_param.resample_time = declare_parameter("post_resample_time"); + p.post_resample_param.dense_resample_dt = declare_parameter("post_dense_resample_dt"); p.post_resample_param.dense_min_interval_distance = - declare_parameter("post_dense_min_interval_distance", 0.1); - p.post_resample_param.sparse_resample_dt = declare_parameter("post_sparse_resample_dt", 0.1); + declare_parameter("post_dense_min_interval_distance"); + p.post_resample_param.sparse_resample_dt = declare_parameter("post_sparse_resample_dt"); p.post_resample_param.sparse_min_interval_distance = - declare_parameter("post_sparse_min_interval_distance", 1.0); - p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type", "JerkFiltered")); + declare_parameter("post_sparse_min_interval_distance"); + p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type")); } void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 3b013b77e4b01..a02e432e1e584 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -69,24 +69,24 @@ AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Nod : SmootherBase(node) { auto & p = smoother_param_; - p.resample.ds_resample = node.declare_parameter("resample.ds_resample", 0.1); - p.resample.num_resample = node.declare_parameter("resample.num_resample", 1); - p.resample.delta_yaw_threshold = node.declare_parameter("resample.delta_yaw_threshold", 0.785); + p.resample.ds_resample = node.declare_parameter("resample.ds_resample"); + p.resample.num_resample = node.declare_parameter("resample.num_resample"); + p.resample.delta_yaw_threshold = node.declare_parameter("resample.delta_yaw_threshold"); p.latacc.enable_constant_velocity_while_turning = - node.declare_parameter("latacc.enable_constant_velocity_while_turning", false); + node.declare_parameter("latacc.enable_constant_velocity_while_turning"); p.latacc.constant_velocity_dist_threshold = - node.declare_parameter("latacc.constant_velocity_dist_threshold", 2.0); - p.forward.max_acc = node.declare_parameter("forward.max_acc", 1.0); - p.forward.min_acc = node.declare_parameter("forward.min_acc", -1.0); - p.forward.max_jerk = node.declare_parameter("forward.max_jerk", 0.3); - p.forward.min_jerk = node.declare_parameter("forward.min_jerk", -0.3); - p.forward.kp = node.declare_parameter("forward.kp", 0.3); - p.backward.start_jerk = node.declare_parameter("backward.start_jerk", -0.1); - p.backward.min_jerk_mild_stop = node.declare_parameter("backward.min_jerk_mild_stop", -0.3); - p.backward.min_jerk = node.declare_parameter("backward.min_jerk", -1.5); - p.backward.min_acc_mild_stop = node.declare_parameter("backward.min_acc_mild_stop", -1.0); - p.backward.min_acc = node.declare_parameter("backward.min_acc", -2.5); - p.backward.span_jerk = node.declare_parameter("backward.span_jerk", -0.01); + node.declare_parameter("latacc.constant_velocity_dist_threshold"); + p.forward.max_acc = node.declare_parameter("forward.max_acc"); + p.forward.min_acc = node.declare_parameter("forward.min_acc"); + p.forward.max_jerk = node.declare_parameter("forward.max_jerk"); + p.forward.min_jerk = node.declare_parameter("forward.min_jerk"); + p.forward.kp = node.declare_parameter("forward.kp"); + p.backward.start_jerk = node.declare_parameter("backward.start_jerk"); + p.backward.min_jerk_mild_stop = node.declare_parameter("backward.min_jerk_mild_stop"); + p.backward.min_jerk = node.declare_parameter("backward.min_jerk"); + p.backward.min_acc_mild_stop = node.declare_parameter("backward.min_acc_mild_stop"); + p.backward.min_acc = node.declare_parameter("backward.min_acc"); + p.backward.span_jerk = node.declare_parameter("backward.span_jerk"); } void AnalyticalJerkConstrainedSmoother::setParam(const Param & smoother_param) diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 904f9b3b309ae..62d680af2d72b 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -31,11 +31,11 @@ namespace motion_velocity_smoother JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) { auto & p = smoother_param_; - p.jerk_weight = node.declare_parameter("jerk_weight", 10.0); - p.over_v_weight = node.declare_parameter("over_v_weight", 100000.0); - p.over_a_weight = node.declare_parameter("over_a_weight", 5000.0); - p.over_j_weight = node.declare_parameter("over_j_weight", 2000.0); - p.jerk_filter_ds = node.declare_parameter("jerk_filter_ds", 0.1); + p.jerk_weight = node.declare_parameter("jerk_weight"); + p.over_v_weight = node.declare_parameter("over_v_weight"); + p.over_a_weight = node.declare_parameter("over_a_weight"); + p.over_j_weight = node.declare_parameter("over_j_weight"); + p.jerk_filter_ds = node.declare_parameter("jerk_filter_ds"); qp_solver_.updateMaxIter(20000); qp_solver_.updateRhoInterval(0); // 0 means automatic diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index e958cc6567edb..fbce49c1c9af0 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -27,9 +27,9 @@ namespace motion_velocity_smoother L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { auto & p = smoother_param_; - p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight", 100.0); - p.over_v_weight = node.declare_parameter("over_v_weight", 100000.0); - p.over_a_weight = node.declare_parameter("over_a_weight", 1000.0); + p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); + p.over_v_weight = node.declare_parameter("over_v_weight"); + p.over_a_weight = node.declare_parameter("over_a_weight"); qp_solver_.updateMaxIter(4000); qp_solver_.updateRhoInterval(0); // 0 means automatic diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 94f951796be20..e76c1cb4ffe5c 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -27,9 +27,9 @@ namespace motion_velocity_smoother LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { auto & p = smoother_param_; - p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight", 200.0); - p.over_v_weight = node.declare_parameter("over_v_weight", 100000.0); - p.over_a_weight = node.declare_parameter("over_a_weight", 5000.0); + p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); + p.over_v_weight = node.declare_parameter("over_v_weight"); + p.over_a_weight = node.declare_parameter("over_a_weight"); qp_solver_.updateMaxIter(20000); qp_solver_.updateRhoInterval(5000); diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index b0688fa698f1f..01f6090e157aa 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -31,30 +31,31 @@ using vehicle_info_util::VehicleInfoUtil; SmootherBase::SmootherBase(rclcpp::Node & node) { auto & p = base_param_; - p.max_accel = node.declare_parameter("normal.max_acc", 2.0); - p.min_decel = node.declare_parameter("normal.min_acc", -3.0); - p.stop_decel = node.declare_parameter("stop_decel", 0.0); - p.max_jerk = node.declare_parameter("normal.max_jerk", 0.3); - p.min_jerk = node.declare_parameter("normal.min_jerk", -0.1); - p.max_lateral_accel = node.declare_parameter("max_lateral_accel", 0.2); + p.max_accel = node.declare_parameter("normal.max_acc"); + p.min_decel = node.declare_parameter("normal.min_acc"); + p.stop_decel = node.declare_parameter("stop_decel"); + p.max_jerk = node.declare_parameter("normal.max_jerk"); + p.min_jerk = node.declare_parameter("normal.min_jerk"); + p.max_lateral_accel = node.declare_parameter("max_lateral_accel"); p.min_decel_for_lateral_acc_lim_filter = - node.declare_parameter("min_decel_for_lateral_acc_lim_filter", -2.5); - p.sample_ds = node.declare_parameter("resample_ds", 0.5); - p.curvature_threshold = node.declare_parameter("curvature_threshold", 0.2); - p.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate", 5.0); - p.curvature_calculation_distance = node.declare_parameter("curvature_calculation_distance", 1.0); - p.decel_distance_before_curve = node.declare_parameter("decel_distance_before_curve", 3.5); - p.decel_distance_after_curve = node.declare_parameter("decel_distance_after_curve", 0.0); - p.min_curve_velocity = node.declare_parameter("min_curve_velocity", 1.38); - p.resample_param.max_trajectory_length = node.declare_parameter("max_trajectory_length", 200.0); - p.resample_param.min_trajectory_length = node.declare_parameter("min_trajectory_length", 30.0); - p.resample_param.resample_time = node.declare_parameter("resample_time", 10.0); - p.resample_param.dense_resample_dt = node.declare_parameter("dense_resample_dt", 0.1); + node.declare_parameter("min_decel_for_lateral_acc_lim_filter"); + p.sample_ds = node.declare_parameter("resample_ds"); + p.curvature_threshold = node.declare_parameter("curvature_threshold"); + p.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate"); + p.curvature_calculation_distance = + node.declare_parameter("curvature_calculation_distance"); + p.decel_distance_before_curve = node.declare_parameter("decel_distance_before_curve"); + p.decel_distance_after_curve = node.declare_parameter("decel_distance_after_curve"); + p.min_curve_velocity = node.declare_parameter("min_curve_velocity"); + p.resample_param.max_trajectory_length = node.declare_parameter("max_trajectory_length"); + p.resample_param.min_trajectory_length = node.declare_parameter("min_trajectory_length"); + p.resample_param.resample_time = node.declare_parameter("resample_time"); + p.resample_param.dense_resample_dt = node.declare_parameter("dense_resample_dt"); p.resample_param.dense_min_interval_distance = - node.declare_parameter("dense_min_interval_distance", 0.1); - p.resample_param.sparse_resample_dt = node.declare_parameter("sparse_resample_dt", 0.5); + node.declare_parameter("dense_min_interval_distance"); + p.resample_param.sparse_resample_dt = node.declare_parameter("sparse_resample_dt"); p.resample_param.sparse_min_interval_distance = - node.declare_parameter("sparse_min_interval_distance", 4.0); + node.declare_parameter("sparse_min_interval_distance"); } void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; }