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_stop_planner): prevent from obstacle hunting #1458

Merged
merged 3 commits into from
Aug 3, 2022
Merged

feat(obstacle_stop_planner): prevent from obstacle hunting #1458

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e294aa8
feat(obstacle_stop_planner): prevent from obstacle hunting
satoshi-ota Jul 28, 2022
35b0081
docs(obstacle_stop_planner): add common parameter description
satoshi-ota Jul 29, 2022
759aa41
fix(obstacle_stop_planner): fix bug
satoshi-ota Jul 29, 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
10 changes: 10 additions & 0 deletions planning/obstacle_stop_planner/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,16 @@ When the deceleration section is inserted, the start point of the section is ins

## Modules

### Common Parameter

| Parameter | Type | Description |
| ----------------------- | ------ | ---------------------------------------------------------------------------------------- |
| `enable_slow_down` | bool | enable slow down planner [-] |
| `max_velocity` | double | max velocity [m/s] |
| `hunting_threshold` | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
| `lowpass_gain` | double | low pass gain for calculating acceleration [-] |
| `max_yaw_deviation_deg` | double | # maximum ego yaw deviation from trajectory [deg] (measures against overlapping lanes) |

### Obstacle Stop Planner

#### Role
Expand Down
2 changes: 2 additions & 0 deletions planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
/**:
ros__parameters:
hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]

stop_planner:
stop_margin: 5.0 # stop margin distance from obstacle on the path [m]
min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m]
Expand Down
6 changes: 4 additions & 2 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -185,12 +185,14 @@ class ObstacleStopPlannerNode : public rclcpp::Node
std::unique_ptr<motion_planning::AdaptiveCruiseController> acc_controller_;
std::shared_ptr<ObstacleStopPlannerDebugNode> debug_ptr_;
std::shared_ptr<LowpassFilter1d> lpf_acc_{nullptr};
boost::optional<SlowDownSection> latest_slow_down_section_{};
boost::optional<StopPoint> latest_stop_point_{boost::none};
boost::optional<SlowDownSection> latest_slow_down_section_{boost::none};
tf2_ros::Buffer tf_buffer_{get_clock()};
tf2_ros::TransformListener tf_listener_{tf_buffer_};
sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
PredictedObjects::ConstSharedPtr object_ptr_{nullptr};
rclcpp::Time last_detection_time_;
rclcpp::Time last_detect_time_collision_point_;
rclcpp::Time last_detect_time_slowdown_point_;

nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr};
nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr};
Expand Down
43 changes: 34 additions & 9 deletions planning/obstacle_stop_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -515,7 +515,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
acc_controller_ = std::make_unique<motion_planning::AdaptiveCruiseController>(
this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m);
debug_ptr_ = std::make_shared<ObstacleStopPlannerDebugNode>(this, i.max_longitudinal_offset_m);
last_detection_time_ = this->now();
last_detect_time_slowdown_point_ = this->now();
last_detect_time_collision_point_ = this->now();

// Publishers
path_pub_ = this->create_publisher<Trajectory>("~/output/trajectory", 1);
Expand Down Expand Up @@ -654,8 +655,9 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu
current_vel, stop_param);

const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_;
const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() >
node_param_.hunting_threshold;
const auto no_hunting =
(rclcpp::Time(input_msg->header.stamp) - last_detect_time_slowdown_point_).seconds() >
node_param_.hunting_threshold;
if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) {
resetExternalVelocityLimit(current_acc, current_vel);
}
Expand Down Expand Up @@ -723,6 +725,8 @@ void ObstacleStopPlannerNode::searchObstacle(
debug_ptr_->pushObstaclePoint(planner_data.nearest_slow_down_point, PointType::SlowDown);
debug_ptr_->pushPolygon(
one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown);

last_detect_time_slowdown_point_ = trajectory_header.stamp;
}

} else {
Expand Down Expand Up @@ -769,6 +773,10 @@ void ObstacleStopPlannerNode::searchObstacle(
planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr,
&planner_data.stop_require, &output, trajectory_header);

if (planner_data.stop_require) {
last_detect_time_collision_point_ = trajectory_header.stamp;
}

break;
}
}
Expand All @@ -781,6 +789,9 @@ void ObstacleStopPlannerNode::insertVelocity(
const double current_acc, const double current_vel, const StopParam & stop_param)
{
const auto & base_link2front = vehicle_info.max_longitudinal_offset_m;
const auto no_hunting_collision_point =
(rclcpp::Time(trajectory_header.stamp) - last_detect_time_collision_point_).seconds() >
node_param_.hunting_threshold;

if (planner_data.stop_require) {
// insert stop point
Expand Down Expand Up @@ -820,18 +831,29 @@ void ObstacleStopPlannerNode::insertVelocity(
current_stop_pos.point.pose = ego_pos_on_path.get();

insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag);
latest_stop_point_ = current_stop_pos;

debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop);
}

} else {
insertStopPoint(stop_point, output, planner_data.stop_reason_diag);
latest_stop_point_ = stop_point;

debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop);
}
}
} else if (!no_hunting_collision_point) {
if (latest_stop_point_) {
insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag);
debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::Stop);
}
}

const auto no_hunting_slowdown_point =
(rclcpp::Time(trajectory_header.stamp) - last_detect_time_slowdown_point_).seconds() >
node_param_.hunting_threshold;

if (planner_data.slow_down_require) {
// insert slow down point
const auto traj_end_idx = output.size() - 1;
Expand All @@ -856,16 +878,19 @@ void ObstacleStopPlannerNode::insertVelocity(
current_vel);

if (
!latest_slow_down_section_ &&
dist_baselink_to_obstacle + index_with_dist_remain.get().second <
vehicle_info.max_longitudinal_offset_m) {
(!latest_slow_down_section_ &&
dist_baselink_to_obstacle + index_with_dist_remain.get().second <
vehicle_info.max_longitudinal_offset_m) ||
!no_hunting_slowdown_point) {
latest_slow_down_section_ = slow_down_section;
}

insertSlowDownSection(slow_down_section, output);
}

last_detection_time_ = trajectory_header.stamp;
} else if (!no_hunting_slowdown_point) {
if (latest_slow_down_section_) {
insertSlowDownSection(latest_slow_down_section_.get(), output);
}
}

if (node_param_.enable_slow_down && latest_slow_down_section_) {
Expand All @@ -891,7 +916,7 @@ void ObstacleStopPlannerNode::insertVelocity(
set_velocity_limit_ ? std::numeric_limits<double>::max() : slow_down_param_.slow_down_vel;

insertSlowDownSection(slow_down_section, output);
} else {
} else if (no_hunting_slowdown_point) {
latest_slow_down_section_ = {};
}
}
Expand Down