Skip to content

Commit

Permalink
feat(crosswalk): organize the code
Browse files Browse the repository at this point in the history
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
takayuki5168 committed Aug 1, 2023
1 parent cd9f5ce commit 58354db
Show file tree
Hide file tree
Showing 3 changed files with 118 additions and 90 deletions.
4 changes: 4 additions & 0 deletions planning/behavior_velocity_crosswalk_module/src/manager.cpp
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
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
82 changes: 57 additions & 25 deletions planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,9 @@ class CrosswalkModule : public SceneModuleInterface
double min_jerk_for_stuck_vehicle;
// param for pass judge logic
double ego_pass_first_margin;
double ego_pass_first_additional_margin;
double ego_pass_later_margin;
double ego_pass_later_additional_margin;
double stop_object_velocity;
double min_object_velocity;
bool disable_stop_for_yield_cancel;
Expand All @@ -100,19 +102,22 @@ class CrosswalkModule : public SceneModuleInterface

struct ObjectInfo
{
// NOTE: FULLY_STOPPED means stopped object which can be ignored.
enum class State { STOPPED = 0, FULLY_STOPPED, OTHER };
State state;
CollisionState collision_state{};
std::optional<rclcpp::Time> time_to_start_stopped{std::nullopt};

void updateState(
const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding,
const bool has_traffic_light, const PlannerParam & planner_param)
geometry_msgs::msg::Point position{};
std::optional<CollisionPoint> collision_point{};

void transitState(
const rclcpp::Time & now, const double vel, const bool is_ego_yielding,
const bool has_traffic_light, const std::optional<CollisionPoint> & collision_point,
const PlannerParam & planner_param)
{
const bool is_stopped = obj_vel < planner_param.stop_object_velocity;
const bool is_stopped = vel < planner_param.stop_object_velocity;

// Check if the object can be ignored
if (is_stopped) {
if (state == State::FULLY_STOPPED) {
if (collision_state == CollisionState::IGNORE) {
return;
}

Expand All @@ -124,23 +129,36 @@ class CrosswalkModule : public SceneModuleInterface
if (
(is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) &&
!intent_to_cross) {
state = State::FULLY_STOPPED;
} else {
// NOTE: Object may start moving
state = State::STOPPED;
collision_state = CollisionState::IGNORE;
return;
}
} else {
time_to_start_stopped = std::nullopt;
state = State::OTHER;
}

// Compare time to collision and vehicle
if (collision_point) {
if (
collision_point->time_to_collision + planner_param.ego_pass_first_margin <
collision_point->time_to_vehicle) {
collision_state = CollisionState::EGO_PASS_FIRST;
}
if (
collision_point->time_to_vehicle + planner_param.ego_pass_later_margin <
collision_point->time_to_collision) {
collision_state = CollisionState::EGO_PASS_LATER;
}
collision_state = CollisionState::YIELD;
}
}
};
struct ObjectInfoManager
{
void init() { current_uuids_.clear(); }
void update(
const std::string & uuid, const double obj_vel, const rclcpp::Time & now,
const bool is_ego_yielding, const bool has_traffic_light, const PlannerParam & planner_param)
const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel,
const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light,
const std::optional<CollisionPoint> & collision_point, const PlannerParam & planner_param)
{
// update current uuids
current_uuids_.push_back(uuid);
Expand All @@ -150,14 +168,17 @@ class CrosswalkModule : public SceneModuleInterface
if (
has_traffic_light && planner_param.disable_stop_for_yield_cancel &&
planner_param.disable_yield_for_new_stopped_object) {
objects.emplace(uuid, ObjectInfo{ObjectInfo::State::FULLY_STOPPED});
objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE});
} else {
objects.emplace(uuid, ObjectInfo{ObjectInfo::State::OTHER});
objects.emplace(uuid, ObjectInfo{CollisionState::YIELD});
}
}

// update object state
objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, has_traffic_light, planner_param);
objects.at(uuid).transitState(
now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param);
objects.at(uuid).collision_point = collision_point;
objects.at(uuid).position = position;
}
void finalize()
{
Expand All @@ -174,7 +195,19 @@ class CrosswalkModule : public SceneModuleInterface
objects.erase(obsolete_uuid);
}
}
ObjectInfo::State getState(const std::string & uuid) const { return objects.at(uuid).state; }

std::vector<ObjectInfo> getObject() const
{
std::vector<ObjectInfo> object_info_vec;
for (auto object : objects) {
object_info_vec.push_back(object.second);
}
return object_info_vec;
}
CollisionState getCollisionState(const std::string & uuid) const
{
return objects.at(uuid).collision_state;
}

std::unordered_map<std::string, ObjectInfo> objects;
std::vector<std::string> current_uuids_;
Expand Down Expand Up @@ -210,9 +243,9 @@ class CrosswalkModule : public SceneModuleInterface
const std::vector<geometry_msgs::msg::Point> & path_intersects,
const std::optional<geometry_msgs::msg::Pose> & stop_pose) const;

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

std::optional<StopFactor> getNearestStopFactor(
const PathWithLaneId & ego_path,
Expand Down Expand Up @@ -243,9 +276,6 @@ class CrosswalkModule : public SceneModuleInterface
const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel,
const geometry_msgs::msg::Vector3 & obj_vel) const;

CollisionState getCollisionState(
const std::string & obj_uuid, const double ttc, const double ttv) const;

float calcTargetVelocity(
const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const;

Expand All @@ -257,7 +287,9 @@ class CrosswalkModule : public SceneModuleInterface
const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
const std::vector<geometry_msgs::msg::Point> & path_intersects) const;

void updateObjectState(const double dist_ego_to_stop);
void updateObjectState(
const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,
const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area);

bool isRedSignalForPedestrians() const;

Expand Down

0 comments on commit 58354db

Please sign in to comment.