Skip to content

Commit a790489

Browse files
kosuke55wep21
andauthored
feat!: replace HADMap with Lanelet (autowarefoundation#2356)
* feat!: replace HADMap with Lanelet Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * update topic.yaml Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update perception/traffic_light_map_based_detector/README.md Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update planning/behavior_path_planner/README.md Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update planning/mission_planner/README.md Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update planning/scenario_selector/README.md Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * format readme Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
1 parent 1940b87 commit a790489

File tree

37 files changed

+135
-135
lines changed

37 files changed

+135
-135
lines changed

common/component_interface_specs/include/component_interface_specs/planning.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
2121
#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
2222
#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
23-
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
23+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
2424
#include <autoware_planning_msgs/srv/set_route.hpp>
2525

2626
namespace planning_interface
@@ -55,7 +55,7 @@ struct RouteState
5555

5656
struct Route
5757
{
58-
using Message = autoware_auto_planning_msgs::msg::HADMapRoute;
58+
using Message = autoware_planning_msgs::msg::LaneletRoute;
5959
static constexpr char name[] = "/planning/mission_planning/route";
6060
static constexpr size_t depth = 1;
6161
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;

control/lane_departure_checker/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ This package includes the following features:
4949

5050
- /localization/kinematic_state [`nav_msgs::msg::Odometry`]
5151
- /map/vector_map [`autoware_auto_mapping_msgs::msg::HADMapBin`]
52-
- /planning/mission_planning/route [`autoware_auto_planning_msgs::msg::HADMapRoute`]
52+
- /planning/mission_planning/route [`autoware_planning_msgs::msg::LaneletRoute`]
5353
- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs::msg::Trajectory`]
5454
- /control/trajectory_follower/predicted_trajectory [`autoware_auto_planning_msgs::msg::Trajectory`]
5555

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -21,10 +21,10 @@
2121
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
2222
#include <vehicle_info_util/vehicle_info_util.hpp>
2323

24-
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
2524
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
2625
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
2726
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
27+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
2828
#include <geometry_msgs/msg/pose_stamped.hpp>
2929
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
3030
#include <geometry_msgs/msg/twist_stamped.hpp>
@@ -41,10 +41,10 @@
4141

4242
namespace lane_departure_checker
4343
{
44-
using autoware_auto_planning_msgs::msg::HADMapRoute;
4544
using autoware_auto_planning_msgs::msg::PathWithLaneId;
4645
using autoware_auto_planning_msgs::msg::Trajectory;
4746
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
47+
using autoware_planning_msgs::msg::LaneletRoute;
4848
using tier4_autoware_utils::LinearRing2d;
4949
using tier4_autoware_utils::PoseDeviation;
5050
using TrajectoryPoints = std::vector<TrajectoryPoint>;
@@ -68,7 +68,7 @@ struct Input
6868
geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose{};
6969
nav_msgs::msg::Odometry::ConstSharedPtr current_odom{};
7070
lanelet::LaneletMapPtr lanelet_map{};
71-
HADMapRoute::ConstSharedPtr route{};
71+
LaneletRoute::ConstSharedPtr route{};
7272
lanelet::ConstLanelets route_lanelets{};
7373
Trajectory::ConstSharedPtr reference_trajectory{};
7474
Trajectory::ConstSharedPtr predicted_trajectory{};

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,8 @@
2626
#include <vehicle_info_util/vehicle_info_util.hpp>
2727

2828
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
29-
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
3029
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
30+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
3131
#include <geometry_msgs/msg/pose_stamped.hpp>
3232
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
3333
#include <nav_msgs/msg/odometry.hpp>
@@ -59,7 +59,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node
5959
tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
6060
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
6161
rclcpp::Subscription<HADMapBin>::SharedPtr sub_lanelet_map_bin_;
62-
rclcpp::Subscription<HADMapRoute>::SharedPtr sub_route_;
62+
rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
6363
rclcpp::Subscription<Trajectory>::SharedPtr sub_reference_trajectory_;
6464
rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_trajectory_;
6565

@@ -69,17 +69,17 @@ class LaneDepartureCheckerNode : public rclcpp::Node
6969
lanelet::LaneletMapPtr lanelet_map_;
7070
lanelet::traffic_rules::TrafficRulesPtr traffic_rules_;
7171
lanelet::routing::RoutingGraphPtr routing_graph_;
72-
HADMapRoute::ConstSharedPtr route_;
72+
LaneletRoute::ConstSharedPtr route_;
7373
geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr cov_;
74-
HADMapRoute::ConstSharedPtr last_route_;
74+
LaneletRoute::ConstSharedPtr last_route_;
7575
lanelet::ConstLanelets route_lanelets_;
7676
Trajectory::ConstSharedPtr reference_trajectory_;
7777
Trajectory::ConstSharedPtr predicted_trajectory_;
7878

7979
// Callback
8080
void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
8181
void onLaneletMapBin(const HADMapBin::ConstSharedPtr msg);
82-
void onRoute(const HADMapRoute::ConstSharedPtr msg);
82+
void onRoute(const LaneletRoute::ConstSharedPtr msg);
8383
void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg);
8484
void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg);
8585

control/lane_departure_checker/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515

1616
<depend>autoware_auto_mapping_msgs</depend>
1717
<depend>autoware_auto_planning_msgs</depend>
18+
<depend>autoware_planning_msgs</depend>
1819
<depend>boost</depend>
1920
<depend>diagnostic_updater</depend>
2021
<depend>eigen</depend>

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121
#include <tier4_autoware_utils/ros/marker_helper.hpp>
2222
#include <tier4_autoware_utils/system/stop_watch.hpp>
2323

24-
#include <autoware_auto_mapping_msgs/msg/had_map_segment.hpp>
24+
#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
2525

2626
#include <map>
2727
#include <memory>
@@ -33,7 +33,7 @@ using tier4_autoware_utils::rad2deg;
3333

3434
namespace
3535
{
36-
using autoware_auto_mapping_msgs::msg::HADMapSegment;
36+
using autoware_planning_msgs::msg::LaneletSegment;
3737

3838
std::array<geometry_msgs::msg::Point, 3> triangle2points(
3939
const geometry_msgs::msg::Polygon & triangle)
@@ -54,7 +54,7 @@ std::array<geometry_msgs::msg::Point, 3> triangle2points(
5454
lanelet::ConstLanelets getRouteLanelets(
5555
const lanelet::LaneletMapPtr & lanelet_map,
5656
const lanelet::routing::RoutingGraphPtr & routing_graph,
57-
const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr & route_ptr,
57+
const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr,
5858
const double vehicle_length)
5959
{
6060
lanelet::ConstLanelets route_lanelets;
@@ -159,7 +159,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
159159
sub_lanelet_map_bin_ = this->create_subscription<HADMapBin>(
160160
"~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(),
161161
std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1));
162-
sub_route_ = this->create_subscription<HADMapRoute>(
162+
sub_route_ = this->create_subscription<LaneletRoute>(
163163
"~/input/route", rclcpp::QoS{1}.transient_local(),
164164
std::bind(&LaneDepartureCheckerNode::onRoute, this, _1));
165165
sub_reference_trajectory_ = this->create_subscription<Trajectory>(
@@ -199,7 +199,7 @@ void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr m
199199
lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_);
200200
}
201201

202-
void LaneDepartureCheckerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) { route_ = msg; }
202+
void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { route_ = msg; }
203203

204204
void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg)
205205
{

launch/tier4_system_launch/config/component_state_monitor/topics.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -82,7 +82,7 @@
8282
args:
8383
node_name_suffix: mission_planning_route
8484
topic: /planning/mission_planning/route
85-
topic_type: autoware_auto_planning_msgs/msg/HADMapRoute
85+
topic_type: autoware_planning_msgs/msg/LaneletRoute
8686
best_effort: false
8787
transient_local: true
8888
warn_rate: 0.0

perception/crosswalk_traffic_light_estimator/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
| Name | Type | Description |
1212
| ------------------------------------ | -------------------------------------------------------- | ------------------ |
1313
| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map |
14-
| `~/input/route` | `autoware_auto_planning_msgs::msg::HADMapRoute` | route |
14+
| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route |
1515
| `~/input/classified/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | classified signals |
1616

1717
### Output

perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525
#include <autoware_auto_perception_msgs/msg/traffic_light.hpp>
2626
#include <autoware_auto_perception_msgs/msg/traffic_signal.hpp>
2727
#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
28-
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
28+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
2929
#include <tier4_debug_msgs/msg/float64_stamped.hpp>
3030

3131
#include <lanelet2_core/Attribute.h>
@@ -45,7 +45,7 @@ using autoware_auto_mapping_msgs::msg::HADMapBin;
4545
using autoware_auto_perception_msgs::msg::TrafficLight;
4646
using autoware_auto_perception_msgs::msg::TrafficSignal;
4747
using autoware_auto_perception_msgs::msg::TrafficSignalArray;
48-
using autoware_auto_planning_msgs::msg::HADMapRoute;
48+
using autoware_planning_msgs::msg::LaneletRoute;
4949
using tier4_autoware_utils::DebugPublisher;
5050
using tier4_autoware_utils::StopWatch;
5151
using tier4_debug_msgs::msg::Float64Stamped;
@@ -58,7 +58,7 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
5858

5959
private:
6060
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
61-
rclcpp::Subscription<HADMapRoute>::SharedPtr sub_route_;
61+
rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
6262
rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_light_array_;
6363
rclcpp::Publisher<TrafficSignalArray>::SharedPtr pub_traffic_light_array_;
6464

@@ -70,7 +70,7 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
7070
lanelet::ConstLanelets conflicting_crosswalks_;
7171

7272
void onMap(const HADMapBin::ConstSharedPtr msg);
73-
void onRoute(const HADMapRoute::ConstSharedPtr msg);
73+
void onRoute(const LaneletRoute::ConstSharedPtr msg);
7474
void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg);
7575

7676
void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals);

perception/crosswalk_traffic_light_estimator/src/node.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode(
8686
sub_map_ = create_subscription<HADMapBin>(
8787
"~/input/vector_map", rclcpp::QoS{1}.transient_local(),
8888
std::bind(&CrosswalkTrafficLightEstimatorNode::onMap, this, _1));
89-
sub_route_ = create_subscription<HADMapRoute>(
89+
sub_route_ = create_subscription<LaneletRoute>(
9090
"~/input/route", rclcpp::QoS{1}.transient_local(),
9191
std::bind(&CrosswalkTrafficLightEstimatorNode::onRoute, this, _1));
9292
sub_traffic_light_array_ = create_subscription<TrafficSignalArray>(
@@ -118,7 +118,7 @@ void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr m
118118
RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded");
119119
}
120120

121-
void CrosswalkTrafficLightEstimatorNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
121+
void CrosswalkTrafficLightEstimatorNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
122122
{
123123
if (lanelet_map_ptr_ == nullptr) {
124124
RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map");

perception/traffic_light_map_based_detector/README.md

+5-5
Original file line numberDiff line numberDiff line change
@@ -13,11 +13,11 @@ If the node receives no route information, it looks at a radius of 200 meters an
1313

1414
## Input topics
1515

16-
| Name | Type | Description |
17-
| -------------------- | ---------------------------------------- | ----------------------- |
18-
| `~input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map |
19-
| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter |
20-
| `~input/route` | autoware_auto_planning_msgs::HADMapRoute | optional: route |
16+
| Name | Type | Description |
17+
| -------------------- | ------------------------------------- | ----------------------- |
18+
| `~input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map |
19+
| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter |
20+
| `~input/route` | autoware_planning_msgs::LaneletRoute | optional: route |
2121

2222
## Output topics
2323

perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@
3939

4040
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
4141
#include <autoware_auto_perception_msgs/msg/traffic_light_roi_array.hpp>
42-
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
42+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
4343
#include <geometry_msgs/msg/pose_stamped.hpp>
4444
#include <sensor_msgs/msg/camera_info.hpp>
4545
#include <visualization_msgs/msg/marker_array.hpp>
@@ -84,7 +84,7 @@ class MapBasedDetector : public rclcpp::Node
8484
private:
8585
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
8686
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;
87-
rclcpp::Subscription<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr route_sub_;
87+
rclcpp::Subscription<autoware_planning_msgs::msg::LaneletRoute>::SharedPtr route_sub_;
8888

8989
rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrafficLightRoiArray>::SharedPtr roi_pub_;
9090
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr viz_pub_;
@@ -104,7 +104,7 @@ class MapBasedDetector : public rclcpp::Node
104104

105105
void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr input_msg);
106106
void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg);
107-
void routeCallback(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr input_msg);
107+
void routeCallback(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr input_msg);
108108
void getVisibleTrafficLights(
109109
const TrafficLightSet & all_traffic_lights, const geometry_msgs::msg::Pose & camera_pose,
110110
const image_geometry::PinholeCameraModel & pinhole_camera_model,

perception/traffic_light_map_based_detector/src/node.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ MapBasedDetector::MapBasedDetector(const rclcpp::NodeOptions & node_options)
102102
camera_info_sub_ = create_subscription<sensor_msgs::msg::CameraInfo>(
103103
"~/input/camera_info", rclcpp::SensorDataQoS(),
104104
std::bind(&MapBasedDetector::cameraInfoCallback, this, _1));
105-
route_sub_ = create_subscription<autoware_auto_planning_msgs::msg::HADMapRoute>(
105+
route_sub_ = create_subscription<autoware_planning_msgs::msg::LaneletRoute>(
106106
"~/input/route", rclcpp::QoS{1}.transient_local(),
107107
std::bind(&MapBasedDetector::routeCallback, this, _1));
108108

@@ -290,7 +290,7 @@ void MapBasedDetector::mapCallback(
290290
}
291291

292292
void MapBasedDetector::routeCallback(
293-
const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr input_msg)
293+
const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr input_msg)
294294
{
295295
if (lanelet_map_ptr_ == nullptr) {
296296
RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map");

planning/behavior_path_planner/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ The following modules are currently supported:
4343

4444
### input
4545

46-
- /planning/mission_planning/route [`autoware_auto_planning_msgs/HADMapRoute`] : Current route from start to goal.
46+
- /planning/mission_planning/route [`autoware_planning_msgs/LaneletRoute`] : Current route from start to goal.
4747
- /map/vector_map [autoware_auto_mapping_msgs/HADMapBin] : Map information.
4848
- /perception/object_recognition/objects [`autoware_auto_perception_msgs/PredictedObjects`] : dynamic objects from perception module.
4949
- /perception/occupancy_grid_map/map [nav_msgs/msg/OccupancyGrid] : occupancy grid map from perception module. This is used for only Pull Over module

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -31,11 +31,11 @@
3131
#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp"
3232
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
3333
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
34-
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
3534
#include <autoware_auto_planning_msgs/msg/path.hpp>
3635
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
3736
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
3837
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
38+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
3939
#include <nav_msgs/msg/occupancy_grid.hpp>
4040
#include <nav_msgs/msg/odometry.hpp>
4141
#include <tier4_planning_msgs/msg/approval.hpp>
@@ -66,11 +66,11 @@ namespace behavior_path_planner
6666
using ApprovalMsg = tier4_planning_msgs::msg::Approval;
6767
using autoware_auto_mapping_msgs::msg::HADMapBin;
6868
using autoware_auto_perception_msgs::msg::PredictedObjects;
69-
using autoware_auto_planning_msgs::msg::HADMapRoute;
7069
using autoware_auto_planning_msgs::msg::Path;
7170
using autoware_auto_planning_msgs::msg::PathWithLaneId;
7271
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
7372
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
73+
using autoware_planning_msgs::msg::LaneletRoute;
7474
using geometry_msgs::msg::TwistStamped;
7575
using nav_msgs::msg::OccupancyGrid;
7676
using nav_msgs::msg::Odometry;
@@ -88,7 +88,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
8888
explicit BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options);
8989

9090
private:
91-
rclcpp::Subscription<HADMapRoute>::SharedPtr route_subscriber_;
91+
rclcpp::Subscription<LaneletRoute>::SharedPtr route_subscriber_;
9292
rclcpp::Subscription<HADMapBin>::SharedPtr vector_map_subscriber_;
9393
rclcpp::Subscription<Odometry>::SharedPtr velocity_subscriber_;
9494
rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acceleration_subscriber_;
@@ -136,7 +136,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
136136
void onExternalApproval(const ApprovalMsg::ConstSharedPtr msg);
137137
void onForceApproval(const PathChangeModule::ConstSharedPtr msg);
138138
void onMap(const HADMapBin::ConstSharedPtr map_msg);
139-
void onRoute(const HADMapRoute::ConstSharedPtr route_msg);
139+
void onRoute(const LaneletRoute::ConstSharedPtr route_msg);
140140
SetParametersResult onSetParam(const std::vector<rclcpp::Parameter> & parameters);
141141

142142
/**

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
102102
vector_map_subscriber_ = create_subscription<HADMapBin>(
103103
"~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1),
104104
createSubscriptionOptions(this));
105-
route_subscriber_ = create_subscription<HADMapRoute>(
105+
route_subscriber_ = create_subscription<LaneletRoute>(
106106
"~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1),
107107
createSubscriptionOptions(this));
108108

@@ -835,7 +835,7 @@ void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg)
835835
std::lock_guard<std::mutex> lock(mutex_pd_);
836836
planner_data_->route_handler->setMap(*msg);
837837
}
838-
void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
838+
void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
839839
{
840840
std::lock_guard<std::mutex> lock(mutex_pd_);
841841
const bool is_first_time = !(planner_data_->route_handler->isHandlerReady());

0 commit comments

Comments
 (0)