Skip to content

Commit c25cac8

Browse files
Remove driveblocks dependencies
Signed-off-by: Simon Eisenmann <simon.eisenmann@driveblocks.ai>
1 parent 5e6ca68 commit c25cac8

File tree

24 files changed

+630
-193
lines changed

24 files changed

+630
-193
lines changed

planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
#ifndef HMI_NODE_HPP_
44
#define HMI_NODE_HPP_
55

6-
#include "mission_planner_messages/msg/mission.hpp"
6+
#include "autoware_planning_msgs/msg/mission.hpp"
77
#include "rclcpp/rclcpp.hpp"
88

99
#include <string>
@@ -47,7 +47,7 @@ class HMINode : public rclcpp::Node
4747
// ###########################################################################
4848
// Declare ROS2 publisher and subscriber
4949

50-
rclcpp::Publisher<mission_planner_messages::msg::Mission>::SharedPtr mission_publisher_;
50+
rclcpp::Publisher<autoware_planning_msgs::msg::Mission>::SharedPtr mission_publisher_;
5151

5252
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
5353
};

planning/mapless_architecture/autoware_hmi/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,8 @@
1111

1212
<exec_depend>ros2launch</exec_depend>
1313

14-
<depend>mission_planner_messages</depend>
1514
<depend>rclcpp</depend>
15+
<depend>autoware_planning_msgs</depend>
1616

1717
<test_depend>ament_lint_auto</test_depend>
1818
<test_depend>autoware_lint_common</test_depend>

planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp

+8-8
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33

44
#include "hmi_node.hpp"
55

6-
#include "mission_planner_messages/msg/mission.hpp"
6+
#include "autoware_planning_msgs/msg/mission.hpp"
77
#include "rclcpp/rclcpp.hpp"
88

99
using std::placeholders::_1;
@@ -29,7 +29,7 @@ HMINode::HMINode() : Node("hmi_node")
2929

3030
// Initialize publisher
3131
mission_publisher_ =
32-
this->create_publisher<mission_planner_messages::msg::Mission>("hmi_node/output/mission", 1);
32+
this->create_publisher<autoware_planning_msgs::msg::Mission>("hmi_node/output/mission", 1);
3333

3434
// Initialize parameters callback handle
3535
param_callback_handle_ = this->add_on_set_parameters_callback(
@@ -64,17 +64,17 @@ rcl_interfaces::msg::SetParametersResult HMINode::ParamCallback_(
6464

6565
void HMINode::PublishMission_(std::string mission)
6666
{
67-
mission_planner_messages::msg::Mission missionMessage;
67+
autoware_planning_msgs::msg::Mission missionMessage;
6868
if (mission == "LANE_KEEP") {
69-
missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_KEEP;
69+
missionMessage.mission_type = autoware_planning_msgs::msg::Mission::LANE_KEEP;
7070
} else if (mission == "LANE_CHANGE_LEFT") {
71-
missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_CHANGE_LEFT;
71+
missionMessage.mission_type = autoware_planning_msgs::msg::Mission::LANE_CHANGE_LEFT;
7272
} else if (mission == "LANE_CHANGE_RIGHT") {
73-
missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_CHANGE_RIGHT;
73+
missionMessage.mission_type = autoware_planning_msgs::msg::Mission::LANE_CHANGE_RIGHT;
7474
} else if (mission == "TAKE_NEXT_EXIT_LEFT") {
75-
missionMessage.mission_type = mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_LEFT;
75+
missionMessage.mission_type = autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_LEFT;
7676
} else if (mission == "TAKE_NEXT_EXIT_RIGHT") {
77-
missionMessage.mission_type = mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_RIGHT;
77+
missionMessage.mission_type = autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_RIGHT;
7878
}
7979

8080
// TODO(simon.eisenmann@driveblocks.ai): Change deadline parameter
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11
# Local Map Provider Node
22

3-
This node converts the RoadSegments message into a LocalMap message right now. More functionality can be added later.
3+
This node converts the mission_planner_messages::msg::RoadSegments message into a mission_planner_messages::msg::LocalMap message right now. More functionality can be added later.

planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp

+6-6
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,8 @@
33
#ifndef LOCAL_MAP_PROVIDER_NODE_HPP_
44
#define LOCAL_MAP_PROVIDER_NODE_HPP_
55

6-
#include "mission_planner_messages/msg/local_map.hpp"
7-
#include "mission_planner_messages/msg/road_segments.hpp"
6+
#include "autoware_planning_msgs/msg/local_map.hpp"
7+
#include "autoware_planning_msgs/msg/road_segments.hpp"
88
#include "rclcpp/rclcpp.hpp"
99

1010
/**
@@ -29,18 +29,18 @@ class LocalMapProviderNode : public rclcpp::Node
2929
/**
3030
* @brief The callback for the RoadSegments messages.
3131
*
32-
* @param msg The mission_planner_messages::msg::RoadSegments message.
32+
* @param msg The autoware_planning_msgs::msg::RoadSegments message.
3333
*/
34-
void CallbackRoadSegmentsMessages_(const mission_planner_messages::msg::RoadSegments & msg);
34+
void CallbackRoadSegmentsMessages_(const autoware_planning_msgs::msg::RoadSegments & msg);
3535

3636
// ###########################################################################
3737
// # PRIVATE VARIABLES
3838
// ###########################################################################
3939
// Declare ROS2 publisher and subscriber
4040

41-
rclcpp::Publisher<mission_planner_messages::msg::LocalMap>::SharedPtr map_publisher_;
41+
rclcpp::Publisher<autoware_planning_msgs::msg::LocalMap>::SharedPtr map_publisher_;
4242

43-
rclcpp::Subscription<mission_planner_messages::msg::RoadSegments>::SharedPtr road_subscriber_;
43+
rclcpp::Subscription<autoware_planning_msgs::msg::RoadSegments>::SharedPtr road_subscriber_;
4444
};
4545

4646
#endif // LOCAL_MAP_PROVIDER_NODE_HPP_

planning/mapless_architecture/autoware_local_map_provider/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,8 @@
1313

1414
<depend>db_msgs</depend>
1515
<depend>lib_mission_planner</depend>
16-
<depend>mission_planner_messages</depend>
1716
<depend>rclcpp</depend>
17+
<depend>autoware_planning_msgs</depend>
1818

1919
<test_depend>ament_lint_auto</test_depend>
2020
<test_depend>autoware_lint_common</test_depend>

planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -16,19 +16,19 @@ LocalMapProviderNode::LocalMapProviderNode() : Node("local_map_provider_node")
1616
qos.best_effort();
1717

1818
// Initialize publisher for local map
19-
map_publisher_ = this->create_publisher<mission_planner_messages::msg::LocalMap>(
19+
map_publisher_ = this->create_publisher<autoware_planning_msgs::msg::LocalMap>(
2020
"local_map_provider_node/output/local_map", 1);
2121

2222
// Initialize subscriber to road segments messages
23-
road_subscriber_ = this->create_subscription<mission_planner_messages::msg::RoadSegments>(
23+
road_subscriber_ = this->create_subscription<autoware_planning_msgs::msg::RoadSegments>(
2424
"local_map_provider_node/input/road_segments", qos,
2525
std::bind(&LocalMapProviderNode::CallbackRoadSegmentsMessages_, this, _1));
2626
}
2727

2828
void LocalMapProviderNode::CallbackRoadSegmentsMessages_(
29-
const mission_planner_messages::msg::RoadSegments & msg)
29+
const autoware_planning_msgs::msg::RoadSegments & msg)
3030
{
31-
mission_planner_messages::msg::LocalMap local_map;
31+
autoware_planning_msgs::msg::LocalMap local_map;
3232

3333
// Save road segments in the local map message
3434
local_map.road_segments = msg;

planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt

-1
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,6 @@ set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format)
3939
if(BUILD_TESTING)
4040
find_package(ament_cmake_gtest REQUIRED)
4141
find_package(ament_lint_auto REQUIRED)
42-
find_package(mission_planner_messages REQUIRED)
4342

4443
ament_auto_add_gtest(${PROJECT_NAME}_tests test/gtest_main.cpp
4544
test/src/test_mission_planner_core.cpp

planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp

+30-35
Original file line numberDiff line numberDiff line change
@@ -4,15 +4,12 @@
44
#define MISSION_PLANNER_NODE_HPP_
55

66
#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"
107
#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"
1613
#include "rclcpp/rclcpp.hpp"
1714
#include "tf2_ros/buffer.h"
1815
#include "tf2_ros/transform_listener.h"
@@ -103,13 +100,13 @@ class MissionPlannerNode : public rclcpp::Node
103100
* @param converted_lanelets The lanelets from the road model
104101
(std::vector<lanelet::Lanelet>).
105102
* @param lanelet_connections The lanelet connections from the road model
106-
(std::vector<lanelet_types::LaneletConnection>).
103+
(std::vector<LaneletConnection>).
107104
* @return bool (is on goal lane or not).
108105
*/
109106
bool IsOnGoalLane_(
110107
const int ego_lanelet_index, const lanelet::BasicPoint2d & goal_point,
111108
const std::vector<lanelet::Lanelet> & converted_lanelets,
112-
const std::vector<lanelet_types::LaneletConnection> & lanelet_connections);
109+
const std::vector<LaneletConnection> & lanelet_connections);
113110

114111
/**
115112
* @brief Function which checks if the goal point has a negative x value und
@@ -119,11 +116,11 @@ class MissionPlannerNode : public rclcpp::Node
119116
* @param converted_lanelets The lanelets from the road model
120117
(std::vector<lanelet::Lanelet>).
121118
* @param lanelet_connections The lanelet connections from the road model
122-
(std::vector<lanelet_types::LaneletConnection>).
119+
(std::vector<LaneletConnection>).
123120
*/
124121
void CheckIfGoalPointShouldBeReset_(
125122
const lanelet::Lanelets & converted_lanelets,
126-
const std::vector<lanelet_types::LaneletConnection> & lanelet_connections);
123+
const std::vector<LaneletConnection> & lanelet_connections);
127124

128125
/**
129126
* @brief Function for calculating lanes
@@ -135,7 +132,7 @@ class MissionPlannerNode : public rclcpp::Node
135132
*/
136133
Lanes CalculateLanes_(
137134
const std::vector<lanelet::Lanelet> & converted_lanelets,
138-
std::vector<lanelet_types::LaneletConnection> & lanelet_connections);
135+
std::vector<LaneletConnection> & lanelet_connections);
139136

140137
/**
141138
* @brief Function for creating a marker array.
@@ -152,7 +149,7 @@ class MissionPlannerNode : public rclcpp::Node
152149
const std::vector<lanelet::ConstLineString3d> & centerline,
153150
const std::vector<lanelet::ConstLineString3d> & left,
154151
const std::vector<lanelet::ConstLineString3d> & right,
155-
const mission_planner_messages::msg::RoadSegments & msg);
152+
const autoware_planning_msgs::msg::RoadSegments & msg);
156153

157154
/**
158155
* @brief Getter for goal_point_
@@ -174,36 +171,36 @@ class MissionPlannerNode : public rclcpp::Node
174171
* @param lane The lane which is a std::vector<int> containing all the indices
175172
* of the lane.
176173
* @param converted_lanelets The lanelets (std::vector<lanelet::Lanelet>).
177-
* @return mission_planner_messages::msg::DrivingCorridor
174+
* @return autoware_planning_msgs::msg::DrivingCorridor
178175
*/
179-
mission_planner_messages::msg::DrivingCorridor CreateDrivingCorridor_(
176+
autoware_planning_msgs::msg::DrivingCorridor CreateDrivingCorridor_(
180177
const std::vector<int> & lane, const std::vector<lanelet::Lanelet> & converted_lanelets);
181178

182179
/**
183180
* @brief The callback for the Mission messages.
184181
*
185-
* @param msg The mission_planner_messages::msg::Mission message.
182+
* @param msg The autoware_planning_msgs::msg::Mission message.
186183
*/
187-
void CallbackMissionMessages_(const mission_planner_messages::msg::Mission & msg);
184+
void CallbackMissionMessages_(const autoware_planning_msgs::msg::Mission & msg);
188185

189186
/**
190187
* @brief The callback for the LocalMap messages.
191188
*
192-
* @param msg The mission_planner_messages::msg::LocalMap message.
189+
* @param msg The autoware_planning_msgs::msg::LocalMap message.
193190
*/
194-
void CallbackLocalMapMessages_(const mission_planner_messages::msg::LocalMap & msg);
191+
void CallbackLocalMapMessages_(const autoware_planning_msgs::msg::LocalMap & msg);
195192

196193
/**
197194
* @brief Convert RoadSegments into lanelets.
198195
*
199-
* @param msg The message (mission_planner_messages::msg::RoadSegments).
196+
* @param msg The message (autoware_planning_msgs::msg::RoadSegments).
200197
* @param out_lanelets The lanelets (output).
201198
* @param out_lanelet_connections The lanelet connections (output).
202199
*/
203200
void ConvertInput2LaneletFormat(
204-
const mission_planner_messages::msg::RoadSegments & msg,
201+
const autoware_planning_msgs::msg::RoadSegments & msg,
205202
std::vector<lanelet::Lanelet> & out_lanelets,
206-
std::vector<lanelet_types::LaneletConnection> & out_lanelet_connections);
203+
std::vector<LaneletConnection> & out_lanelet_connections);
207204

208205
private:
209206
// ###########################################################################
@@ -217,7 +214,7 @@ class MissionPlannerNode : public rclcpp::Node
217214
* @param converted_lanelets The lanelets (std::vector<lanelet::Lanelet>).
218215
*/
219216
void VisualizeLanes_(
220-
const mission_planner_messages::msg::RoadSegments & msg,
217+
const autoware_planning_msgs::msg::RoadSegments & msg,
221218
const std::vector<lanelet::Lanelet> & converted_lanelets);
222219

223220
/**
@@ -227,8 +224,8 @@ class MissionPlannerNode : public rclcpp::Node
227224
* @param driving_corridor The considered driving corridor for which the centerline is visualized.
228225
*/
229226
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);
232229

233230
/**
234231
* @brief Function for creating a lanelet::LineString2d.
@@ -264,8 +261,7 @@ class MissionPlannerNode : public rclcpp::Node
264261
* @return std::vector<int>
265262
*/
266263
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,
269265
const int vehicle_side);
270266

271267
/**
@@ -277,35 +273,34 @@ class MissionPlannerNode : public rclcpp::Node
277273
*
278274
*/
279275
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);
282277

283278
/**
284279
* @brief Calculate the predecessors.
285280
*
286281
* @param lanelet_connections The lanelet connections.
287282
*/
288-
void CalculatePredecessors(std::vector<lanelet_types::LaneletConnection> & lanelet_connections);
283+
void CalculatePredecessors(std::vector<LaneletConnection> & lanelet_connections);
289284

290285
// ###########################################################################
291286
// # PRIVATE VARIABLES
292287
// ###########################################################################
293288
// 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_;
295290

296-
rclcpp::Subscription<mission_planner_messages::msg::Mission>::SharedPtr missionSubscriber_;
291+
rclcpp::Subscription<autoware_planning_msgs::msg::Mission>::SharedPtr missionSubscriber_;
297292

298293
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr visualizationPublisher_;
299294

300295
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
301296
visualization_publisher_centerline_;
302297

303-
rclcpp::Publisher<mission_planner_messages::msg::VisualizationDistance>::SharedPtr
298+
rclcpp::Publisher<autoware_planning_msgs::msg::VisualizationDistance>::SharedPtr
304299
visualizationDistancePublisher_;
305300

306301
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr visualizationGoalPointPublisher_;
307302

308-
rclcpp::Publisher<mission_planner_messages::msg::MissionLanesStamped>::SharedPtr
303+
rclcpp::Publisher<autoware_planning_msgs::msg::MissionLanesStamped>::SharedPtr
309304
missionLanesStampedPublisher_;
310305

311306
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr OdometrySubscriber_;

planning/mapless_architecture/autoware_local_mission_planner/package.xml

+1-2
Original file line numberDiff line numberDiff line change
@@ -15,14 +15,13 @@
1515
<depend>db_msgs</depend>
1616
<depend>geometry_msgs</depend>
1717
<depend>lanelet2_core</depend>
18-
<depend>lanelet_helper</depend>
1918
<depend>lib_mission_planner</depend>
20-
<depend>mission_planner_messages</depend>
2119
<depend>rclcpp</depend>
2220
<depend>tf2</depend>
2321
<depend>tf2_geometry_msgs</depend>
2422
<depend>tf2_ros</depend>
2523
<depend>visualization_msgs</depend>
24+
<depend>autoware_planning_msgs</depend>
2625

2726
<test_depend>ament_lint_auto</test_depend>
2827
<test_depend>autoware_lint_common</test_depend>

0 commit comments

Comments
 (0)