Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(map): add differential lanelet loading #6241

Open
wants to merge 18 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <rclcpp/qos.hpp>

#include <autoware_map_msgs/msg/lanelet_map_meta_data.hpp>
#include <tier4_map_msgs/msg/map_projector_info.hpp>

namespace map_interface
Expand All @@ -31,6 +32,15 @@ struct MapProjectorInfo
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
};

struct LaneletMapMetaData
{
using Message = autoware_map_msgs::msg::LaneletMapMetaData;
static constexpr char name[] = "/map/lanelet_map_meta_data";
static constexpr size_t depth = 1;
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
};

} // namespace map_interface

#endif // COMPONENT_INTERFACE_SPECS__MAP_HPP_
1 change: 1 addition & 0 deletions common/component_interface_specs/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
Expand Down
9 changes: 9 additions & 0 deletions common/component_interface_specs/test/test_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,4 +25,13 @@ TEST(map, interface)
EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
}

{
using map_interface::LaneletMapMetaData;
LaneletMapMetaData lanelet_metadata;
size_t depth = 1;
EXPECT_EQ(lanelet_metadata.depth, depth);
EXPECT_EQ(lanelet_metadata.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
EXPECT_EQ(lanelet_metadata.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
}
}
30 changes: 29 additions & 1 deletion launch/tier4_map_launch/launch/map.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -3,19 +3,25 @@
<arg name="pointcloud_map_path"/>
<arg name="pointcloud_map_metadata_path"/>
<arg name="lanelet2_map_path"/>
<arg name="lanelet2_map_metadata_path"/>
<arg name="map_projector_info_path"/>

<!-- Parameter files -->
<arg name="pointcloud_map_loader_param_path"/>
<arg name="lanelet2_map_loader_param_path"/>
<arg name="dynamic_lanelet_provider_param_path"/>
<arg name="map_tf_generator_param_path"/>
<arg name="map_projection_loader_param_path"/>

<!-- Dynamic lanelet loading -->
<arg name="enable_differential_map_loading"/>

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

<!-- select container type -->
<arg name="use_multithread" default="false"/>
<arg name="use_multithread" default="false" unless="$(var enable_differential_map_loading)"/>
<arg name="use_multithread" default="true" if="$(var enable_differential_map_loading)"/>
<let name="container_type" value="component_container" unless="$(var use_multithread)"/>
<let name="container_type" value="component_container_mt" if="$(var use_multithread)"/>

Expand All @@ -38,7 +44,23 @@
<composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader">
<param from="$(var lanelet2_map_loader_param_path)"/>
<param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
<param name="lanelet2_map_metadata_path" value="$(var lanelet2_map_metadata_path)"/>
<param name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
<remap from="output/lanelet2_map" to="vector_map"/>
<remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
</composable_node>

<composable_node
pkg="autoware_dynamic_lanelet_provider"
plugin="autoware::dynamic_lanelet_provider::DynamicLaneletProviderNode"
name="dynamic_lanelet_provider"
if="$(var enable_differential_map_loading)"
>
<param from="$(var dynamic_lanelet_provider_param_path)"/>
<remap from="input/odometry" to="/localization/kinematic_state"/>
<remap from="output/lanelet2_map" to="dynamic_vector_map"/>
<remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
</composable_node>

Expand All @@ -48,6 +70,12 @@
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
</composable_node>

<composable_node pkg="map_loader" plugin="Lanelet2MapVisualizationNode" name="dynamic_lanelet2_map_visualization" if="$(var enable_differential_map_loading)">
<remap from="input/lanelet2_map" to="/map/dynamic_vector_map"/>
<remap from="output/lanelet2_map_marker" to="/map/dynamic_vector_map_marker"/>
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
</composable_node>

<composable_node pkg="autoware_map_tf_generator" plugin="autoware::map_tf_generator::VectorMapTFGeneratorNode" name="vector_map_tf_generator">
<param from="$(var map_tf_generator_param_path)"/>
<extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
Expand Down
19 changes: 19 additions & 0 deletions map/autoware_dynamic_lanelet_provider/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
cmake_minimum_required(VERSION 3.14)
project(autoware_dynamic_lanelet_provider)

find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_add_library(dynamic_lanelet_provider_node SHARED
src/dynamic_lanelet_provider.cpp
)

rclcpp_components_register_node(dynamic_lanelet_provider_node
PLUGIN "autoware::dynamic_lanelet_provider::DynamicLaneletProviderNode"
EXECUTABLE dynamic_lanelet_provider
)

ament_auto_package(INSTALL_TO_SHARE
launch
config
)
37 changes: 37 additions & 0 deletions map/autoware_dynamic_lanelet_provider/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
# autoware_dynamic_lanelet_provider

## Purpose

This package aims to provide a dynamic Lanelet2 map to the other Autoware nodes.
The dynamic Lanelet2 map is a Lanelet2 map that is updated in real-time based
on the current odometry position of the vehicle.

To use this package, you need to provide a divided Lanelet2 maps and a metadata file.
You should check
the [lanelet2_map_loader documentation](https://autowarefoundation.github.io/autoware.universe/main/map/map_loader/#lanelet2_map_loader)
for more information about the divided lanelet map and the metadata file.

## Inputs / Outputs

### Input

| Name | Type | Description |
| ---------------------------- | ------------------------------------------ | ------------------------------- |
| `~/input/odometry` | `nav_msgs::msg::Odometry` | ego vehicle odometry |
| `/map/lanelet_map_meta_data` | autoware_map_msgs::msg::LaneletMapMetaData | metadata info for lanelet2 maps |

### Output

| Name | Type | Description |
| --------------------- | --------------------------------------- | -------------------------- |
| `output/lanelet2_map` | `autoware_map_msgs::msg::LaneletMapBin` | dynamic Lanelet2 map topic |

### Client

| Name | Type | Description |
| -------------------------------------- | ------------------------------------------------ | ------------------------------------------ |
| `service/get_differential_lanelet_map` | `autoware_map_msgs::srv::GetSelectedLanelet2Map` | service to load differential Lanelet2 maps |

## Parameters

{{ json_to_markdown("map/autoware_dynamic_lanelet_provider/schema/dynamic_lanelet_provider.schema.json") }}
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
/**:
ros__parameters:
dynamic_map_loading_update_distance: 100.0 # [m]
dynamic_map_loading_map_radius: 200.0 # [m]
YamatoAndo marked this conversation as resolved.
Show resolved Hide resolved
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
// Copyright 2024 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_
#define AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_

#include <component_interface_specs/map.hpp>
#include <component_interface_utils/rclcpp.hpp>
#include <rclcpp/rclcpp.hpp>

#include "autoware_map_msgs/msg/lanelet_map_meta_data.hpp"
#include "autoware_map_msgs/srv/get_selected_lanelet2_map.hpp"
#include <geometry_msgs/msg/point.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_map_msgs/msg/map_projector_info.hpp>

#include <optional>
#include <string>
#include <vector>

namespace autoware
{
namespace dynamic_lanelet_provider
{

struct Lanelet2FileMetaData
{
std::string id;
double min_x;
double max_x;
double min_y;
double max_y;
};

class DynamicLaneletProviderNode : public rclcpp::Node
{
public:
explicit DynamicLaneletProviderNode(const rclcpp::NodeOptions & options);

private:
void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr & msg);
void mapUpdateTimerCallback();

void updateMap(const geometry_msgs::msg::Point & pose);
bool should_update_map() const;

rclcpp::Publisher<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr dynamic_map_pub_;

rclcpp::Client<autoware_map_msgs::srv::GetSelectedLanelet2Map>::SharedPtr map_loader_client_;

rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;

component_interface_utils::Subscription<map_interface::LaneletMapMetaData>::SharedPtr
lanelet_map_meta_data_sub_;

rclcpp::TimerBase::SharedPtr map_update_timer_;

rclcpp::CallbackGroup::SharedPtr client_callback_group_;
rclcpp::CallbackGroup::SharedPtr timer_callback_group_;

std::string map_frame_;

std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;

const double dynamic_map_loading_update_distance_;
const double dynamic_map_loading_map_radius_;

std::vector<Lanelet2FileMetaData> lanelet_map_meta_data_list_;
};
} // namespace dynamic_lanelet_provider
} // namespace autoware

#endif // AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
<launch>
<arg name="dynamic_lanelet_provider_param_path" default="$(find-pkg-share autoware_dynamic_lanelet_provider)/config/dynamic_lanelet_provider.param.yaml"/>

<node pkg="autoware_dynamic_lanelet_provider" exec="dynamic_lanelet_provider_node" name="dynamic_lanelet_provider_node">
<param from="$(var dynamic_lanelet_provider_param_path)"/>
</node>
</launch>
28 changes: 28 additions & 0 deletions map/autoware_dynamic_lanelet_provider/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>autoware_dynamic_lanelet_provider</name>
<version>0.1.0</version>
<description>Dynamic map provider package</description>
<maintainer email="[email protected]">Yamato Ando</maintainer>
<maintainer email="[email protected]">Ryu Yamamoto</maintainer>
<maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
<maintainer email="[email protected]">NGUYEN Viet Anh</maintainer>
<maintainer email="[email protected]">Taiki Yamada</maintainer>
<maintainer email="[email protected]">Shintaro Sakoda</maintainer>
<maintainer email="[email protected]">Barış Zeren</maintainer>
<license>Apache License 2.0</license>
<author email="[email protected]">Barış Zeren</author>

StepTurtle marked this conversation as resolved.
Show resolved Hide resolved
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_geography_utils</depend>
<depend>autoware_map_msgs</depend>
<depend>component_interface_specs</depend>
<depend>component_interface_utils</depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
{
"$schema": "http://json-schema.org/draft-07/schema#",
"title": "Parameters for autoware_dynamic_lanelet_provider",
"type": "object",
"definitions": {
"autoware_dynamic_lanelet_provider": {
"type": "object",
"properties": {
"dynamic_map_loading_update_distance": {
"type": "number",
"description": "The distance to update the map [m]",
"default": "100.0"
},
"dynamic_map_loading_map_radius": {
"type": "number",
"description": "The radius for the map to be loaded [m]",
"default": "200.0"
}
},
"required": ["dynamic_map_loading_update_distance", "dynamic_map_loading_map_radius"],
"additionalProperties": false
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/autoware_dynamic_lanelet_provider"
}
},
"required": ["ros__parameters"],
"additionalProperties": false
}
},
"required": ["/**"],
"additionalProperties": false
}
Loading
Loading