Skip to content

Commit

Permalink
feat(crosswalk): suppress chattering of GO/STOP decision (#4471)
Browse files Browse the repository at this point in the history 
* feat(crosswalk): organize the code

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* suppress chattering

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 authored Aug 2, 2023
1 parent 07acbd7 commit 0becdc6
Show file tree
Hide file tree
Showing 4 changed files with 135 additions and 90 deletions.
2 changes: 2 additions & 0 deletions planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml
View file
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
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
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

0 comments on commit 0becdc6

Please sign in to comment.