Skip to content

Commit 8c62d21

Browse files
takayuki5168SakodaShintaro
takayuki5168
authored and
SakodaShintaro
committed
feat(motion_velocity_planner): common implementation for motion_velocity_obstacle_<stop/slow_down/cruise>_module (autowarefoundation#10035)
* feat(motion_velocity_planner): prepare for motion_velocity_<stop/slow_down/cruise>_module Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update launch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent f496b37 commit 8c62d21

File tree

32 files changed

+1122
-98
lines changed

32 files changed

+1122
-98
lines changed

‎launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

+23
Original file line numberDiff line numberDiff line change
@@ -2,13 +2,31 @@
22
<arg name="interface_input_topic" default="/planning/scenario_planning/lane_driving/behavior_planning/path"/>
33
<arg name="interface_output_topic" default="/planning/scenario_planning/lane_driving/trajectory"/>
44

5+
<arg name="launch_obstacle_stop_module" default="true"/>
6+
<arg name="launch_obstacle_slow_down_module" default="true"/>
7+
<arg name="launch_obstacle_cruise_module" default="true"/>
58
<arg name="launch_dynamic_obstacle_stop_module" default="true"/>
69
<arg name="launch_out_of_lane_module" default="true"/>
710
<arg name="launch_obstacle_velocity_limiter_module" default="true"/>
811
<arg name="launch_module_list_end" default="&quot;&quot;]"/>
912

1013
<!-- assemble launch config for motion velocity planner -->
1114
<arg name="motion_velocity_planner_launch_modules" default="["/>
15+
<let
16+
name="motion_velocity_planner_launch_modules"
17+
value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleStopModule, '&quot;)"
18+
if="$(var launch_obstacle_stop_module)"
19+
/>
20+
<let
21+
name="motion_velocity_planner_launch_modules"
22+
value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleSlowDownModule, '&quot;)"
23+
if="$(var launch_obstacle_slow_down_module)"
24+
/>
25+
<let
26+
name="motion_velocity_planner_launch_modules"
27+
value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleCruiseModule, '&quot;)"
28+
if="$(var launch_obstacle_cruise_module)"
29+
/>
1230
<let
1331
name="motion_velocity_planner_launch_modules"
1432
value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::OutOfLaneModule, '&quot;)"
@@ -131,6 +149,8 @@
131149
<remap from="~/input/virtual_traffic_light_states" to="/perception/virtual_traffic_light_states"/>
132150
<remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
133151
<remap from="~/output/trajectory" to="motion_velocity_planner/trajectory"/>
152+
<remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
153+
<remap from="~/output/clear_velocity_limit" to="/planning/scenario_planning/clear_velocity_limit"/>
134154
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
135155
<remap from="~/output/velocity_factors" to="/planning/velocity_factors/motion_velocity_planner"/>
136156
<!-- params -->
@@ -141,6 +161,9 @@
141161
<param from="$(var velocity_smoother_param_path)"/>
142162
<param from="$(var motion_velocity_planner_velocity_smoother_type_param_path)"/>
143163
<param from="$(var motion_velocity_planner_param_path)"/>
164+
<param from="$(var motion_velocity_planner_obstacle_stop_module_param_path)"/>
165+
<param from="$(var motion_velocity_planner_obstacle_slow_down_module_param_path)"/>
166+
<param from="$(var motion_velocity_planner_obstacle_cruise_module_param_path)"/>
144167
<param from="$(var motion_velocity_planner_dynamic_obstacle_stop_module_param_path)"/>
145168
<param from="$(var motion_velocity_planner_out_of_lane_module_param_path)"/>
146169
<param from="$(var motion_velocity_planner_obstacle_velocity_limiter_param_path)"/>

‎planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp

+6-4
Original file line numberDiff line numberDiff line change
@@ -97,19 +97,21 @@ void DynamicObstacleStopModule::update_parameters(const std::vector<rclcpp::Para
9797
}
9898

9999
VelocityPlanningResult DynamicObstacleStopModule::plan(
100-
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
100+
[[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
101+
raw_trajectory_points,
102+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
101103
const std::shared_ptr<const PlannerData> planner_data)
102104
{
103105
VelocityPlanningResult result;
104106
debug_data_.reset_data();
105-
if (ego_trajectory_points.size() < 2) return result;
107+
if (smoothed_trajectory_points.size() < 2) return result;
106108

107109
autoware::universe_utils::StopWatch<std::chrono::microseconds> stopwatch;
108110
stopwatch.tic();
109111
stopwatch.tic("preprocessing");
110112
dynamic_obstacle_stop::EgoData ego_data;
111113
ego_data.pose = planner_data->current_odometry.pose.pose;
112-
ego_data.trajectory = ego_trajectory_points;
114+
ego_data.trajectory = smoothed_trajectory_points;
113115
autoware::motion_utils::removeOverlapPoints(ego_data.trajectory);
114116
ego_data.first_trajectory_idx =
115117
autoware::motion_utils::findNearestSegmentIndex(ego_data.trajectory, ego_data.pose.position);
@@ -164,7 +166,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
164166
if (stop_pose) {
165167
result.stop_points.push_back(stop_pose->position);
166168
planning_factor_interface_->add(
167-
ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP,
169+
smoothed_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP,
168170
SafetyFactorArray{});
169171
create_virtual_walls();
170172
}

‎planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,8 @@ class DynamicObstacleStopModule : public PluginModuleInterface
3636
void publish_planning_factor() override { planning_factor_interface_->publish(); };
3737
void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
3838
VelocityPlanningResult plan(
39-
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
39+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
40+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
4041
const std::shared_ptr<const PlannerData> planner_data) override;
4142
std::string get_module_name() const override { return module_name_; }
4243

‎planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -96,12 +96,12 @@ bool is_unavoidable(
9696
};
9797

9898
std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
99-
const std::vector<PlannerData::Object> & objects, const EgoData & ego_data,
99+
const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const EgoData & ego_data,
100100
const PlannerParam & params, const double hysteresis)
101101
{
102102
std::vector<autoware_perception_msgs::msg::PredictedObject> filtered_objects;
103103
for (const auto & object : objects) {
104-
const auto & predicted_object = object.predicted_object;
104+
const auto & predicted_object = object->predicted_object;
105105
const auto is_not_too_slow =
106106
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >=
107107
params.minimum_object_velocity;

‎planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,7 @@ bool is_unavoidable(
7676
/// @param hysteresis [m] extra distance threshold used for filtering
7777
/// @return filtered predicted objects
7878
std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
79-
const std::vector<PlannerData::Object> & objects, const EgoData & ego_data,
79+
const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const EgoData & ego_data,
8080
const PlannerParam & params, const double hysteresis);
8181

8282
} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop

‎planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp

+11-11
Original file line numberDiff line numberDiff line change
@@ -53,23 +53,23 @@ struct PlannerParam
5353

5454
struct EgoData
5555
{
56-
TrajectoryPoints trajectory;
56+
TrajectoryPoints trajectory{};
5757
size_t first_trajectory_idx{};
58-
double longitudinal_offset_to_first_trajectory_idx; // [m]
59-
geometry_msgs::msg::Pose pose;
60-
autoware::universe_utils::MultiPolygon2d trajectory_footprints;
61-
Rtree rtree;
62-
std::optional<geometry_msgs::msg::Pose> earliest_stop_pose;
58+
double longitudinal_offset_to_first_trajectory_idx{}; // [m]
59+
geometry_msgs::msg::Pose pose{};
60+
autoware::universe_utils::MultiPolygon2d trajectory_footprints{};
61+
Rtree rtree{};
62+
std::optional<geometry_msgs::msg::Pose> earliest_stop_pose{};
6363
};
6464

6565
/// @brief debug data
6666
struct DebugData
6767
{
68-
autoware::universe_utils::MultiPolygon2d obstacle_footprints;
68+
autoware::universe_utils::MultiPolygon2d obstacle_footprints{};
6969
size_t prev_dynamic_obstacles_nb{};
70-
autoware::universe_utils::MultiPolygon2d ego_footprints;
70+
autoware::universe_utils::MultiPolygon2d ego_footprints{};
7171
size_t prev_ego_footprints_nb{};
72-
std::optional<geometry_msgs::msg::Pose> stop_pose;
72+
std::optional<geometry_msgs::msg::Pose> stop_pose{};
7373
size_t prev_collisions_nb{};
7474
double z{};
7575
void reset_data()
@@ -82,8 +82,8 @@ struct DebugData
8282

8383
struct Collision
8484
{
85-
geometry_msgs::msg::Point point;
86-
std::string object_uuid;
85+
geometry_msgs::msg::Point point{};
86+
std::string object_uuid{};
8787
};
8888
} // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop
8989

‎planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -205,7 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable)
205205
TEST(TestObjectFiltering, filterPredictedObjects)
206206
{
207207
using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects;
208-
std::vector<autoware::motion_velocity_planner::PlannerData::Object> objects;
208+
std::vector<std::shared_ptr<autoware::motion_velocity_planner::PlannerData::Object>> objects;
209209
autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data;
210210
autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params;
211211
double hysteresis{};

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
2929
{
3030

3131
multi_polygon_t createPolygonMasks(
32-
const std::vector<PlannerData::Object> & dynamic_obstacles, const double buffer,
32+
const std::vector<std::shared_ptr<PlannerData::Object>> & dynamic_obstacles, const double buffer,
3333
const double min_vel)
3434
{
3535
return createObjectPolygons(dynamic_obstacles, buffer, min_vel);

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b
5656
/// @param[in] min_vel minimum velocity for an object to be masked
5757
/// @return polygon masks around dynamic objects
5858
multi_polygon_t createPolygonMasks(
59-
const std::vector<PlannerData::Object> & dynamic_obstacles, const double buffer,
59+
const std::vector<std::shared_ptr<PlannerData::Object>> & dynamic_obstacles, const double buffer,
6060
const double min_vel);
6161

6262
/// @brief create footprint polygons from projection lines

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

+5-3
Original file line numberDiff line numberDiff line change
@@ -135,22 +135,24 @@ void ObstacleVelocityLimiterModule::update_parameters(
135135
}
136136

137137
VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
138-
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
138+
[[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
139+
raw_trajectory_points,
140+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
139141
const std::shared_ptr<const PlannerData> planner_data)
140142
{
141143
autoware::universe_utils::StopWatch<std::chrono::microseconds> stopwatch;
142144
stopwatch.tic();
143145
VelocityPlanningResult result;
144146
stopwatch.tic("preprocessing");
145147
const auto ego_idx = autoware::motion_utils::findNearestIndex(
146-
ego_trajectory_points, planner_data->current_odometry.pose.pose);
148+
smoothed_trajectory_points, planner_data->current_odometry.pose.pose);
147149
if (!ego_idx) {
148150
RCLCPP_WARN_THROTTLE(
149151
logger_, *clock_, rcutils_duration_value_t(1000),
150152
"Cannot calculate ego index on the trajectory");
151153
return result;
152154
}
153-
auto original_traj_points = ego_trajectory_points;
155+
auto original_traj_points = smoothed_trajectory_points;
154156
if (preprocessing_params_.calculate_steering_angles)
155157
obstacle_velocity_limiter::calculateSteeringAngles(
156158
original_traj_points, projection_params_.wheel_base);

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,8 @@ class ObstacleVelocityLimiterModule : public PluginModuleInterface
4949
void init(rclcpp::Node & node, const std::string & module_name) override;
5050
void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
5151
VelocityPlanningResult plan(
52-
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
52+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
53+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
5354
const std::shared_ptr<const PlannerData> planner_data) override;
5455
std::string get_module_name() const override { return module_name_; }
5556

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -59,11 +59,12 @@ polygon_t createObjectPolygon(
5959
}
6060

6161
multi_polygon_t createObjectPolygons(
62-
const std::vector<PlannerData::Object> & objects, const double buffer, const double min_velocity)
62+
const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const double buffer,
63+
const double min_velocity)
6364
{
6465
multi_polygon_t polygons;
6566
for (const auto & object : objects) {
66-
const auto & predicted_object = object.predicted_object;
67+
const auto & predicted_object = object->predicted_object;
6768
const double obj_vel_norm = std::hypot(
6869
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
6970
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y);

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -164,7 +164,8 @@ polygon_t createObjectPolygon(
164164
/// @param [in] min_velocity objects with velocity lower will be ignored
165165
/// @return polygons of the objects
166166
multi_polygon_t createObjectPolygons(
167-
const std::vector<PlannerData::Object> & objects, const double buffer, const double min_velocity);
167+
const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const double buffer,
168+
const double min_velocity);
168169

169170
/// @brief add obstacles obtained from sensors to the given Obstacles object
170171
/// @param[out] obstacles Obstacles object in which to add the sensor obstacles

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -394,7 +394,7 @@ TEST(TestCollisionDistance, createObjPolygons)
394394
using autoware_perception_msgs::msg::PredictedObject;
395395
using autoware_perception_msgs::msg::PredictedObjects;
396396

397-
std::vector<PlannerData::Object> objects;
397+
std::vector<std::shared_ptr<PlannerData::Object>> objects;
398398

399399
auto polygons = createObjectPolygons(objects, 0.0, 0.0);
400400
EXPECT_TRUE(polygons.empty());
@@ -407,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons)
407407
object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0;
408408
object1.shape.dimensions.x = 1.0;
409409
object1.shape.dimensions.y = 1.0;
410-
objects.push_back(PlannerData::Object(object1));
410+
objects.push_back(std::make_shared<PlannerData::Object>(object1));
411411

412412
polygons = createObjectPolygons(objects, 0.0, 1.0);
413413
EXPECT_TRUE(polygons.empty());
@@ -434,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons)
434434
object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0;
435435
object2.shape.dimensions.x = 2.0;
436436
object2.shape.dimensions.y = 1.0;
437-
objects.push_back(PlannerData::Object(object2));
437+
objects.push_back(std::make_shared<PlannerData::Object>(object2));
438438

439439
polygons = createObjectPolygons(objects, 0.0, 2.0);
440440
ASSERT_EQ(polygons.size(), 1ul);

‎planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -108,7 +108,7 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
108108
autoware_perception_msgs::msg::PredictedObjects filtered_objects;
109109
filtered_objects.header = planner_data.predicted_objects_header;
110110
for (const auto & object : planner_data.objects) {
111-
const auto & predicted_object = object.predicted_object;
111+
const auto & predicted_object = object->predicted_object;
112112
const auto is_pedestrian =
113113
std::find_if(
114114
predicted_object.classification.begin(), predicted_object.classification.end(),

‎planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

+13-10
Original file line numberDiff line numberDiff line change
@@ -148,22 +148,23 @@ void OutOfLaneModule::update_parameters(const std::vector<rclcpp::Parameter> & p
148148

149149
void OutOfLaneModule::limit_trajectory_size(
150150
out_of_lane::EgoData & ego_data,
151-
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
151+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
152152
const double max_arc_length)
153153
{
154154
ego_data.first_trajectory_idx =
155-
motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
155+
motion_utils::findNearestSegmentIndex(smoothed_trajectory_points, ego_data.pose.position);
156156
ego_data.longitudinal_offset_to_first_trajectory_index =
157157
motion_utils::calcLongitudinalOffsetToSegment(
158-
ego_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position);
158+
smoothed_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position);
159159
auto l = -ego_data.longitudinal_offset_to_first_trajectory_index;
160-
ego_data.trajectory_points.push_back(ego_trajectory_points[ego_data.first_trajectory_idx]);
161-
for (auto i = ego_data.first_trajectory_idx + 1; i < ego_trajectory_points.size(); ++i) {
162-
l += universe_utils::calcDistance2d(ego_trajectory_points[i - 1], ego_trajectory_points[i]);
160+
ego_data.trajectory_points.push_back(smoothed_trajectory_points[ego_data.first_trajectory_idx]);
161+
for (auto i = ego_data.first_trajectory_idx + 1; i < smoothed_trajectory_points.size(); ++i) {
162+
l += universe_utils::calcDistance2d(
163+
smoothed_trajectory_points[i - 1], smoothed_trajectory_points[i]);
163164
if (l >= max_arc_length) {
164165
break;
165166
}
166-
ego_data.trajectory_points.push_back(ego_trajectory_points[i]);
167+
ego_data.trajectory_points.push_back(smoothed_trajectory_points[i]);
167168
}
168169
}
169170

@@ -213,7 +214,9 @@ out_of_lane::OutOfLaneData prepare_out_of_lane_data(const out_of_lane::EgoData &
213214
}
214215

215216
VelocityPlanningResult OutOfLaneModule::plan(
216-
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
217+
[[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
218+
raw_trajectory_points,
219+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
217220
const std::shared_ptr<const PlannerData> planner_data)
218221
{
219222
VelocityPlanningResult result;
@@ -223,7 +226,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
223226
stopwatch.tic("preprocessing");
224227
out_of_lane::EgoData ego_data;
225228
ego_data.pose = planner_data->current_odometry.pose.pose;
226-
limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length);
229+
limit_trajectory_size(ego_data, smoothed_trajectory_points, params_.max_arc_length);
227230
out_of_lane::calculate_min_stop_and_slowdown_distances(
228231
ego_data, *planner_data, previous_slowdown_pose_);
229232
prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length);
@@ -314,7 +317,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
314317
}
315318

316319
planning_factor_interface_->add(
317-
ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN,
320+
smoothed_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN,
318321
SafetyFactorArray{});
319322
virtual_wall_marker_creator.add_virtual_walls(
320323
out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_));

‎planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,8 @@ class OutOfLaneModule : public PluginModuleInterface
4141
void publish_planning_factor() override { planning_factor_interface_->publish(); };
4242
void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
4343
VelocityPlanningResult plan(
44-
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
44+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
45+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
4546
const std::shared_ptr<const PlannerData> planner_data) override;
4647
std::string get_module_name() const override { return module_name_; }
4748

@@ -51,7 +52,7 @@ class OutOfLaneModule : public PluginModuleInterface
5152
/// given length
5253
static void limit_trajectory_size(
5354
out_of_lane::EgoData & ego_data,
54-
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
55+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
5556
const double max_arc_length);
5657

5758
out_of_lane::PlannerParam params_{};

0 commit comments

Comments
 (0)