Skip to content

Commit c4f4981

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

File tree

17 files changed

+857
-8
lines changed

17 files changed

+857
-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

+19-1
Original file line numberDiff line numberDiff line change
@@ -3,19 +3,21 @@
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

1416
<!-- whether use intra-process -->
1517
<arg name="use_intra_process" default="false"/>
1618

1719
<!-- select container type -->
18-
<arg name="use_multithread" default="false"/>
20+
<arg name="use_multithread" default="true"/>
1921
<let name="container_type" value="component_container" unless="$(var use_multithread)"/>
2022
<let name="container_type" value="component_container_mt" if="$(var use_multithread)"/>
2123

@@ -38,7 +40,17 @@
3840
<composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader">
3941
<param from="$(var lanelet2_map_loader_param_path)"/>
4042
<param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
43+
<param name="lanelet2_map_metadata_path" value="$(var lanelet2_map_metadata_path)"/>
4144
<remap from="output/lanelet2_map" to="vector_map"/>
45+
<remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
46+
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
47+
</composable_node>
48+
49+
<composable_node pkg="autoware_dynamic_lanelet_provider" plugin="autoware::dynamic_lanelet_provider::DynamicLaneletProviderNode" name="dynamic_lanelet_provider">
50+
<param from="$(var dynamic_lanelet_provider_param_path)"/>
51+
<remap from="input/odometry" to="/localization/kinematic_state"/>
52+
<remap from="output/lanelet2_map" to="dynamic_vector_map"/>
53+
<remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
4254
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
4355
</composable_node>
4456

@@ -48,6 +60,12 @@
4860
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
4961
</composable_node>
5062

63+
<composable_node pkg="map_loader" plugin="Lanelet2MapVisualizationNode" name="dynamic_lanelet2_map_visualization">
64+
<remap from="input/lanelet2_map" to="/map/dynamic_vector_map"/>
65+
<remap from="output/lanelet2_map_marker" to="/map/dynamic_vector_map_marker"/>
66+
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
67+
</composable_node>
68+
5169
<composable_node pkg="map_tf_generator" plugin="VectorMapTFGeneratorNode" name="vector_map_tf_generator">
5270
<param from="$(var map_tf_generator_param_path)"/>
5371
<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

+73
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
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+
rclcpp::Publisher<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr dynamic_map_pub_;
46+
47+
rclcpp::Client<autoware_map_msgs::srv::GetDifferentialLanelet2Map>::SharedPtr map_loader_client_;
48+
49+
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
50+
51+
component_interface_utils::Subscription<map_interface::LaneletMapMetaData>::SharedPtr
52+
lanelet_map_meta_data_sub_;
53+
54+
rclcpp::TimerBase::SharedPtr map_update_timer_;
55+
56+
rclcpp::CallbackGroup::SharedPtr client_callback_group_;
57+
rclcpp::CallbackGroup::SharedPtr timer_callback_group_;
58+
59+
std::string map_frame_;
60+
61+
std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
62+
std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
63+
64+
const double dynamic_map_loading_grid_size_;
65+
const double dynamic_map_loading_update_distance_;
66+
const double dynamic_map_loading_map_radius_;
67+
68+
std::vector<Lanelet2FileMetaData> lanelet_map_meta_data_list_;
69+
};
70+
} // namespace dynamic_lanelet_provider
71+
} // namespace autoware
72+
73+
#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)