From 58354db3c802be532fb2b5c7d6497f4ab4c8d78c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 31 Jul 2023 20:20:52 +0900 Subject: [PATCH 1/4] feat(crosswalk): organize the code Signed-off-by: Takayuki Murooka --- .../src/manager.cpp | 4 + .../src/scene_crosswalk.cpp | 122 ++++++++---------- .../src/scene_crosswalk.hpp | 82 ++++++++---- 3 files changed, 118 insertions(+), 90 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index d287787006aff..400c3333412f5 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -73,8 +73,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for pass judge logic cp.ego_pass_first_margin = node.declare_parameter(ns + ".pass_judge.ego_pass_first_margin"); + cp.ego_pass_first_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); cp.ego_pass_later_margin = node.declare_parameter(ns + ".pass_judge.ego_pass_later_margin"); + cp.ego_pass_later_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); cp.stop_object_velocity = node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index d45004324e1b2..1265c13ab1871 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -297,7 +297,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional & 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; @@ -309,45 +308,30 @@ std::optional 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> nearest_stop_info; std::vector 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); } } } @@ -560,9 +544,9 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } -std::vector CrosswalkModule::getCollisionPoints( - const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon2d & attention_area, - const std::pair & crosswalk_attention_range) +std::optional CrosswalkModule::getCollisionPoint( + const PathWithLaneId & ego_path, const PredictedObject & object, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { stop_watch_.tic(__func__); @@ -576,6 +560,8 @@ std::vector CrosswalkModule::getCollisionPoints( const auto obj_polygon = createObjectPolygon(object.shape.dimensions.x, object.shape.dimensions.y); + double minimum_stop_dist = std::numeric_limits::max(); + std::optional 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); @@ -588,26 +574,26 @@ std::vector CrosswalkModule::getCollisionPoints( continue; } - // Calculate nearest collision point - double minimum_stop_dist = std::numeric_limits::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::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 || @@ -615,10 +601,13 @@ std::vector CrosswalkModule::getCollisionPoints( 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; } @@ -628,7 +617,7 @@ std::vector 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( @@ -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 & path_intersects) { @@ -854,7 +824,9 @@ std::optional 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 & crosswalk_attention_range, const Polygon2d & attention_area) { const auto & objects_ptr = planner_data_->predicted_objects; @@ -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(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 957fa9540f2f5..880f505e032d7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -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; @@ -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 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 collision_point{}; + + void transitState( + const rclcpp::Time & now, const double vel, const bool is_ego_yielding, + const bool has_traffic_light, const std::optional & 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; } @@ -124,14 +129,26 @@ 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; } } }; @@ -139,8 +156,9 @@ class CrosswalkModule : public SceneModuleInterface { 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 & collision_point, const PlannerParam & planner_param) { // update current uuids current_uuids_.push_back(uuid); @@ -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() { @@ -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 getObject() const + { + std::vector 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 objects; std::vector current_uuids_; @@ -210,9 +243,9 @@ class CrosswalkModule : public SceneModuleInterface const std::vector & path_intersects, const std::optional & stop_pose) const; - std::vector getCollisionPoints( + std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, - const Polygon2d & attention_area, const std::pair & crosswalk_attention_range); + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); std::optional getNearestStopFactor( const PathWithLaneId & ego_path, @@ -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; @@ -257,7 +287,9 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & 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 & crosswalk_attention_range, const Polygon2d & attention_area); bool isRedSignalForPedestrians() const; From 60e6ba6c00d159bc49df4957ec09688a05319ea0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 31 Jul 2023 20:35:24 +0900 Subject: [PATCH 2/4] suppress chattering Signed-off-by: Takayuki Murooka --- .../src/scene_crosswalk.hpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 880f505e032d7..cd0f3b8c7f074 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -138,13 +138,25 @@ class CrosswalkModule : public SceneModuleInterface // Compare time to collision and vehicle if (collision_point) { + // Check if ego will pass first + const double ego_pass_first_additional_margin = + collision_state == CollisionState::EGO_PASS_FIRST + ? 0.0 + : planner_param.ego_pass_first_additional_margin; if ( - collision_point->time_to_collision + planner_param.ego_pass_first_margin < + collision_point->time_to_collision + planner_param.ego_pass_first_margin + + ego_pass_first_additional_margin < collision_point->time_to_vehicle) { collision_state = CollisionState::EGO_PASS_FIRST; } + // Check if ego will pass later + const double ego_pass_later_additional_margin = + collision_state == CollisionState::EGO_PASS_LATER + ? 0.0 + : planner_param.ego_pass_later_additional_margin; if ( - collision_point->time_to_vehicle + planner_param.ego_pass_later_margin < + collision_point->time_to_vehicle + planner_param.ego_pass_later_margin + + ego_pass_later_additional_margin < collision_point->time_to_collision) { collision_state = CollisionState::EGO_PASS_LATER; } From 36cd8fe052b9d15b791a02681aa4fb96e731ceaa Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 1 Aug 2023 11:18:18 +0900 Subject: [PATCH 3/4] fix Signed-off-by: Takayuki Murooka --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index cd0f3b8c7f074..df396b987da58 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -148,6 +148,7 @@ class CrosswalkModule : public SceneModuleInterface ego_pass_first_additional_margin < collision_point->time_to_vehicle) { collision_state = CollisionState::EGO_PASS_FIRST; + return; } // Check if ego will pass later const double ego_pass_later_additional_margin = @@ -159,8 +160,10 @@ class CrosswalkModule : public SceneModuleInterface ego_pass_later_additional_margin < collision_point->time_to_collision) { collision_state = CollisionState::EGO_PASS_LATER; + return; } collision_state = CollisionState::YIELD; + return; } } }; From 4afa8378a01b58d9ad2d7fdfac386cb9f6036a35 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 1 Aug 2023 19:36:55 +0900 Subject: [PATCH 4/4] update config Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 097b2c6c94aef..11184eedf4aaf 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -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