Skip to content

Commit b6d7477

Browse files
satoshi-otasatoshi-ota
authored
refactor(avoidance): organize alias (#6947)
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent da9df0c commit b6d7477

File tree

11 files changed

+128
-133
lines changed

11 files changed

+128
-133
lines changed

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

+1-16
Original file line numberDiff line numberDiff line change
@@ -15,16 +15,12 @@
1515
#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_
1616
#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_
1717

18+
#include "behavior_path_avoidance_module/type_alias.hpp"
1819
#include "behavior_path_planner_common/data_manager.hpp"
1920
#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
2021
#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
2122

2223
#include <rclcpp/time.hpp>
23-
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
24-
25-
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
26-
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
27-
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
2824

2925
#include <lanelet2_core/primitives/Lanelet.h>
3026
#include <lanelet2_core/primitives/LineString.h>
@@ -38,19 +34,8 @@
3834

3935
namespace behavior_path_planner
4036
{
41-
using autoware_auto_perception_msgs::msg::PredictedObject;
42-
using autoware_auto_perception_msgs::msg::PredictedPath;
43-
using autoware_auto_planning_msgs::msg::PathWithLaneId;
44-
45-
using tier4_autoware_utils::Point2d;
46-
using tier4_autoware_utils::Polygon2d;
47-
using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
48-
49-
using geometry_msgs::msg::Point;
50-
using geometry_msgs::msg::Pose;
5137

5238
using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
53-
5439
using route_handler::Direction;
5540

5641
enum class ObjectInfo {

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp

+4-10
Original file line numberDiff line numberDiff line change
@@ -16,26 +16,20 @@
1616
#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_
1717

1818
#include "behavior_path_avoidance_module/data_structs.hpp"
19-
20-
#include <tier4_autoware_utils/ros/marker_helper.hpp>
21-
22-
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
23-
#include <visualization_msgs/msg/marker_array.hpp>
19+
#include "behavior_path_avoidance_module/type_alias.hpp"
2420

2521
#include <string>
2622

27-
namespace marker_utils::avoidance_marker
23+
namespace behavior_path_planner::utils::avoidance
2824
{
29-
using autoware_auto_planning_msgs::msg::PathWithLaneId;
25+
3026
using behavior_path_planner::AvoidancePlanningData;
3127
using behavior_path_planner::AvoidLineArray;
3228
using behavior_path_planner::DebugData;
3329
using behavior_path_planner::ObjectDataArray;
3430
using behavior_path_planner::ObjectInfo;
3531
using behavior_path_planner::PathShifter;
3632
using behavior_path_planner::ShiftLineArray;
37-
using geometry_msgs::msg::Pose;
38-
using visualization_msgs::msg::MarkerArray;
3933

4034
MarkerArray createEgoStatusMarkerArray(
4135
const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns);
@@ -52,7 +46,7 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
5246

5347
MarkerArray createDebugMarkerArray(
5448
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug);
55-
} // namespace marker_utils::avoidance_marker
49+
} // namespace behavior_path_planner::utils::avoidance
5650

5751
std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr);
5852

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

+10-15
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,10 @@
1616
#define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_
1717

1818
#include "behavior_path_avoidance_module/data_structs.hpp"
19+
#include "behavior_path_avoidance_module/type_alias.hpp"
1920
#include "behavior_path_avoidance_module/utils.hpp"
2021
#include "behavior_path_planner_common/utils/utils.hpp"
2122

22-
#include <motion_utils/distance/distance.hpp>
23-
2423
#include <algorithm>
2524
#include <limits>
2625
#include <memory>
@@ -32,11 +31,6 @@ namespace behavior_path_planner::helper::avoidance
3231

3332
using behavior_path_planner::PathShifter;
3433
using behavior_path_planner::PlannerData;
35-
using motion_utils::calcDecelDistWithJerkAndAccConstraints;
36-
using motion_utils::calcLongitudinalOffsetPose;
37-
using motion_utils::calcSignedArcLength;
38-
using motion_utils::findNearestIndex;
39-
using tier4_autoware_utils::getPose;
4034

4135
class AvoidanceHelper
4236
{
@@ -181,14 +175,14 @@ class AvoidanceHelper
181175
double getShift(const Point & p) const
182176
{
183177
validate();
184-
const auto idx = findNearestIndex(prev_reference_path_.points, p);
178+
const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p);
185179
return prev_spline_shift_path_.shift_length.at(idx);
186180
}
187181

188182
double getLinearShift(const Point & p) const
189183
{
190184
validate();
191-
const auto idx = findNearestIndex(prev_reference_path_.points, p);
185+
const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p);
192186
return prev_linear_shift_path_.shift_length.at(idx);
193187
}
194188

@@ -282,8 +276,8 @@ class AvoidanceHelper
282276
}
283277

284278
const auto start_idx = data_->findEgoIndex(path.points);
285-
const auto distance =
286-
calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position);
279+
const auto distance = motion_utils::calcSignedArcLength(
280+
path.points, start_idx, max_v_point_.value().first.position);
287281
return std::make_pair(distance, max_v_point_.value().second);
288282
}
289283

@@ -294,7 +288,7 @@ class AvoidanceHelper
294288
const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
295289
const auto & a_lim = use_hard_constraints ? p->max_deceleration : p->nominal_deceleration;
296290
const auto & j_lim = use_hard_constraints ? p->max_jerk : p->nominal_jerk;
297-
const auto ret = calcDecelDistWithJerkAndAccConstraints(
291+
const auto ret = motion_utils::calcDecelDistWithJerkAndAccConstraints(
298292
getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim);
299293

300294
if (!!ret) {
@@ -441,14 +435,15 @@ class AvoidanceHelper
441435
const auto x_max_accel =
442436
v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0;
443437

444-
const auto point =
445-
calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
438+
const auto point = motion_utils::calcLongitudinalOffsetPose(
439+
path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
446440
if (point.has_value()) {
447441
max_v_point_ = std::make_pair(point.value(), v_max);
448442
return;
449443
}
450444

451-
const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
445+
const auto x_end =
446+
motion_utils::calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
452447
const auto t_end =
453448
(std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration;
454449
const auto v_end = v0 + p->max_acceleration * t_end;

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

+4-13
Original file line numberDiff line numberDiff line change
@@ -18,19 +18,14 @@
1818
#include "behavior_path_avoidance_module/data_structs.hpp"
1919
#include "behavior_path_avoidance_module/helper.hpp"
2020
#include "behavior_path_avoidance_module/shift_line_generator.hpp"
21+
#include "behavior_path_avoidance_module/type_alias.hpp"
2122
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
2223
#include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
2324

2425
#include <rclcpp/logging.hpp>
2526
#include <rclcpp/node.hpp>
2627
#include <rclcpp/time.hpp>
2728

28-
#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
29-
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
30-
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
31-
#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
32-
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
33-
3429
#include <memory>
3530
#include <string>
3631
#include <unordered_map>
@@ -39,12 +34,8 @@
3934
namespace behavior_path_planner
4035
{
4136

42-
using motion_utils::calcSignedArcLength;
43-
using motion_utils::findNearestIndex;
44-
45-
using tier4_planning_msgs::msg::AvoidanceDebugMsg;
46-
4737
using helper::avoidance::AvoidanceHelper;
38+
using tier4_planning_msgs::msg::AvoidanceDebugMsg;
4839

4940
class AvoidanceModule : public SceneModuleInterface
5041
{
@@ -114,7 +105,7 @@ class AvoidanceModule : public SceneModuleInterface
114105

115106
for (const auto & left_shift : left_shift_array_) {
116107
const double start_distance =
117-
calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
108+
motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
118109
const double finish_distance = start_distance + left_shift.relative_longitudinal;
119110
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
120111
left_shift.uuid, true, start_distance, finish_distance, clock_->now());
@@ -127,7 +118,7 @@ class AvoidanceModule : public SceneModuleInterface
127118

128119
for (const auto & right_shift : right_shift_array_) {
129120
const double start_distance =
130-
calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
121+
motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
131122
const double finish_distance = start_distance + right_shift.relative_longitudinal;
132123
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
133124
right_shift.uuid, true, start_distance, finish_distance, clock_->now());

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include "behavior_path_avoidance_module/data_structs.hpp"
1919
#include "behavior_path_avoidance_module/helper.hpp"
20+
#include "behavior_path_avoidance_module/type_alias.hpp"
2021
#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
2122

2223
#include <memory>

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp

+72
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_
16+
#define BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_
17+
18+
#include <motion_utils/distance/distance.hpp>
19+
#include <motion_utils/trajectory/path_with_lane_id.hpp>
20+
#include <motion_utils/trajectory/trajectory.hpp>
21+
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
22+
#include <tier4_autoware_utils/geometry/geometry.hpp>
23+
#include <tier4_autoware_utils/ros/marker_helper.hpp>
24+
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
25+
26+
#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
27+
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
28+
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
29+
#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
30+
#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
31+
#include <visualization_msgs/msg/marker_array.hpp>
32+
33+
namespace behavior_path_planner
34+
{
35+
// auto msgs
36+
using autoware_auto_perception_msgs::msg::PredictedObject;
37+
using autoware_auto_perception_msgs::msg::PredictedPath;
38+
using autoware_auto_planning_msgs::msg::PathWithLaneId;
39+
40+
// ROS 2 general msgs
41+
using geometry_msgs::msg::Point;
42+
using geometry_msgs::msg::Pose;
43+
using geometry_msgs::msg::TransformStamped;
44+
using geometry_msgs::msg::Vector3;
45+
using std_msgs::msg::ColorRGBA;
46+
using visualization_msgs::msg::Marker;
47+
using visualization_msgs::msg::MarkerArray;
48+
49+
// tier4 msgs
50+
using tier4_planning_msgs::msg::AvoidanceDebugMsg;
51+
using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
52+
53+
// tier4 utils functions
54+
using tier4_autoware_utils::appendMarkerArray;
55+
using tier4_autoware_utils::calcDistance2d;
56+
using tier4_autoware_utils::calcLateralDeviation;
57+
using tier4_autoware_utils::calcOffsetPose;
58+
using tier4_autoware_utils::calcYawDeviation;
59+
using tier4_autoware_utils::createDefaultMarker;
60+
using tier4_autoware_utils::createMarkerColor;
61+
using tier4_autoware_utils::createMarkerScale;
62+
using tier4_autoware_utils::createPoint;
63+
using tier4_autoware_utils::createQuaternionFromRPY;
64+
using tier4_autoware_utils::getPoint;
65+
using tier4_autoware_utils::getPose;
66+
using tier4_autoware_utils::Point2d;
67+
using tier4_autoware_utils::Polygon2d;
68+
using tier4_autoware_utils::pose2transform;
69+
using tier4_autoware_utils::toHexString;
70+
} // namespace behavior_path_planner
71+
72+
#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525

2626
namespace behavior_path_planner::utils::avoidance
2727
{
28+
2829
using behavior_path_planner::PlannerData;
2930
using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
3031
using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;

‎planning/behavior_path_avoidance_module/src/debug.cpp

+2-14
Original file line numberDiff line numberDiff line change
@@ -19,24 +19,12 @@
1919

2020
#include <lanelet2_extension/visualization/visualization.hpp>
2121
#include <magic_enum.hpp>
22-
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
2322

2423
#include <string>
2524
#include <vector>
2625

27-
namespace marker_utils::avoidance_marker
26+
namespace behavior_path_planner::utils::avoidance
2827
{
29-
30-
using tier4_autoware_utils::appendMarkerArray;
31-
using tier4_autoware_utils::calcDistance2d;
32-
using tier4_autoware_utils::calcOffsetPose;
33-
using tier4_autoware_utils::createDefaultMarker;
34-
using tier4_autoware_utils::createMarkerColor;
35-
using tier4_autoware_utils::createMarkerScale;
36-
using tier4_autoware_utils::createPoint;
37-
using tier4_autoware_utils::getPose;
38-
using visualization_msgs::msg::Marker;
39-
4028
namespace
4129
{
4230

@@ -650,7 +638,7 @@ MarkerArray createDebugMarkerArray(
650638

651639
return msg;
652640
}
653-
} // namespace marker_utils::avoidance_marker
641+
} // namespace behavior_path_planner::utils::avoidance
654642

655643
std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr)
656644
{

0 commit comments

Comments
 (0)