diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ab2092a3120ca..70c0c687b2514 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -100,12 +100,13 @@ std::pair 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 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 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);