refactor(avoidance): organize alias (#6947)

Signed-off-by: satoshi-ota <[email protected]>

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -15,16 +15,12 @@
 #ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_
 #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_
 
+#include "behavior_path_avoidance_module/type_alias.hpp"
 #include "behavior_path_planner_common/data_manager.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
 #include <rclcpp/time.hpp>
-#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
-
-#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_core/primitives/LineString.h>
@@ -38,19 +34,8 @@
 
 namespace behavior_path_planner
 {
-using autoware_auto_perception_msgs::msg::PredictedObject;
-using autoware_auto_perception_msgs::msg::PredictedPath;
-using autoware_auto_planning_msgs::msg::PathWithLaneId;
-
-using tier4_autoware_utils::Point2d;
-using tier4_autoware_utils::Polygon2d;
-using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
-
-using geometry_msgs::msg::Point;
-using geometry_msgs::msg::Pose;
 
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
-
 using route_handler::Direction;
 
 enum class ObjectInfo {

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp

@@ -16,26 +16,20 @@
 #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
-
-#include <tier4_autoware_utils/ros/marker_helper.hpp>
-
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
+#include "behavior_path_avoidance_module/type_alias.hpp"
 
 #include <string>
 
-namespace marker_utils::avoidance_marker
+namespace behavior_path_planner::utils::avoidance
 {
-using autoware_auto_planning_msgs::msg::PathWithLaneId;
+
 using behavior_path_planner::AvoidancePlanningData;
 using behavior_path_planner::AvoidLineArray;
 using behavior_path_planner::DebugData;
 using behavior_path_planner::ObjectDataArray;
 using behavior_path_planner::ObjectInfo;
 using behavior_path_planner::PathShifter;
 using behavior_path_planner::ShiftLineArray;
-using geometry_msgs::msg::Pose;
-using visualization_msgs::msg::MarkerArray;
 
 MarkerArray createEgoStatusMarkerArray(
   const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns);
@@ -52,7 +46,7 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
 
 MarkerArray createDebugMarkerArray(
   const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug);
-}  // namespace marker_utils::avoidance_marker
+}  // namespace behavior_path_planner::utils::avoidance
 
 std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr);
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -16,11 +16,10 @@
 #define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
+#include "behavior_path_avoidance_module/type_alias.hpp"
 #include "behavior_path_avoidance_module/utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
-#include <motion_utils/distance/distance.hpp>
-
 #include <algorithm>
 #include <limits>
 #include <memory>
@@ -32,11 +31,6 @@ namespace behavior_path_planner::helper::avoidance
 
 using behavior_path_planner::PathShifter;
 using behavior_path_planner::PlannerData;
-using motion_utils::calcDecelDistWithJerkAndAccConstraints;
-using motion_utils::calcLongitudinalOffsetPose;
-using motion_utils::calcSignedArcLength;
-using motion_utils::findNearestIndex;
-using tier4_autoware_utils::getPose;
 
 class AvoidanceHelper
 {
@@ -181,14 +175,14 @@ class AvoidanceHelper
   double getShift(const Point & p) const
   {
     validate();
-    const auto idx = findNearestIndex(prev_reference_path_.points, p);
+    const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p);
     return prev_spline_shift_path_.shift_length.at(idx);
   }
 
   double getLinearShift(const Point & p) const
   {
     validate();
-    const auto idx = findNearestIndex(prev_reference_path_.points, p);
+    const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p);
     return prev_linear_shift_path_.shift_length.at(idx);
   }
 
@@ -282,8 +276,8 @@ class AvoidanceHelper
     }
 
     const auto start_idx = data_->findEgoIndex(path.points);
-    const auto distance =
-      calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position);
+    const auto distance = motion_utils::calcSignedArcLength(
+      path.points, start_idx, max_v_point_.value().first.position);
     return std::make_pair(distance, max_v_point_.value().second);
   }
 
@@ -294,7 +288,7 @@ class AvoidanceHelper
     const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
     const auto & a_lim = use_hard_constraints ? p->max_deceleration : p->nominal_deceleration;
     const auto & j_lim = use_hard_constraints ? p->max_jerk : p->nominal_jerk;
-    const auto ret = calcDecelDistWithJerkAndAccConstraints(
+    const auto ret = motion_utils::calcDecelDistWithJerkAndAccConstraints(
       getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim);
 
     if (!!ret) {
@@ -441,14 +435,15 @@ class AvoidanceHelper
     const auto x_max_accel =
       v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0;
 
-    const auto point =
-      calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
+    const auto point = motion_utils::calcLongitudinalOffsetPose(
+      path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
     if (point.has_value()) {
       max_v_point_ = std::make_pair(point.value(), v_max);
       return;
     }
 
-    const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
+    const auto x_end =
+      motion_utils::calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
     const auto t_end =
       (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration;
     const auto v_end = v0 + p->max_acceleration * t_end;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -18,19 +18,14 @@
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/helper.hpp"
 #include "behavior_path_avoidance_module/shift_line_generator.hpp"
+#include "behavior_path_avoidance_module/type_alias.hpp"
 #include "behavior_path_planner_common/interface/scene_module_interface.hpp"
 #include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
 
 #include <rclcpp/logging.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/time.hpp>
 
-#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
-#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
-#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
-
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -39,12 +34,8 @@
 namespace behavior_path_planner
 {
 
-using motion_utils::calcSignedArcLength;
-using motion_utils::findNearestIndex;
-
-using tier4_planning_msgs::msg::AvoidanceDebugMsg;
-
 using helper::avoidance::AvoidanceHelper;
+using tier4_planning_msgs::msg::AvoidanceDebugMsg;
 
 class AvoidanceModule : public SceneModuleInterface
 {
@@ -114,7 +105,7 @@ class AvoidanceModule : public SceneModuleInterface
 
     for (const auto & left_shift : left_shift_array_) {
       const double start_distance =
-        calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
+        motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
       const double finish_distance = start_distance + left_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
         left_shift.uuid, true, start_distance, finish_distance, clock_->now());
@@ -127,7 +118,7 @@ class AvoidanceModule : public SceneModuleInterface
 
     for (const auto & right_shift : right_shift_array_) {
       const double start_distance =
-        calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
+        motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
       const double finish_distance = start_distance + right_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
         right_shift.uuid, true, start_distance, finish_distance, clock_->now());

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp

@@ -17,6 +17,7 @@
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/helper.hpp"
+#include "behavior_path_avoidance_module/type_alias.hpp"
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
 #include <memory>

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp

@@ -0,0 +1,72 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_
+#define BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_
+
+#include <motion_utils/distance/distance.hpp>
+#include <motion_utils/trajectory/path_with_lane_id.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace behavior_path_planner
+{
+// auto msgs
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedPath;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+
+// ROS 2 general msgs
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
+using geometry_msgs::msg::Vector3;
+using std_msgs::msg::ColorRGBA;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+
+// tier4 msgs
+using tier4_planning_msgs::msg::AvoidanceDebugMsg;
+using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
+
+// tier4 utils functions
+using tier4_autoware_utils::appendMarkerArray;
+using tier4_autoware_utils::calcDistance2d;
+using tier4_autoware_utils::calcLateralDeviation;
+using tier4_autoware_utils::calcOffsetPose;
+using tier4_autoware_utils::calcYawDeviation;
+using tier4_autoware_utils::createDefaultMarker;
+using tier4_autoware_utils::createMarkerColor;
+using tier4_autoware_utils::createMarkerScale;
+using tier4_autoware_utils::createPoint;
+using tier4_autoware_utils::createQuaternionFromRPY;
+using tier4_autoware_utils::getPoint;
+using tier4_autoware_utils::getPose;
+using tier4_autoware_utils::Point2d;
+using tier4_autoware_utils::Polygon2d;
+using tier4_autoware_utils::pose2transform;
+using tier4_autoware_utils::toHexString;
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -25,6 +25,7 @@
 
 namespace behavior_path_planner::utils::avoidance
 {
+
 using behavior_path_planner::PlannerData;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -19,24 +19,12 @@
 
 #include <lanelet2_extension/visualization/visualization.hpp>
 #include <magic_enum.hpp>
-#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <string>
 #include <vector>
 
-namespace marker_utils::avoidance_marker
+namespace behavior_path_planner::utils::avoidance
 {
-
-using tier4_autoware_utils::appendMarkerArray;
-using tier4_autoware_utils::calcDistance2d;
-using tier4_autoware_utils::calcOffsetPose;
-using tier4_autoware_utils::createDefaultMarker;
-using tier4_autoware_utils::createMarkerColor;
-using tier4_autoware_utils::createMarkerScale;
-using tier4_autoware_utils::createPoint;
-using tier4_autoware_utils::getPose;
-using visualization_msgs::msg::Marker;
-
 namespace
 {
 
@@ -650,7 +638,7 @@ MarkerArray createDebugMarkerArray(
 
   return msg;
 }
-}  // namespace marker_utils::avoidance_marker
+}  // namespace behavior_path_planner::utils::avoidance
 
 std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr)
 {