Skip to content

Commit aafd37e

Browse files
StepTurtleStepTurtle
committed
feat(map): add differential lanelet loading dynamic lanelets
Signed-off-by: Barış Zeren <bzeren1819@gmail.com>
1 parent 35d2143 commit aafd37e

File tree

17 files changed

+905
-8
lines changed

17 files changed

+905
-8
lines changed

‎common/component_interface_specs/include/component_interface_specs/map.hpp

+10
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include <rclcpp/qos.hpp>
1919

20+
#include <autoware_map_msgs/msg/lanelet_map_meta_data.hpp>
2021
#include <tier4_map_msgs/msg/map_projector_info.hpp>
2122

2223
namespace map_interface
@@ -31,6 +32,15 @@ struct MapProjectorInfo
3132
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
3233
};
3334

35+
struct LaneletMapMetaData
36+
{
37+
using Message = autoware_map_msgs::msg::LaneletMapMetaData;
38+
static constexpr char name[] = "/map/lanelet_map_meta_data";
39+
static constexpr size_t depth = 1;
40+
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
41+
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
42+
};
43+
3444
} // namespace map_interface
3545

3646
#endif // COMPONENT_INTERFACE_SPECS__MAP_HPP_

‎common/component_interface_specs/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
<depend>autoware_auto_perception_msgs</depend>
1616
<depend>autoware_auto_planning_msgs</depend>
1717
<depend>autoware_auto_vehicle_msgs</depend>
18+
<depend>autoware_map_msgs</depend>
1819
<depend>autoware_planning_msgs</depend>
1920
<depend>nav_msgs</depend>
2021
<depend>rcl</depend>

‎common/component_interface_specs/test/test_map.cpp

+9
Original file line numberDiff line numberDiff line change
@@ -25,4 +25,13 @@ TEST(map, interface)
2525
EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
2626
EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
2727
}
28+
29+
{
30+
using map_interface::LaneletMapMetaData;
31+
LaneletMapMetaData lanelet_metadata;
32+
size_t depth = 1;
33+
EXPECT_EQ(lanelet_metadata.depth, depth);
34+
EXPECT_EQ(lanelet_metadata.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
35+
EXPECT_EQ(lanelet_metadata.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
36+
}
2837
}

‎launch/tier4_map_launch/launch/map.launch.xml

+28-1
Original file line numberDiff line numberDiff line change
@@ -3,19 +3,25 @@
33
<arg name="pointcloud_map_path"/>
44
<arg name="pointcloud_map_metadata_path"/>
55
<arg name="lanelet2_map_path"/>
6+
<arg name="lanelet2_map_metadata_path"/>
67
<arg name="map_projector_info_path"/>
78

89
<!-- Parameter files -->
910
<arg name="pointcloud_map_loader_param_path"/>
1011
<arg name="lanelet2_map_loader_param_path"/>
12+
<arg name="dynamic_lanelet_provider_param_path"/>
1113
<arg name="map_tf_generator_param_path"/>
1214
<arg name="map_projection_loader_param_path"/>
1315

16+
<!-- Dynamic lanelet loading -->
17+
<arg name="enable_differential_map_loading" default="false"/>
18+
1419
<!-- whether use intra-process -->
1520
<arg name="use_intra_process" default="false"/>
1621

1722
<!-- select container type -->
18-
<arg name="use_multithread" default="false"/>
23+
<arg name="use_multithread" default="false" unless="$(var enable_differential_map_loading)"/>
24+
<arg name="use_multithread" default="true" if="$(var enable_differential_map_loading)"/>
1925
<let name="container_type" value="component_container" unless="$(var use_multithread)"/>
2026
<let name="container_type" value="component_container_mt" if="$(var use_multithread)"/>
2127

@@ -38,7 +44,22 @@
3844
<composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader">
3945
<param from="$(var lanelet2_map_loader_param_path)"/>
4046
<param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
47+
<param name="lanelet2_map_metadata_path" value="$(var lanelet2_map_metadata_path)"/>
4148
<remap from="output/lanelet2_map" to="vector_map"/>
49+
<remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
50+
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
51+
</composable_node>
52+
53+
<composable_node
54+
pkg="autoware_dynamic_lanelet_provider"
55+
plugin="autoware::dynamic_lanelet_provider::DynamicLaneletProviderNode"
56+
name="dynamic_lanelet_provider"
57+
if="$(var enable_differential_map_loading)"
58+
>
59+
<param from="$(var dynamic_lanelet_provider_param_path)"/>
60+
<remap from="input/odometry" to="/localization/kinematic_state"/>
61+
<remap from="output/lanelet2_map" to="dynamic_vector_map"/>
62+
<remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
4263
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
4364
</composable_node>
4465

@@ -48,6 +69,12 @@
4869
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
4970
</composable_node>
5071

72+
<composable_node pkg="map_loader" plugin="Lanelet2MapVisualizationNode" name="dynamic_lanelet2_map_visualization" if="$(var enable_differential_map_loading)">
73+
<remap from="input/lanelet2_map" to="/map/dynamic_vector_map"/>
74+
<remap from="output/lanelet2_map_marker" to="/map/dynamic_vector_map_marker"/>
75+
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
76+
</composable_node>
77+
5178
<composable_node pkg="map_tf_generator" plugin="VectorMapTFGeneratorNode" name="vector_map_tf_generator">
5279
<param from="$(var map_tf_generator_param_path)"/>
5380
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>

‎map/autoware_dynamic_lanelet_provider/CMakeLists.txt

+19
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_dynamic_lanelet_provider)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(dynamic_lanelet_provider_node SHARED
8+
src/dynamic_lanelet_provider.cpp
9+
)
10+
11+
rclcpp_components_register_node(dynamic_lanelet_provider_node
12+
PLUGIN "autoware::dynamic_lanelet_provider::DynamicLaneletProviderNode"
13+
EXECUTABLE dynamic_lanelet_provider
14+
)
15+
16+
ament_auto_package(INSTALL_TO_SHARE
17+
launch
18+
config
19+
)

‎map/autoware_dynamic_lanelet_provider/config/dynamic_lanelet_provider.param.yaml

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
/**:
2+
ros__parameters:
3+
dynamic_map_loading_grid_size: 50.0 # [m]
4+
dynamic_map_loading_update_distance: 100.0 # [m]
5+
dynamic_map_loading_map_radius: 200.0 # [m]

‎map/autoware_dynamic_lanelet_provider/include/autoware/dynamic_lanelet_provider/dynamic_lanelet_provider.hpp

+77
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
2+
3+
#ifndef AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_
4+
#define AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_
5+
6+
#include <component_interface_specs/map.hpp>
7+
#include <component_interface_utils/rclcpp.hpp>
8+
#include <geography_utils/lanelet2_projector.hpp>
9+
#include <lanelet2_extension/projection/mgrs_projector.hpp>
10+
#include <rclcpp/rclcpp.hpp>
11+
12+
#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
13+
#include "autoware_map_msgs/msg/lanelet_map_meta_data.hpp"
14+
#include "autoware_map_msgs/srv/get_differential_lanelet2_map.hpp"
15+
#include <geometry_msgs/msg/point.hpp>
16+
#include <nav_msgs/msg/odometry.hpp>
17+
#include <tier4_map_msgs/msg/map_projector_info.hpp>
18+
19+
namespace autoware
20+
{
21+
namespace dynamic_lanelet_provider
22+
{
23+
24+
struct Lanelet2FileMetaData
25+
{
26+
int id;
27+
double min_x;
28+
double max_x;
29+
double min_y;
30+
double max_y;
31+
};
32+
33+
class DynamicLaneletProviderNode : public rclcpp::Node
34+
{
35+
public:
36+
explicit DynamicLaneletProviderNode(const rclcpp::NodeOptions & options);
37+
38+
private:
39+
void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr & msg);
40+
void mapUpdateTimerCallback();
41+
42+
void updateMap(const geometry_msgs::msg::Point & pose);
43+
bool should_update_map() const;
44+
45+
static void convertLaneletMapBinToHADMapBin(
46+
const autoware_map_msgs::msg::LaneletMapBin & lanelet_map_bin,
47+
autoware_auto_mapping_msgs::msg::HADMapBin & had_map_bin);
48+
49+
rclcpp::Publisher<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr dynamic_map_pub_;
50+
51+
rclcpp::Client<autoware_map_msgs::srv::GetDifferentialLanelet2Map>::SharedPtr map_loader_client_;
52+
53+
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
54+
55+
component_interface_utils::Subscription<map_interface::LaneletMapMetaData>::SharedPtr
56+
lanelet_map_meta_data_sub_;
57+
58+
rclcpp::TimerBase::SharedPtr map_update_timer_;
59+
60+
rclcpp::CallbackGroup::SharedPtr client_callback_group_;
61+
rclcpp::CallbackGroup::SharedPtr timer_callback_group_;
62+
63+
std::string map_frame_;
64+
65+
std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
66+
std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
67+
68+
const double dynamic_map_loading_grid_size_;
69+
const double dynamic_map_loading_update_distance_;
70+
const double dynamic_map_loading_map_radius_;
71+
72+
std::vector<Lanelet2FileMetaData> lanelet_map_meta_data_list_;
73+
};
74+
} // namespace dynamic_lanelet_provider
75+
} // namespace autoware
76+
77+
#endif // AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_

‎map/autoware_dynamic_lanelet_provider/launch/dynamic_lanelet_provider.launch.xml

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<launch>
2+
<arg name="dynamic_lanelet_provider_param_path" default="$(find-pkg-share autoware_dynamic_lanelet_provider)/config/dynamic_lanelet_provider.param.yaml"/>
3+
4+
<node pkg="autoware_dynamic_lanelet_provider" exec="dynamic_lanelet_provider_node" name="dynamic_lanelet_provider_node">
5+
<param from="$(var dynamic_lanelet_provider_param_path)"/>
6+
</node>
7+
</launch>

‎map/autoware_dynamic_lanelet_provider/package.xml

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>autoware_dynamic_lanelet_provider</name>
5+
<version>0.1.0</version>
6+
<description>Dynamic map provider package</description>
7+
<maintainer email="baris@leodrive.ai">bzeren</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
11+
<buildtool_depend>autoware_cmake</buildtool_depend>
12+
13+
<depend>autoware_auto_mapping_msgs</depend>
14+
<depend>autoware_map_msgs</depend>
15+
<depend>component_interface_specs</depend>
16+
<depend>component_interface_utils</depend>
17+
<depend>geography_utils</depend>
18+
19+
<export>
20+
<build_type>ament_cmake</build_type>
21+
</export>
22+
</package>

0 commit comments

Comments
 (0)