4
4
#define MISSION_PLANNER_NODE_HPP_
5
5
6
6
#include " lanelet2_core/geometry/LineString.h"
7
- #include " lanelet_helper/lanelet_converter.hpp"
8
- #include " lanelet_helper/lanelet_geometry.hpp"
9
- #include " lanelet_helper/lanelet_tools.hpp"
10
7
#include " lib_mission_planner/helper_functions.hpp"
11
- #include " mission_planner_messages /msg/driving_corridor.hpp"
12
- #include " mission_planner_messages /msg/local_map.hpp"
13
- #include " mission_planner_messages /msg/mission.hpp"
14
- #include " mission_planner_messages /msg/mission_lanes_stamped.hpp"
15
- #include " mission_planner_messages /msg/visualization_distance.hpp"
8
+ #include " autoware_planning_msgs /msg/driving_corridor.hpp"
9
+ #include " autoware_planning_msgs /msg/local_map.hpp"
10
+ #include " autoware_planning_msgs /msg/mission.hpp"
11
+ #include " autoware_planning_msgs /msg/mission_lanes_stamped.hpp"
12
+ #include " autoware_planning_msgs /msg/visualization_distance.hpp"
16
13
#include " rclcpp/rclcpp.hpp"
17
14
#include " tf2_ros/buffer.h"
18
15
#include " tf2_ros/transform_listener.h"
@@ -103,13 +100,13 @@ class MissionPlannerNode : public rclcpp::Node
103
100
* @param converted_lanelets The lanelets from the road model
104
101
(std::vector<lanelet::Lanelet>).
105
102
* @param lanelet_connections The lanelet connections from the road model
106
- (std::vector<lanelet_types:: LaneletConnection>).
103
+ (std::vector<LaneletConnection>).
107
104
* @return bool (is on goal lane or not).
108
105
*/
109
106
bool IsOnGoalLane_ (
110
107
const int ego_lanelet_index, const lanelet::BasicPoint2d & goal_point,
111
108
const std::vector<lanelet::Lanelet> & converted_lanelets,
112
- const std::vector<lanelet_types:: LaneletConnection> & lanelet_connections);
109
+ const std::vector<LaneletConnection> & lanelet_connections);
113
110
114
111
/* *
115
112
* @brief Function which checks if the goal point has a negative x value und
@@ -119,11 +116,11 @@ class MissionPlannerNode : public rclcpp::Node
119
116
* @param converted_lanelets The lanelets from the road model
120
117
(std::vector<lanelet::Lanelet>).
121
118
* @param lanelet_connections The lanelet connections from the road model
122
- (std::vector<lanelet_types:: LaneletConnection>).
119
+ (std::vector<LaneletConnection>).
123
120
*/
124
121
void CheckIfGoalPointShouldBeReset_ (
125
122
const lanelet::Lanelets & converted_lanelets,
126
- const std::vector<lanelet_types:: LaneletConnection> & lanelet_connections);
123
+ const std::vector<LaneletConnection> & lanelet_connections);
127
124
128
125
/* *
129
126
* @brief Function for calculating lanes
@@ -135,7 +132,7 @@ class MissionPlannerNode : public rclcpp::Node
135
132
*/
136
133
Lanes CalculateLanes_ (
137
134
const std::vector<lanelet::Lanelet> & converted_lanelets,
138
- std::vector<lanelet_types:: LaneletConnection> & lanelet_connections);
135
+ std::vector<LaneletConnection> & lanelet_connections);
139
136
140
137
/* *
141
138
* @brief Function for creating a marker array.
@@ -152,7 +149,7 @@ class MissionPlannerNode : public rclcpp::Node
152
149
const std::vector<lanelet::ConstLineString3d> & centerline,
153
150
const std::vector<lanelet::ConstLineString3d> & left,
154
151
const std::vector<lanelet::ConstLineString3d> & right,
155
- const mission_planner_messages ::msg::RoadSegments & msg);
152
+ const autoware_planning_msgs ::msg::RoadSegments & msg);
156
153
157
154
/* *
158
155
* @brief Getter for goal_point_
@@ -174,36 +171,36 @@ class MissionPlannerNode : public rclcpp::Node
174
171
* @param lane The lane which is a std::vector<int> containing all the indices
175
172
* of the lane.
176
173
* @param converted_lanelets The lanelets (std::vector<lanelet::Lanelet>).
177
- * @return mission_planner_messages ::msg::DrivingCorridor
174
+ * @return autoware_planning_msgs ::msg::DrivingCorridor
178
175
*/
179
- mission_planner_messages ::msg::DrivingCorridor CreateDrivingCorridor_ (
176
+ autoware_planning_msgs ::msg::DrivingCorridor CreateDrivingCorridor_ (
180
177
const std::vector<int > & lane, const std::vector<lanelet::Lanelet> & converted_lanelets);
181
178
182
179
/* *
183
180
* @brief The callback for the Mission messages.
184
181
*
185
- * @param msg The mission_planner_messages ::msg::Mission message.
182
+ * @param msg The autoware_planning_msgs ::msg::Mission message.
186
183
*/
187
- void CallbackMissionMessages_ (const mission_planner_messages ::msg::Mission & msg);
184
+ void CallbackMissionMessages_ (const autoware_planning_msgs ::msg::Mission & msg);
188
185
189
186
/* *
190
187
* @brief The callback for the LocalMap messages.
191
188
*
192
- * @param msg The mission_planner_messages ::msg::LocalMap message.
189
+ * @param msg The autoware_planning_msgs ::msg::LocalMap message.
193
190
*/
194
- void CallbackLocalMapMessages_ (const mission_planner_messages ::msg::LocalMap & msg);
191
+ void CallbackLocalMapMessages_ (const autoware_planning_msgs ::msg::LocalMap & msg);
195
192
196
193
/* *
197
194
* @brief Convert RoadSegments into lanelets.
198
195
*
199
- * @param msg The message (mission_planner_messages ::msg::RoadSegments).
196
+ * @param msg The message (autoware_planning_msgs ::msg::RoadSegments).
200
197
* @param out_lanelets The lanelets (output).
201
198
* @param out_lanelet_connections The lanelet connections (output).
202
199
*/
203
200
void ConvertInput2LaneletFormat (
204
- const mission_planner_messages ::msg::RoadSegments & msg,
201
+ const autoware_planning_msgs ::msg::RoadSegments & msg,
205
202
std::vector<lanelet::Lanelet> & out_lanelets,
206
- std::vector<lanelet_types:: LaneletConnection> & out_lanelet_connections);
203
+ std::vector<LaneletConnection> & out_lanelet_connections);
207
204
208
205
private:
209
206
// ###########################################################################
@@ -217,7 +214,7 @@ class MissionPlannerNode : public rclcpp::Node
217
214
* @param converted_lanelets The lanelets (std::vector<lanelet::Lanelet>).
218
215
*/
219
216
void VisualizeLanes_ (
220
- const mission_planner_messages ::msg::RoadSegments & msg,
217
+ const autoware_planning_msgs ::msg::RoadSegments & msg,
221
218
const std::vector<lanelet::Lanelet> & converted_lanelets);
222
219
223
220
/* *
@@ -227,8 +224,8 @@ class MissionPlannerNode : public rclcpp::Node
227
224
* @param driving_corridor The considered driving corridor for which the centerline is visualized.
228
225
*/
229
226
void VisualizeCenterlineOfDrivingCorridor_ (
230
- const mission_planner_messages ::msg::RoadSegments & msg,
231
- const mission_planner_messages ::msg::DrivingCorridor & driving_corridor);
227
+ const autoware_planning_msgs ::msg::RoadSegments & msg,
228
+ const autoware_planning_msgs ::msg::DrivingCorridor & driving_corridor);
232
229
233
230
/* *
234
231
* @brief Function for creating a lanelet::LineString2d.
@@ -264,8 +261,7 @@ class MissionPlannerNode : public rclcpp::Node
264
261
* @return std::vector<int>
265
262
*/
266
263
std::vector<int > GetAllNeighborsOfLane (
267
- const std::vector<int > & lane,
268
- const std::vector<lanelet_types::LaneletConnection> & lanelet_connections,
264
+ const std::vector<int > & lane, const std::vector<LaneletConnection> & lanelet_connections,
269
265
const int vehicle_side);
270
266
271
267
/* *
@@ -277,35 +273,34 @@ class MissionPlannerNode : public rclcpp::Node
277
273
*
278
274
*/
279
275
void InsertPredecessorLanelet (
280
- std::vector<int > & lane_idx,
281
- const std::vector<lanelet_types::LaneletConnection> & lanelet_connections);
276
+ std::vector<int > & lane_idx, const std::vector<LaneletConnection> & lanelet_connections);
282
277
283
278
/* *
284
279
* @brief Calculate the predecessors.
285
280
*
286
281
* @param lanelet_connections The lanelet connections.
287
282
*/
288
- void CalculatePredecessors (std::vector<lanelet_types:: LaneletConnection> & lanelet_connections);
283
+ void CalculatePredecessors (std::vector<LaneletConnection> & lanelet_connections);
289
284
290
285
// ###########################################################################
291
286
// # PRIVATE VARIABLES
292
287
// ###########################################################################
293
288
// Declare ROS2 publisher and subscriber
294
- rclcpp::Subscription<mission_planner_messages ::msg::LocalMap>::SharedPtr mapSubscriber_;
289
+ rclcpp::Subscription<autoware_planning_msgs ::msg::LocalMap>::SharedPtr mapSubscriber_;
295
290
296
- rclcpp::Subscription<mission_planner_messages ::msg::Mission>::SharedPtr missionSubscriber_;
291
+ rclcpp::Subscription<autoware_planning_msgs ::msg::Mission>::SharedPtr missionSubscriber_;
297
292
298
293
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr visualizationPublisher_;
299
294
300
295
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
301
296
visualization_publisher_centerline_;
302
297
303
- rclcpp::Publisher<mission_planner_messages ::msg::VisualizationDistance>::SharedPtr
298
+ rclcpp::Publisher<autoware_planning_msgs ::msg::VisualizationDistance>::SharedPtr
304
299
visualizationDistancePublisher_;
305
300
306
301
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr visualizationGoalPointPublisher_;
307
302
308
- rclcpp::Publisher<mission_planner_messages ::msg::MissionLanesStamped>::SharedPtr
303
+ rclcpp::Publisher<autoware_planning_msgs ::msg::MissionLanesStamped>::SharedPtr
309
304
missionLanesStampedPublisher_;
310
305
311
306
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr OdometrySubscriber_;
0 commit comments