From d45e71d1f6df065e9f86840a0e104f08253bcf93 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Tue, 7 Jun 2022 19:28:46 +0900 Subject: [PATCH 1/2] fix(behavior_velocity): use pose for min velocity if the object is stopped Signed-off-by: Tomohito Ando --- .../src/scene_module/run_out/scene.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 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 71bb6ee12a29b..236988d21f710 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 @@ -505,10 +505,15 @@ std::vector RunOutModule::createBoundingBoxForRangedP tier4_autoware_utils::calcDistance2d(pose_with_range.pose_min, pose_with_range.pose_max); geometry_msgs::msg::Pose p_min_to_p_max; - const auto azimuth_angle = tier4_autoware_utils::calcAzimuthAngle( - pose_with_range.pose_min.position, pose_with_range.pose_max.position); - p_min_to_p_max.position = pose_with_range.pose_min.position; - p_min_to_p_max.orientation = tier4_autoware_utils::createQuaternionFromYaw(azimuth_angle); + if (dist_p1_p2 < std::numeric_limits::epsilon()) { + // can't calculate the angle if two points are the same + p_min_to_p_max = pose_with_range.pose_min; + } else { + const auto azimuth_angle = tier4_autoware_utils::calcAzimuthAngle( + pose_with_range.pose_min.position, pose_with_range.pose_max.position); + p_min_to_p_max.position = pose_with_range.pose_min.position; + p_min_to_p_max.orientation = tier4_autoware_utils::createQuaternionFromYaw(azimuth_angle); + } std::vector poly; poly.emplace_back( From 26a63fd988ee35bae88e63f18829265b909bbdb3 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 9 Jun 2022 14:35:15 +0900 Subject: [PATCH 2/2] fix typo Signed-off-by: Tomohito Ando --- .../src/scene_module/run_out/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 236988d21f710..1e11645d01bba 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 @@ -55,7 +55,7 @@ bool RunOutModule::modifyPathVelocity( const auto current_acc = planner_data_->current_accel.get(); const auto & current_pose = planner_data_->current_pose.pose; - // smooth velocity of the path to calcute time to collision accurately + // smooth velocity of the path to calculate time to collision accurately PathWithLaneId smoothed_path; if (!smoothPath(*path, smoothed_path, planner_data_)) { return true;