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(crosswalk): suppress chattering of GO/STOP decision #4471

Merged
merged 4 commits into from
Aug 2, 2023
Merged

feat(crosswalk): suppress chattering of GO/STOP decision #4471

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
58354db
feat(crosswalk): organize the code
takayuki5168 Jul 31, 2023
60e6ba6
suppress chattering
takayuki5168 Jul 31, 2023
36cd8fe
fix
takayuki5168 Aug 1, 2023
4afa837
update config
takayuki5168 Aug 1, 2023
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
2 changes: 2 additions & 0 deletions planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,9 @@
# param for pass judge logic
pass_judge:
ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition)
ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering
ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition)
ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering
stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph)
min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph)
## param for yielding
Expand Down
4 changes: 4 additions & 0 deletions planning/behavior_velocity_crosswalk_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
// param for pass judge logic
cp.ego_pass_first_margin =
node.declare_parameter<double>(ns + ".pass_judge.ego_pass_first_margin");
cp.ego_pass_first_additional_margin =
node.declare_parameter<double>(ns + ".pass_judge.ego_pass_first_additional_margin");
cp.ego_pass_later_margin =
node.declare_parameter<double>(ns + ".pass_judge.ego_pass_later_margin");
cp.ego_pass_later_additional_margin =
node.declare_parameter<double>(ns + ".pass_judge.ego_pass_later_additional_margin");
cp.stop_object_velocity =
node.declare_parameter<double>(ns + ".pass_judge.stop_object_velocity_threshold");
cp.min_object_velocity = node.declare_parameter<double>(ns + ".pass_judge.min_object_velocity");
Expand Down
122 changes: 57 additions & 65 deletions planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -297,7 +297,6 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose)
{
const auto & ego_pos = planner_data_->current_odometry->pose.position;
const auto & objects_ptr = planner_data_->predicted_objects;
const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
const auto dist_ego_to_stop =
calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second;
Expand All @@ -309,45 +308,30 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range);

// Update object state
updateObjectState(dist_ego_to_stop);
updateObjectState(
dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area);

// Check pedestrian for stop
// NOTE: first stop point and its minimum distance from ego to stop
std::optional<std::pair<geometry_msgs::msg::Point, double>> nearest_stop_info;
std::vector<geometry_msgs::msg::Point> stop_factor_points;
const auto ignore_crosswalk = debug_data_.ignore_crosswalk = isRedSignalForPedestrians();
for (const auto & object : objects_ptr->objects) {
const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position;
const auto obj_uuid = toHexString(object.object_id);

if (!isCrosswalkUserType(object)) {
continue;
}

if (ignore_crosswalk) {
continue;
}

// NOTE: Collision points are calculated on each predicted path
const auto collision_point =
getCollisionPoints(sparse_resample_path, object, attention_area, crosswalk_attention_range);

for (const auto & cp : collision_point) {
const auto collision_state =
getCollisionState(obj_uuid, cp.time_to_collision, cp.time_to_vehicle);
debug_data_.collision_points.push_back(std::make_pair(cp, collision_state));

for (const auto & object : object_info_manager_.getObject()) {
const auto & collision_point = object.collision_point;
if (collision_point) {
const auto & collision_state = object.collision_state;
if (collision_state != CollisionState::YIELD) {
continue;
}

stop_factor_points.push_back(obj_pos);
stop_factor_points.push_back(object.position);

const auto dist_ego2cp =
calcSignedArcLength(sparse_resample_path.points, ego_pos, cp.collision_point) -
calcSignedArcLength(
sparse_resample_path.points, ego_pos, collision_point->collision_point) -
planner_param_.stop_distance_from_object;
if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) {
nearest_stop_info = std::make_pair(cp.collision_point, dist_ego2cp - base_link2front);
nearest_stop_info =
std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front);
}
}
}
Expand Down Expand Up @@ -560,9 +544,9 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
return std::make_pair(clamped_near_attention_range, clamped_far_attention_range);
}

std::vector<CollisionPoint> CrosswalkModule::getCollisionPoints(
const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon2d & attention_area,
const std::pair<double, double> & crosswalk_attention_range)
std::optional<CollisionPoint> CrosswalkModule::getCollisionPoint(
const PathWithLaneId & ego_path, const PredictedObject & object,
const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area)
{
stop_watch_.tic(__func__);

Expand All @@ -576,6 +560,8 @@ std::vector<CollisionPoint> CrosswalkModule::getCollisionPoints(
const auto obj_polygon =
createObjectPolygon(object.shape.dimensions.x, object.shape.dimensions.y);

double minimum_stop_dist = std::numeric_limits<double>::max();
std::optional<CollisionPoint> nearest_collision_point{std::nullopt};
for (const auto & obj_path : object.kinematics.predicted_paths) {
for (size_t i = 0; i < obj_path.path.size() - 1; ++i) {
const auto & p_obj_front = obj_path.path.at(i);
Expand All @@ -588,37 +574,40 @@ std::vector<CollisionPoint> CrosswalkModule::getCollisionPoints(
continue;
}

// Calculate nearest collision point
double minimum_stop_dist = std::numeric_limits<double>::max();
geometry_msgs::msg::Point nearest_collision_point{};
// Calculate nearest collision point among collisions with a predicted path
double local_minimum_stop_dist = std::numeric_limits<double>::max();
geometry_msgs::msg::Point local_nearest_collision_point{};
for (const auto & p : tmp_intersection) {
const auto cp = createPoint(p.x(), p.y(), ego_pos.z);
const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp);

if (dist_ego2cp < minimum_stop_dist) {
minimum_stop_dist = dist_ego2cp;
nearest_collision_point = cp;
if (dist_ego2cp < local_minimum_stop_dist) {
local_minimum_stop_dist = dist_ego2cp;
local_nearest_collision_point = cp;
}
}

const auto dist_ego2cp =
calcSignedArcLength(ego_path.points, ego_pos, nearest_collision_point);
calcSignedArcLength(ego_path.points, ego_pos, local_nearest_collision_point);
constexpr double eps = 1e-3;
const auto dist_obj2cp =
calcArcLength(obj_path.path) < eps
? 0.0
: calcSignedArcLength(obj_path.path, size_t(0), nearest_collision_point);
: calcSignedArcLength(obj_path.path, size_t(0), local_nearest_collision_point);

if (
dist_ego2cp < crosswalk_attention_range.first ||
crosswalk_attention_range.second < dist_ego2cp) {
continue;
}

ret.push_back(
createCollisionPoint(nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel));

debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z));
// Calculate nearest collision point among predicted paths
if (dist_ego2cp < minimum_stop_dist) {
minimum_stop_dist = dist_ego2cp;
nearest_collision_point = createCollisionPoint(
local_nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel);
debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z));
}

break;
}
Expand All @@ -628,7 +617,7 @@ std::vector<CollisionPoint> CrosswalkModule::getCollisionPoints(
logger_, planner_param_.show_processing_time, "%s : %f ms", __func__,
stop_watch_.toc(__func__, true));

return ret;
return nearest_collision_point;
}

CollisionPoint CrosswalkModule::createCollisionPoint(
Expand All @@ -653,25 +642,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint(
return collision_point;
}

CollisionState CrosswalkModule::getCollisionState(
const std::string & obj_uuid, const double ttc, const double ttv) const
{
// First, check if the object can be ignored
const auto obj_state = object_info_manager_.getState(obj_uuid);
if (obj_state == ObjectInfo::State::FULLY_STOPPED) {
return CollisionState::IGNORE;
}

// Compare time to collision and vehicle
if (ttc + planner_param_.ego_pass_first_margin < ttv) {
return CollisionState::EGO_PASS_FIRST;
}
if (ttv + planner_param_.ego_pass_later_margin < ttc) {
return CollisionState::EGO_PASS_LATER;
}
return CollisionState::YIELD;
}

void CrosswalkModule::applySafetySlowDownSpeed(
PathWithLaneId & output, const std::vector<geometry_msgs::msg::Point> & path_intersects)
{
Expand Down Expand Up @@ -854,7 +824,9 @@ std::optional<StopFactor> CrosswalkModule::getNearestStopFactor(
return {};
}

void CrosswalkModule::updateObjectState(const double dist_ego_to_stop)
void CrosswalkModule::updateObjectState(
const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,
const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area)
{
const auto & objects_ptr = planner_data_->predicted_objects;

Expand All @@ -870,14 +842,34 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop)
has_reached_stop_point;
}();

const auto ignore_crosswalk = isRedSignalForPedestrians();
debug_data_.ignore_crosswalk = ignore_crosswalk;

// Update object state
object_info_manager_.init();
for (const auto & object : objects_ptr->objects) {
const auto obj_uuid = toHexString(object.object_id);
const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position;
const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear;

// calculate collision point and state
if (!isCrosswalkUserType(object)) {
continue;
}
if (ignore_crosswalk) {
continue;
}

const auto collision_point =
getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area);
object_info_manager_.update(
obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, has_traffic_light,
planner_param_);
obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding,
has_traffic_light, collision_point, planner_param_);

if (collision_point) {
const auto collision_state = object_info_manager_.getCollisionState(obj_uuid);
debug_data_.collision_points.push_back(std::make_pair(*collision_point, collision_state));
}
}
object_info_manager_.finalize();
}
Expand Down
Loading