Skip to content

Commit ae833f3

Browse files
takayuki5168takayuki5168
authored
fix(behavior_velocity_planner): support u-turn for detection_area, vtl and stop line (#1700)
* fix(behavior_velocity_planner): deal with u-turn for detection_area, vtl, stop_line Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * enable behavior velocity test and fix bug Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix bug Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix collision check Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add test Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * remove unnecessary debug output Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent 6d425f8 commit ae833f3

File tree

30 files changed

+659
-550
lines changed

30 files changed

+659
-550
lines changed

‎common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp

+7-2
Original file line numberDiff line numberDiff line change
@@ -20,13 +20,14 @@
2020

2121
#include <boost/optional.hpp>
2222

23+
#include <algorithm>
2324
#include <utility>
2425
#include <vector>
2526

2627
namespace motion_utils
2728
{
2829
inline boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
29-
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id)
30+
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id)
3031
{
3132
size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error
3233
size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error
@@ -35,7 +36,7 @@ inline boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
3536
for (size_t i = 0; i < path.points.size(); ++i) {
3637
const auto & p = path.points.at(i);
3738
for (const auto & id : p.lane_ids) {
38-
if (id == lane_id) {
39+
if (id == target_lane_id) {
3940
if (!found_first_idx) {
4041
start_idx = i;
4142
found_first_idx = true;
@@ -46,6 +47,10 @@ inline boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
4647
}
4748

4849
if (found_first_idx) {
50+
// NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded.
51+
start_idx = static_cast<size_t>(std::max(0, static_cast<int>(start_idx) - 1));
52+
end_idx = std::min(path.points.size() - 1, end_idx + 1);
53+
4954
return std::make_pair(start_idx, end_idx);
5055
}
5156

‎common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -72,16 +72,16 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId)
7272
{
7373
const auto res = getPathIndexRangeWithLaneId(points, 3);
7474
EXPECT_EQ(res->first, 0U);
75-
EXPECT_EQ(res->second, 1U);
75+
EXPECT_EQ(res->second, 2U);
7676
}
7777
{
7878
const auto res = getPathIndexRangeWithLaneId(points, 1);
79-
EXPECT_EQ(res->first, 2U);
80-
EXPECT_EQ(res->second, 2U);
79+
EXPECT_EQ(res->first, 1U);
80+
EXPECT_EQ(res->second, 3U);
8181
}
8282
{
8383
const auto res = getPathIndexRangeWithLaneId(points, 2);
84-
EXPECT_EQ(res->first, 3U);
84+
EXPECT_EQ(res->first, 2U);
8585
EXPECT_EQ(res->second, 5U);
8686
}
8787
{
@@ -142,7 +142,7 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId)
142142
for (size_t i = 3; i < 9; ++i) {
143143
modified_path.points.at(i).lane_ids = {100};
144144
}
145-
EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 3U);
145+
EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U);
146146
EXPECT_EQ(
147147
findNearestSegmentIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U);
148148
}

‎planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,10 @@ struct PlannerData
7878
// occupancy grid
7979
nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid;
8080

81+
// nearest search
82+
double ego_nearest_dist_threshold;
83+
double ego_nearest_yaw_threshold;
84+
8185
// other internal data
8286
std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> traffic_light_id_map;
8387
// external data

‎planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -123,7 +123,7 @@ class BlindSpotModule : public SceneModuleInterface
123123
visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
124124

125125
private:
126-
int64_t lane_id_;
126+
const int64_t lane_id_;
127127
TurnDirection turn_direction_;
128128
bool has_traffic_light_;
129129
bool is_over_pass_judge_line_;

‎planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,8 @@ class DetectionAreaModule : public SceneModuleInterface
6666

6767
public:
6868
DetectionAreaModule(
69-
const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem,
69+
const int64_t module_id, const int64_t lane_id,
70+
const lanelet::autoware::DetectionArea & detection_area_reg_elem,
7071
const PlannerParam & planner_param, const rclcpp::Logger logger,
7172
const rclcpp::Clock::SharedPtr clock);
7273

@@ -85,6 +86,9 @@ class DetectionAreaModule : public SceneModuleInterface
8586
bool hasEnoughBrakingDistance(
8687
const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const;
8788

89+
// Lane id
90+
int64_t lane_id_;
91+
8892
// Key Feature
8993
const lanelet::autoware::DetectionArea & detection_area_reg_elem_;
9094

‎planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,8 @@ class NoStoppingAreaModule : public SceneModuleInterface
7979

8080
public:
8181
NoStoppingAreaModule(
82-
const int64_t module_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem,
82+
const int64_t module_id, const int64_t lane_id,
83+
const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem,
8384
const PlannerParam & planner_param, const rclcpp::Logger logger,
8485
const rclcpp::Clock::SharedPtr clock);
8586

@@ -91,6 +92,8 @@ class NoStoppingAreaModule : public SceneModuleInterface
9192
visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
9293

9394
private:
95+
const int64_t lane_id_;
96+
9497
mutable bool pass_judged_ = false;
9598
mutable bool is_stoppable_ = true;
9699
StateMachine state_machine_; //! for state

‎planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -203,8 +203,8 @@ struct DebugData
203203
PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0);
204204
//!< @brief wrapper for detection area polygon generation
205205
bool buildDetectionAreaPolygon(
206-
Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & pose,
207-
const PlannerParam & param);
206+
Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose,
207+
const size_t target_seg_idx, const PlannerParam & param);
208208
lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path);
209209
// Note : consider offset_from_start_to_ego and safety margin for collision here
210210
void handleCollisionOffset(std::vector<PossibleCollisionInfo> & possible_collisions, double offset);

‎planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp

+34
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
#include <set>
3838
#include <string>
3939
#include <unordered_map>
40+
#include <vector>
4041

4142
// Debug
4243
#include <rclcpp/rclcpp.hpp>
@@ -110,6 +111,22 @@ class SceneModuleInterface
110111

111112
void setSafe(const bool safe) { safe_ = safe; }
112113
void setDistance(const double distance) { distance_ = distance; }
114+
115+
template <class T>
116+
size_t findEgoSegmentIndex(const std::vector<T> & points) const
117+
{
118+
const auto & p = planner_data_;
119+
return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
120+
points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
121+
}
122+
123+
template <class T>
124+
size_t findEgoIndex(const std::vector<T> & points) const
125+
{
126+
const auto & p = planner_data_;
127+
return motion_utils::findFirstNearestIndexWithSoftConstraints(
128+
points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
129+
}
113130
};
114131

115132
class SceneModuleManagerInterface
@@ -261,6 +278,23 @@ class SceneModuleManagerInterface
261278
scene_modules_.erase(scene_module);
262279
}
263280

281+
template <class T>
282+
size_t findEgoSegmentIndex(const std::vector<T> & points) const
283+
{
284+
const auto & p = planner_data_;
285+
return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
286+
points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
287+
}
288+
289+
template <class T>
290+
size_t findEgoIndex(
291+
const std::vector<T> & points, const geometry_msgs::msg::Pose & ego_pose) const
292+
{
293+
const auto & p = planner_data_;
294+
return motion_utils::findFirstNearestIndexWithSoftConstraints(
295+
points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold);
296+
}
297+
264298
std::set<std::shared_ptr<SceneModuleInterface>> scene_modules_;
265299
std::set<int64_t> registered_module_id_set_;
266300

‎planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp

+4-7
Original file line numberDiff line numberDiff line change
@@ -90,14 +90,8 @@ class StopLineModule : public SceneModuleInterface
9090
visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
9191

9292
private:
93-
int64_t module_id_;
94-
9593
geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line);
9694

97-
boost::optional<StopLineModule::SegmentIndexWithPoint2d> findCollision(
98-
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
99-
const SearchRangeIndex & search_index);
100-
10195
boost::optional<StopLineModule::SegmentIndexWithOffset> findOffsetSegment(
10296
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
10397
const StopLineModule::SegmentIndexWithPoint2d & collision);
@@ -111,8 +105,11 @@ class StopLineModule : public SceneModuleInterface
111105
const StopLineModule::SegmentIndexWithPose & insert_index_with_pose,
112106
tier4_planning_msgs::msg::StopReason * stop_reason);
113107

114-
lanelet::ConstLineString3d stop_line_;
115108
int64_t lane_id_;
109+
110+
lanelet::ConstLineString3d stop_line_;
111+
112+
// State machine
116113
State state_;
117114

118115
// Parameter

‎planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -66,8 +66,9 @@ class TrafficLightModule : public SceneModuleInterface
6666

6767
public:
6868
TrafficLightModule(
69-
const int64_t module_id, const lanelet::TrafficLight & traffic_light_reg_elem,
70-
lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
69+
const int64_t module_id, const int64_t lane_id,
70+
const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane,
71+
const PlannerParam & planner_param, const rclcpp::Logger logger,
7172
const rclcpp::Clock::SharedPtr clock);
7273

7374
bool modifyPathVelocity(
@@ -123,6 +124,9 @@ class TrafficLightModule : public SceneModuleInterface
123124
autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState(
124125
const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const;
125126

127+
// Lane id
128+
const int64_t lane_id_;
129+
126130
// Key Feature
127131
const lanelet::TrafficLight & traffic_light_reg_elem_;
128132
lanelet::ConstLanelet lane_;

‎planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp

+12-9
Original file line numberDiff line numberDiff line change
@@ -75,8 +75,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface
7575

7676
public:
7777
VirtualTrafficLightModule(
78-
const int64_t module_id, const lanelet::autoware::VirtualTrafficLight & reg_elem,
79-
lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
78+
const int64_t module_id, const int64_t lane_id,
79+
const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane,
80+
const PlannerParam & planner_param, const rclcpp::Logger logger,
8081
const rclcpp::Clock::SharedPtr clock);
8182

8283
bool modifyPathVelocity(
@@ -87,7 +88,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface
8788
visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
8889

8990
private:
90-
const int64_t module_id_;
91+
const int64_t lane_id_;
9192
const lanelet::autoware::VirtualTrafficLight & reg_elem_;
9293
const lanelet::ConstLanelet lane_;
9394
const PlannerParam planner_param_;
@@ -101,13 +102,15 @@ class VirtualTrafficLightModule : public SceneModuleInterface
101102
void setStopReason(
102103
const geometry_msgs::msg::Pose & stop_pose, tier4_planning_msgs::msg::StopReason * stop_reason);
103104

104-
bool isBeforeStartLine();
105+
boost::optional<size_t> getPathIndexOfFirstEndLine();
105106

106-
bool isBeforeStopLine();
107+
bool isBeforeStartLine(const size_t end_line_idx);
107108

108-
bool isAfterAnyEndLine();
109+
bool isBeforeStopLine(const size_t end_line_idx);
109110

110-
bool isNearAnyEndLine();
111+
bool isAfterAnyEndLine(const size_t end_line_idx);
112+
113+
bool isNearAnyEndLine(const size_t end_line_idx);
111114

112115
boost::optional<tier4_v2x_msgs::msg::VirtualTrafficLightState> findCorrespondingState();
113116

@@ -117,11 +120,11 @@ class VirtualTrafficLightModule : public SceneModuleInterface
117120

118121
void insertStopVelocityAtStopLine(
119122
autoware_auto_planning_msgs::msg::PathWithLaneId * path,
120-
tier4_planning_msgs::msg::StopReason * stop_reason);
123+
tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx);
121124

122125
void insertStopVelocityAtEndLine(
123126
autoware_auto_planning_msgs::msg::PathWithLaneId * path,
124-
tier4_planning_msgs::msg::StopReason * stop_reason);
127+
tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx);
125128
};
126129
} // namespace behavior_velocity_planner
127130
#endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_

0 commit comments

Comments
 (0)