Skip to content

Commit 1e4a53c

Browse files
satoshi-otasatoshi-ota
authored
refactor(safety_checker): remove lane change dependency (#4387)
* refactor(safety_checker): remove lane change dependency Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(safety_checker): init primitive member variables Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 4ae4705 commit 1e4a53c

File tree

5 files changed

+62
-52
lines changed

5 files changed

+62
-52
lines changed

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,10 @@
2626
namespace behavior_path_planner
2727
{
2828
using autoware_auto_planning_msgs::msg::PathWithLaneId;
29+
using behavior_path_planner::utils::safety_check::ExtendedPredictedObject;
30+
using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped;
31+
using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped;
32+
using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon;
2933
using geometry_msgs::msg::Point;
3034
using geometry_msgs::msg::Pose;
3135
using geometry_msgs::msg::Twist;

‎planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

+4-48
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
1616

1717
#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
18+
#include "behavior_path_planner/utils/safety_check.hpp"
1819
#include "lanelet2_core/geometry/Lanelet.h"
1920

2021
#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp"
@@ -28,51 +29,6 @@
2829
namespace behavior_path_planner
2930
{
3031

31-
struct PoseWithVelocity
32-
{
33-
Pose pose;
34-
double velocity;
35-
36-
PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {}
37-
};
38-
39-
struct PoseWithVelocityStamped : public PoseWithVelocity
40-
{
41-
double time;
42-
43-
PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity)
44-
: PoseWithVelocity(pose, velocity), time(time)
45-
{
46-
}
47-
};
48-
49-
struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped
50-
{
51-
Polygon2d poly;
52-
53-
PoseWithVelocityAndPolygonStamped(
54-
const double time, const Pose & pose, const double velocity, const Polygon2d & poly)
55-
: PoseWithVelocityStamped(time, pose, velocity), poly(poly)
56-
{
57-
}
58-
};
59-
60-
struct PredictedPathWithPolygon
61-
{
62-
float confidence;
63-
std::vector<PoseWithVelocityAndPolygonStamped> path;
64-
};
65-
66-
struct ExtendedPredictedObject
67-
{
68-
unique_identifier_msgs::msg::UUID uuid;
69-
geometry_msgs::msg::PoseWithCovariance initial_pose;
70-
geometry_msgs::msg::TwistWithCovariance initial_twist;
71-
geometry_msgs::msg::AccelWithCovariance initial_acceleration;
72-
autoware_auto_perception_msgs::msg::Shape shape;
73-
std::vector<PredictedPathWithPolygon> predicted_paths;
74-
};
75-
7632
struct LaneChangeCancelParameters
7733
{
7834
bool enable_on_prepare_phase{true};
@@ -176,9 +132,9 @@ struct LaneChangeTargetObjectIndices
176132

177133
struct LaneChangeTargetObjects
178134
{
179-
std::vector<ExtendedPredictedObject> current_lane{};
180-
std::vector<ExtendedPredictedObject> target_lane{};
181-
std::vector<ExtendedPredictedObject> other_lane{};
135+
std::vector<utils::safety_check::ExtendedPredictedObject> current_lane{};
136+
std::vector<utils::safety_check::ExtendedPredictedObject> target_lane{};
137+
std::vector<utils::safety_check::ExtendedPredictedObject> other_lane{};
182138
};
183139

184140
enum class LaneChangeModuleType {

‎planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -43,9 +43,10 @@ using autoware_auto_perception_msgs::msg::PredictedObject;
4343
using autoware_auto_perception_msgs::msg::PredictedObjects;
4444
using autoware_auto_perception_msgs::msg::PredictedPath;
4545
using autoware_auto_planning_msgs::msg::PathWithLaneId;
46-
using behavior_path_planner::ExtendedPredictedObject;
47-
using behavior_path_planner::PoseWithVelocityAndPolygonStamped;
48-
using behavior_path_planner::PredictedPathWithPolygon;
46+
using behavior_path_planner::utils::safety_check::ExtendedPredictedObject;
47+
using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped;
48+
using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped;
49+
using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon;
4950
using data::lane_change::PathSafetyStatus;
5051
using geometry_msgs::msg::Point;
5152
using geometry_msgs::msg::Pose;

‎planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp

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

1818
#include "behavior_path_planner/data_manager.hpp"
1919
#include "behavior_path_planner/marker_utils/utils.hpp"
20-
#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
2120

2221
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
2322

@@ -50,6 +49,51 @@ using tier4_autoware_utils::Point2d;
5049
using tier4_autoware_utils::Polygon2d;
5150
using vehicle_info_util::VehicleInfo;
5251

52+
struct PoseWithVelocity
53+
{
54+
Pose pose;
55+
double velocity{0.0};
56+
57+
PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {}
58+
};
59+
60+
struct PoseWithVelocityStamped : public PoseWithVelocity
61+
{
62+
double time{0.0};
63+
64+
PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity)
65+
: PoseWithVelocity(pose, velocity), time(time)
66+
{
67+
}
68+
};
69+
70+
struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped
71+
{
72+
Polygon2d poly;
73+
74+
PoseWithVelocityAndPolygonStamped(
75+
const double time, const Pose & pose, const double velocity, const Polygon2d & poly)
76+
: PoseWithVelocityStamped(time, pose, velocity), poly(poly)
77+
{
78+
}
79+
};
80+
81+
struct PredictedPathWithPolygon
82+
{
83+
float confidence{0.0};
84+
std::vector<PoseWithVelocityAndPolygonStamped> path;
85+
};
86+
87+
struct ExtendedPredictedObject
88+
{
89+
unique_identifier_msgs::msg::UUID uuid;
90+
geometry_msgs::msg::PoseWithCovariance initial_pose;
91+
geometry_msgs::msg::TwistWithCovariance initial_twist;
92+
geometry_msgs::msg::AccelWithCovariance initial_acceleration;
93+
autoware_auto_perception_msgs::msg::Shape shape;
94+
std::vector<PredictedPathWithPolygon> predicted_paths;
95+
};
96+
5397
namespace bg = boost::geometry;
5498

5599
bool isTargetObjectFront(

‎planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

+5
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include "behavior_path_planner/marker_utils/utils.hpp"
2020
#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
2121
#include "behavior_path_planner/utils/lane_following/module_data.hpp"
22+
#include "behavior_path_planner/utils/safety_check.hpp"
2223
#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
2324
#include "motion_utils/motion_utils.hpp"
2425
#include "object_recognition_utils/predicted_path_utils.hpp"
@@ -65,6 +66,10 @@ using autoware_auto_perception_msgs::msg::Shape;
6566
using autoware_auto_planning_msgs::msg::Path;
6667
using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
6768
using autoware_auto_planning_msgs::msg::PathWithLaneId;
69+
using behavior_path_planner::utils::safety_check::ExtendedPredictedObject;
70+
using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped;
71+
using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped;
72+
using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon;
6873
using geometry_msgs::msg::Point;
6974
using geometry_msgs::msg::Pose;
7075
using geometry_msgs::msg::PoseArray;

0 commit comments

Comments
 (0)