From 9259fbea320e8a70a9a6bdde9c27620bb41f7013 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 5 Sep 2022 15:39:09 +0900 Subject: [PATCH 1/3] refactor: use util function to calculate stop point Signed-off-by: Tomohito Ando --- .../scene_module/run_out/path_utils.hpp | 40 ------------ .../include/scene_module/run_out/scene.hpp | 5 +- .../src/scene_module/run_out/scene.cpp | 62 ++++++++++--------- 3 files changed, 36 insertions(+), 71 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp index 0b8f2b4ef444f..50a0f8728f0b3 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp @@ -48,46 +48,6 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( return min_dist_point; } -template -size_t calcIndexByLength( - const T & points, const geometry_msgs::msg::Pose & current_pose, const double target_length) -{ - const size_t nearest_index = motion_utils::findNearestIndex(points, current_pose.position); - if (target_length < 0) { - return nearest_index; - } - - for (size_t i = nearest_index; i < points.size(); i++) { - double length_sum = motion_utils::calcSignedArcLength(points, current_pose.position, i); - if (length_sum > target_length) { - return i; - } - } - - // reach the end of the points, so return the last index - return points.size() - 1; -} - -template -size_t calcIndexByLengthReverse( - const T & points, const geometry_msgs::msg::Point & src_point, const float target_length) -{ - const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(points, src_point); - if (nearest_seg_idx == 0) { - return 0; - } - - for (size_t i = nearest_seg_idx; i > 0; i--) { - const auto length_sum = std::abs(motion_utils::calcSignedArcLength(points, src_point, i)); - if (length_sum > target_length) { - return i + 1; - } - } - - // reach the beginning of the path - return 0; -} - } // namespace run_out_utils } // namespace behavior_velocity_planner #endif // SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp index bb6fc7c237418..d9d6ae95c6e68 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp @@ -124,12 +124,11 @@ class RunOutModule : public SceneModuleInterface void insertStoppingVelocity( const boost::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, - const PathWithLaneId & smoothed_path, PathWithLaneId & output_path); + PathWithLaneId & output_path); void insertApproachingVelocity( const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, - const float approaching_vel, const float approach_margin, const PathWithLaneId & resampled_path, - PathWithLaneId & output_path); + const float approaching_vel, const float approach_margin, PathWithLaneId & output_path); void applyMaxJerkLimit( const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index 9f629927036f8..c22b3cd0b9fb5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -101,8 +101,7 @@ bool RunOutModule::modifyPathVelocity( dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path); } else { // just insert zero velocity for stopping - insertStoppingVelocity( - dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path); + insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); } // apply max jerk limit if the ego can't stop with specified max jerk and acc @@ -560,17 +559,20 @@ boost::optional RunOutModule::calcStopPoint( // insert the stop point without considering the distance from the obstacle // smoother will calculate appropriate jerk for deceleration if (!planner_param_.run_out.specify_decel_jerk) { - // calculate index of stop point + // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; - const size_t stop_index = run_out_utils::calcIndexByLengthReverse( - path.points, dynamic_obstacle->nearest_collision_point, base_to_collision_point); - const auto & stop_point = path.points.at(stop_index).point.pose; + const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + path.points, dynamic_obstacle->nearest_collision_point, -base_to_collision_point, false); + if (!stop_point) { + RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); + return {}; + } // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( - stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; } @@ -615,17 +617,20 @@ boost::optional RunOutModule::calcStopPoint( return {}; } - // calculate index of stop point + // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; - const size_t stop_index = run_out_utils::calcIndexByLengthReverse( - path.points, dynamic_obstacle->nearest_collision_point, base_to_collision_point); - const auto & stop_point = path.points.at(stop_index).point.pose; + const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + path.points, dynamic_obstacle->nearest_collision_point, -base_to_collision_point, false); + if (!stop_point) { + RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); + return {}; + } // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( - stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; } @@ -691,8 +696,7 @@ void RunOutModule::insertVelocity( state_ = State::STOP; } - insertStoppingVelocity( - dynamic_obstacle, current_pose, current_vel, current_acc, smoothed_path, output_path); + insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path); break; } @@ -703,8 +707,7 @@ void RunOutModule::insertVelocity( elapsed_time > planner_param_.approaching.stop_time_thresh ? State::APPROACH : State::STOP; RCLCPP_DEBUG_STREAM(logger_, "elapsed time: " << elapsed_time); - insertStoppingVelocity( - dynamic_obstacle, current_pose, current_vel, current_acc, smoothed_path, output_path); + insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path); break; } @@ -712,7 +715,7 @@ void RunOutModule::insertVelocity( RCLCPP_DEBUG_STREAM(logger_, "APPROACH state"); insertApproachingVelocity( *dynamic_obstacle, current_pose, planner_param_.approaching.limit_vel_kmph / 3.6, - planner_param_.approaching.margin, smoothed_path, output_path); + planner_param_.approaching.margin, output_path); debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); break; } @@ -727,17 +730,16 @@ void RunOutModule::insertVelocity( void RunOutModule::insertStoppingVelocity( const boost::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, - const PathWithLaneId & smoothed_path, PathWithLaneId & output_path) + PathWithLaneId & output_path) { const auto stop_point = - calcStopPoint(dynamic_obstacle, smoothed_path, current_pose, current_vel, current_acc); + calcStopPoint(dynamic_obstacle, output_path, current_pose, current_vel, current_acc); insertStopPoint(stop_point, output_path); } void RunOutModule::insertApproachingVelocity( const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, - const float approaching_vel, const float approach_margin, const PathWithLaneId & resampled_path, - PathWithLaneId & output_path) + const float approaching_vel, const float approach_margin, PathWithLaneId & output_path) { // insert slow down velocity from nearest segment point const auto nearest_seg_idx = @@ -752,25 +754,29 @@ void RunOutModule::insertApproachingVelocity( // calculate stop point to insert 0 velocity const float base_to_collision_point = approach_margin + planner_param_.vehicle_param.base_to_front; - const auto stop_idx = run_out_utils::calcIndexByLengthReverse( - resampled_path.points, dynamic_obstacle.nearest_collision_point, base_to_collision_point); - const auto & stop_point = resampled_path.points.at(stop_idx).point.pose; + const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + output_path.points, dynamic_obstacle.nearest_collision_point, -base_to_collision_point, false); + if (!stop_point) { + RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); + return; + } // debug debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( - stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); const auto nearest_seg_idx_stop = - motion_utils::findNearestSegmentIndex(output_path.points, stop_point.position); + motion_utils::findNearestSegmentIndex(output_path.points, stop_point->position); auto insert_idx_stop = nearest_seg_idx_stop + 1; // to PathPointWithLaneId // use lane id of point behind inserted point autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); - stop_point_with_lane_id.point.pose = stop_point; + stop_point_with_lane_id.point.pose = *stop_point; - planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx_stop); + constexpr double eps = 0.05; + planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx_stop, eps); } void RunOutModule::applyMaxJerkLimit( From 397f3679a356729fc3e96e57287bee0db9fb0659 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 5 Sep 2022 15:41:05 +0900 Subject: [PATCH 2/3] refactor: remove unnecessary debug marker Signed-off-by: Tomohito Ando --- .../src/scene_module/run_out/scene.cpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index c22b3cd0b9fb5..fdb29ca4161b4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -593,16 +593,7 @@ boost::optional RunOutModule::calcStopPoint( stop_dist = boost::make_optional(dist_to_collision); } - // debug - { - const float base_to_obstacle = - planner_param_.vehicle_param.base_to_front + planner_param_.run_out.stop_margin; - const auto vehicle_stop_idx = run_out_utils::calcIndexByLength( - path.points, current_pose, stop_dist.get() + base_to_obstacle); - const auto & p = path.points.at(vehicle_stop_idx).point.pose.position; - debug_ptr_->pushDebugPoints(p, PointType::Yellow); - debug_ptr_->setDebugValues(DebugValues::TYPE::STOP_DISTANCE, *stop_dist); - } + debug_ptr_->setDebugValues(DebugValues::TYPE::STOP_DISTANCE, *stop_dist); // vehicle have to decelerate if there is not enough distance with deceleration_jerk const bool deceleration_needed = From de4d662a2f620985dac777641146d4614088b314 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 5 Sep 2022 16:24:20 +0900 Subject: [PATCH 3/3] refactor: revert an unrelated change Signed-off-by: Tomohito Ando --- .../src/scene_module/run_out/scene.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index fdb29ca4161b4..7358c8b73924a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -766,8 +766,7 @@ void RunOutModule::insertApproachingVelocity( stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); stop_point_with_lane_id.point.pose = *stop_point; - constexpr double eps = 0.05; - planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx_stop, eps); + planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx_stop); } void RunOutModule::applyMaxJerkLimit(