Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(obstacle_cruise_planner): add goal safe distance #2031

Merged
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
safe_distance_margin : 6.0 # This is also used as a stop margin [m]
terminal_safe_distance_margin : 3.0 # Stop margin at the goal [m]

nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
safe_distance_margin : 6.0 # This is also used as a stop margin [m]
terminal_safe_distance_margin : 3.0 # Stop margin at the goal [m]

nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,7 @@ struct LongitudinalInfo
double min_ego_accel_for_rss;
double min_object_accel_for_rss;
double safe_distance_margin;
double terminal_safe_distance_margin;
};

struct DebugData
Expand Down
5 changes: 4 additions & 1 deletion planning/obstacle_cruise_planner/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -245,6 +245,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
declare_parameter<double>("common.min_object_accel_for_rss");
const double idling_time = declare_parameter<double>("common.idling_time");
const double safe_distance_margin = declare_parameter<double>("common.safe_distance_margin");
const double terminal_safe_distance_margin =
declare_parameter<double>("common.terminal_safe_distance_margin");

return LongitudinalInfo{
max_accel,
Expand All @@ -258,7 +260,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
idling_time,
min_ego_accel_for_rss,
min_object_accel_for_rss,
safe_distance_margin};
safe_distance_margin,
terminal_safe_distance_margin};
}();

is_showing_debug_info_ = declare_parameter<bool>("common.is_showing_debug_info");
Expand Down
11 changes: 8 additions & 3 deletions planning/obstacle_cruise_planner/src/planner_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -140,9 +140,11 @@ Trajectory PlannerInterface::generateStopTrajectory(
const auto closest_behavior_stop_idx =
motion_utils::searchZeroVelocityIndex(planner_data.traj.points, nearest_segment_idx + 1);

if (
closest_behavior_stop_idx &&
closest_behavior_stop_idx != planner_data.traj.points.size() - 1) {
if (!closest_behavior_stop_idx) {
return longitudinal_info_.safe_distance_margin;
}

if (*closest_behavior_stop_idx != planner_data.traj.points.size() - 1) {
const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength(
planner_data.traj.points, planner_data.current_pose.position, nearest_segment_idx,
*closest_behavior_stop_idx);
Expand All @@ -156,6 +158,9 @@ Trajectory PlannerInterface::generateStopTrajectory(
// Use shorter margin (min_behavior_stop_margin) for obstacle stop
return min_behavior_stop_margin_;
}
} else {
// Closest behavior stop point is the end point
return longitudinal_info_.terminal_safe_distance_margin;
}
return longitudinal_info_.safe_distance_margin;
}();
Expand Down