update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.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_vel = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y);
-  const double object_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x);
+  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(
-    object_vel * std::cos(object_yaw - traj_yaw), object_vel * 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)
@@ -819,7 +820,7 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle(
   }
 
   if (
-    std::abs(std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y)) <
+    std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) <
     p.outside_obstacle_min_velocity_threshold) {
     RCLCPP_INFO_EXPRESSION(
       get_logger(), enable_debug_info_,
@@ -921,9 +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(std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y));
+  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(
@@ -1144,10 +1144,8 @@ void ObstacleCruisePlannerNode::checkConsistency(
   } else {
     // prev obstacle is not in the target obstacles, but in the perception list
     const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds();
-    const auto & object_twist =
-      predicted_object_itr->kinematics.initial_twist_with_covariance.twist;
     if (
-      std::hypot(object_twist.linear.x, object_twist.linear.y) <
+      predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
         behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
       elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) {
       stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_);