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.

Sign up for GitHub

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 2 commits into from
Oct 6, 2022
Merged

feat(obstacle_cruise_planner): add goal safe distance #2031

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
037115c
feat(obstacle_cruise_planner): add goal safe distance
purewater0901 Oct 6, 2022
1f9dc40
update
purewater0901 Oct 6, 2022
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
1 change: 1 addition & 0 deletions ...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
View file
Open in desktop
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
1 change: 1 addition & 0 deletions planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
View file
Open in desktop
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
1 change: 1 addition & 0 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
View file
Open in desktop
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
View file
Open in desktop
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
27 changes: 20 additions & 7 deletions planning/obstacle_cruise_planner/src/planner_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,16 +140,29 @@ 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) {
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);
if (!closest_behavior_stop_idx) {
return longitudinal_info_.safe_distance_margin;
}

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);

if (*closest_behavior_stop_idx == planner_data.traj.points.size() - 1) {
// Closest behavior stop point is the end point
const double closest_obstacle_stop_dist_from_ego =
closest_obstacle_dist - dist_to_ego - longitudinal_info_.terminal_safe_distance_margin -
abs_ego_offset;
const double stop_dist_diff =
closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
if (stop_dist_diff < 0.5) {
// Use terminal margin (terminal_safe_distance_margin) for obstacle stop
return longitudinal_info_.terminal_safe_distance_margin;
}
} else {
const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego -
longitudinal_info_.safe_distance_margin -
abs_ego_offset;

const double stop_dist_diff =
closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) {
Expand Down