feat(obstacle_stop_planner): consider object velocity direction (auto…

…warefoundation#4648)

* feat(obstacle_stop_planner): consider object velocity direction

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

* update

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

---------

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

planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp

@@ -414,23 +414,27 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject(
 
   /* get object velocity, and current yaw */
   bool get_obj = false;
-  double obj_vel;
-  double obj_yaw;
+  double obj_vel_norm;
+  double obj_vel_yaw;
   const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros);
   for (const auto & obj : object_ptr->objects) {
     const Polygon obj_poly = getPolygon(
       obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0,
       param_.object_polygon_length_margin, param_.object_polygon_width_margin);
     if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) {
-      obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x;
-      obj_yaw = tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation);
+      obj_vel_norm = std::hypot(
+        obj.kinematics.initial_twist_with_covariance.twist.linear.x,
+        obj.kinematics.initial_twist_with_covariance.twist.linear.y);
+      obj_vel_yaw = std::atan2(
+        obj.kinematics.initial_twist_with_covariance.twist.linear.y,
+        obj.kinematics.initial_twist_with_covariance.twist.linear.x);
       get_obj = true;
       break;
     }
   }
 
   if (get_obj) {
-    *velocity = obj_vel * std::cos(obj_yaw - traj_yaw);
+    *velocity = obj_vel_norm * std::cos(obj_vel_yaw - traj_yaw);
     debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity;
     return true;
   } else {
@@ -442,10 +446,15 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject(
   const PredictedObject & object, const double traj_yaw, double * velocity)
 {
   /* get object velocity, and current yaw */
-  double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;
-  double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation);
-
-  *velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw));
+  double obj_vel_norm = std::hypot(
+    object.kinematics.initial_twist_with_covariance.twist.linear.x,
+    object.kinematics.initial_twist_with_covariance.twist.linear.y);
+  double obj_vel_yaw = std::atan2(
+    object.kinematics.initial_twist_with_covariance.twist.linear.y,
+    object.kinematics.initial_twist_with_covariance.twist.linear.x);
+
+  *velocity =
+    obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw));
   debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity;
 }