feat(obstacle_cruise_planner): consider object velocity direction (au…

…towarefoundation#4647)

* feat(obstacle_cruise_planner): consider object velocity direction

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

* update

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

* chore: spell fix

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

planning/obstacle_cruise_planner/src/node.cpp

@@ -100,12 +100,13 @@ std::pair<double, double> projectObstacleVelocityToTrajectory(
 {
   const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position);
 
-  const double object_yaw = tf2::getYaw(obstacle.pose.orientation);
+  const double object_vel_norm = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y);
+  const double object_vel_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x);
   const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation);
 
   return std::make_pair(
-    obstacle.twist.linear.x * std::cos(object_yaw - traj_yaw),
-    obstacle.twist.linear.x * std::sin(object_yaw - traj_yaw));
+    object_vel_norm * std::cos(object_vel_yaw - traj_yaw),
+    object_vel_norm * std::sin(object_vel_yaw - traj_yaw));
 }
 
 double calcObstacleMaxLength(const Shape & shape)
@@ -818,7 +819,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle(
     return std::nullopt;
   }
 
-  if (std::abs(obstacle.twist.linear.x) < p.outside_obstacle_min_velocity_threshold) {
+  if (
+    std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) <
+    p.outside_obstacle_min_velocity_threshold) {
     RCLCPP_INFO_EXPRESSION(
       get_logger(), enable_debug_info_,
       "[Cruise] Ignore outside obstacle (%s) since the obstacle velocity is low.",
@@ -919,8 +922,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle(
   // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed,
   //       and the collision between ego and obstacles are within the margin threshold.
   const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle);
-  const double has_high_speed =
-    p.crossing_obstacle_velocity_threshold < std::abs(obstacle.twist.linear.x);
+  const double has_high_speed = p.crossing_obstacle_velocity_threshold <
+                                std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y);
   if (is_obstacle_crossing && has_high_speed) {
     // Get highest confidence predicted path
     const auto resampled_predicted_path = resampleHighestConfidencePredictedPath(
@@ -995,7 +998,7 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
       }
       return true;
     }
-    // check if entrying slow down
+    // check if entering slow down
     if (is_lat_dist_low) {
       const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid);
       if (p.successive_num_to_entry_slow_down_condition <= count) {
@@ -1016,7 +1019,7 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
   const auto obstacle_poly = obstacle.toPolygon();
 
   // calculate collision points with trajectory with lateral stop margin
-  // NOTE: For additional margin, hysteresis is not devided by two.
+  // NOTE: For additional margin, hysteresis is not divided by two.
   const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons(
     traj_points, vehicle_info_,
     p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);