Skip to content

Commit a30b263

Browse files
knzo25knzo25
authored
Merge branch 'main' into chore/lidar_centerpoint/use_config
2 parents 9c559df + c89f863 commit a30b263

File tree

74 files changed

+4993
-739
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

74 files changed

+4993
-739
lines changed

‎.github/CODEOWNERS

+24-21
Large diffs are not rendered by default.

‎.github/workflows/build-and-test-differential.yaml

-11
Original file line numberDiff line numberDiff line change
@@ -2,20 +2,9 @@ name: build-and-test-differential
22

33
on:
44
pull_request:
5-
types:
6-
- opened
7-
- synchronize
8-
- labeled
95

106
jobs:
11-
prevent-no-label-execution:
12-
uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
13-
with:
14-
label: tag:run-build-and-test-differential
15-
167
build-and-test-differential:
17-
needs: prevent-no-label-execution
18-
if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
198
runs-on: ubuntu-latest
209
container: ${{ matrix.container }}${{ matrix.container-suffix }}
2110
strategy:

‎common/tensorrt_common/include/tensorrt_common/logger.hpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -493,7 +493,7 @@ namespace
493493
//!
494494
inline LogStreamConsumer LOG_VERBOSE(const Logger & logger)
495495
{
496-
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE) << "[TRT] ";
496+
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE);
497497
}
498498

499499
//!
@@ -505,7 +505,7 @@ inline LogStreamConsumer LOG_VERBOSE(const Logger & logger)
505505
//!
506506
inline LogStreamConsumer LOG_INFO(const Logger & logger)
507507
{
508-
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO) << "[TRT] ";
508+
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO);
509509
}
510510

511511
//!
@@ -517,7 +517,7 @@ inline LogStreamConsumer LOG_INFO(const Logger & logger)
517517
//!
518518
inline LogStreamConsumer LOG_WARN(const Logger & logger)
519519
{
520-
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING) << "[TRT] ";
520+
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING);
521521
}
522522

523523
//!
@@ -529,7 +529,7 @@ inline LogStreamConsumer LOG_WARN(const Logger & logger)
529529
//!
530530
inline LogStreamConsumer LOG_ERROR(const Logger & logger)
531531
{
532-
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR) << "[TRT] ";
532+
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR);
533533
}
534534

535535
//!
@@ -543,7 +543,7 @@ inline LogStreamConsumer LOG_ERROR(const Logger & logger)
543543
//!
544544
inline LogStreamConsumer LOG_FATAL(const Logger & logger)
545545
{
546-
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR) << "[TRT] ";
546+
return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR);
547547
}
548548

549549
} // anonymous namespace

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

+2
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
<!-- Parameter files -->
99
<arg name="pointcloud_map_loader_param_path"/>
1010
<arg name="lanelet2_map_loader_param_path"/>
11+
<arg name="map_projection_loader_param_path"/>
1112

1213
<!-- whether use intra-process -->
1314
<arg name="use_intra_process" default="false"/>
@@ -59,6 +60,7 @@
5960
</node>
6061

6162
<include file="$(find-pkg-share map_projection_loader)/launch/map_projection_loader.launch.xml">
63+
<arg name="param_path" value="$(var map_projection_loader_param_path)"/>
6264
<arg name="map_projector_info_path" value="$(var map_projector_info_path)"/>
6365
<arg name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
6466
</include>

‎map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

+8
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
7474
// load map from file
7575
const auto map = load_map(lanelet2_filename, *msg);
7676
if (!map) {
77+
RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published.");
7778
return;
7879
}
7980

@@ -87,6 +88,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
8788
pub_map_bin_ =
8889
create_publisher<HADMapBin>("output/lanelet2_map", rclcpp::QoS{1}.transient_local());
8990
pub_map_bin_->publish(map_bin_msg);
91+
RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published.");
9092
}
9193

9294
lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
@@ -105,6 +107,12 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
105107
const lanelet::projection::LocalProjector projector;
106108
const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
107109

110+
if (!errors.empty()) {
111+
for (const auto & error : errors) {
112+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
113+
}
114+
}
115+
108116
// overwrite local_x, local_y
109117
for (lanelet::Point3d point : map->pointLayer) {
110118
if (point.hasAttribute("local_x")) {

‎map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

+2-3
Original file line numberDiff line numberDiff line change
@@ -90,9 +90,8 @@ std::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::getPCDMetadata(
9090
{
9191
std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
9292
if (pcd_paths.size() != 1) {
93-
while (!fs::exists(pcd_metadata_path)) {
94-
RCLCPP_ERROR_STREAM(get_logger(), "PCD metadata file not found: " << pcd_metadata_path);
95-
std::this_thread::sleep_for(std::chrono::seconds(1));
93+
if (!fs::exists(pcd_metadata_path)) {
94+
throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path);
9695
}
9796

9897
pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path);

‎map/map_projection_loader/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -59,4 +59,5 @@ endif()
5959

6060
ament_auto_package(INSTALL_TO_SHARE
6161
launch
62+
config
6263
)

‎map/map_projection_loader/README.md

+4-5
Original file line numberDiff line numberDiff line change
@@ -82,11 +82,10 @@ map_origin:
8282
8383
## Published Topics
8484
85-
- ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information
85+
- `~/map_projector_info` (tier4_map_msgs/MapProjectorInfo) : This topic shows the definition of map projector information
8686

8787
## Parameters
8888

89-
| Name | Type | Description |
90-
| :---------------------- | :---------- | :------------------------------------------------------------------------------- |
91-
| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default) |
92-
| lanelet2_map_path | std::string | A path to lanelet2 map (used only when `map_projector_info_path` does not exist) |
89+
Note that these parameters are assumed to be passed from launch arguments, and it is not recommended to directly write them in `map_projection_loader.param.yaml`.
90+
91+
{{ json_to_markdown("map/map_projection_loader/schema/map_projection_loader.schema.json") }}

‎map/map_projection_loader/config/map_projection_loader.param.yaml

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
/**:
2+
ros__parameters:
3+
map_projector_info_path: $(var map_projector_info_path)
4+
lanelet2_map_path: $(var lanelet2_map_path)

‎map/map_projection_loader/launch/map_projection_loader.launch.xml

+3-2
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,10 @@
11
<launch>
2+
<arg name="param_path" default="$(find-pkg-share map_projection_loader)/config/map_projection_loader.param.yaml"/>
3+
24
<arg name="map_projector_info_path" description="Path to the yaml file"/>
35
<arg name="lanelet2_map_path" description="Path to the lanelet2 map file"/>
46

57
<node pkg="map_projection_loader" exec="map_projection_loader" name="map_projection_loader" output="screen">
6-
<param name="map_projector_info_path" value="$(var map_projector_info_path)"/>
7-
<param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
8+
<param from="$(var param_path)" allow_substs="true"/>
89
</node>
910
</launch>

‎map/map_projection_loader/schema/map_projection_loader.schema.json

+38
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for map_projection_loader",
4+
"type": "object",
5+
"definitions": {
6+
"map_projection_loader": {
7+
"type": "object",
8+
"properties": {
9+
"map_projector_info_path": {
10+
"type": "string",
11+
"description": "The path where map_projector_info.yaml is located",
12+
"default": "$(var map_projector_info_path)"
13+
},
14+
"lanelet2_map_path": {
15+
"type": "string",
16+
"description": "The path where the lanelet2 map file (.osm) is located",
17+
"default": "$(var lanelet2_map_path)"
18+
}
19+
},
20+
"required": ["map_projector_info_path", "lanelet2_map_path"],
21+
"additionalProperties": false
22+
}
23+
},
24+
"properties": {
25+
"/**": {
26+
"type": "object",
27+
"properties": {
28+
"ros__parameters": {
29+
"$ref": "#/definitions/map_projection_loader"
30+
}
31+
},
32+
"required": ["ros__parameters"],
33+
"additionalProperties": false
34+
}
35+
},
36+
"required": ["/**"],
37+
"additionalProperties": false
38+
}

‎perception/image_projection_based_fusion/README.md

+9
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,15 @@ The timeout threshold should be set according to the postprocessing time.
5353
E.g, if the postprocessing time is around 50ms, the timeout threshold should be set smaller than 50ms, so that the whole processing time could be less than 100ms.
5454
current default value at autoware.universe for XX1: - timeout_ms: 50.0
5555

56+
#### The `build_only` option
57+
58+
The `pointpainting_fusion` node has `build_only` option to build the TensorRT engine file from the ONNX file.
59+
Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command:
60+
61+
```bash
62+
ros2 launch image_projection_based_fusion pointpainting_fusion.launch.xml model_name:=pointpainting model_path:=/home/autoware/autoware_data/image_projection_based_fusion model_param_path:=$(ros2 pkg prefix image_projection_based_fusion --share)/config/pointpainting.param.yaml build_only:=true
63+
```
64+
5665
#### Known Limits
5766

5867
The rclcpp::TimerBase timer could not break a for loop, therefore even if time is out when fusing a roi msg at the middle, the program will run until all msgs are fused.

‎perception/image_projection_based_fusion/config/pointpainting.param.yaml

+6
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,11 @@
11
/**:
22
ros__parameters:
3+
trt_precision: fp16
4+
encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"
5+
encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine"
6+
head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
7+
head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"
8+
39
model_params:
410
class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
511
paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"]

‎perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml

+10
Original file line numberDiff line numberDiff line change
@@ -28,3 +28,13 @@
2828
# this is selected based on semantic segmentation model accuracy,
2929
# calibration accuracy and unknown reaction distance
3030
filter_distance_threshold: 60.0
31+
32+
# debug
33+
debug_mode: false
34+
filter_scope_minx: -100
35+
filter_scope_maxx: 100
36+
filter_scope_miny: -100
37+
filter_scope_maxy: 100
38+
filter_scope_minz: -100
39+
filter_scope_maxz: 100
40+
image_buffer_size: 15

‎perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml

+6-12
Original file line numberDiff line numberDiff line change
@@ -47,15 +47,12 @@
4747
<composable_node pkg="image_projection_based_fusion" plugin="image_projection_based_fusion::PointPaintingFusionNode" name="pointpainting">
4848
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
4949
<remap from="~/output/objects" to="$(var output/objects)"/>
50-
<param name="trt_precision" value="fp16"/>
51-
<param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
52-
<param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
53-
<param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
54-
<param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
55-
<param from="$(var model_param_path)"/>
50+
<param from="$(var model_param_path)" allow_substs="true"/>
5651
<param from="$(var sync_param_path)"/>
5752
<param from="$(var class_remapper_param_path)"/>
5853
<param name="rois_number" value="$(var input/rois_number)"/>
54+
55+
<!-- This parameter shall NOT be included in param file. See the PR from the git blame for details. -->
5956
<param name="build_only" value="$(var build_only)"/>
6057

6158
<!-- rois, camera and info -->
@@ -91,15 +88,12 @@
9188
<node pkg="image_projection_based_fusion" exec="pointpainting_fusion_node" name="pointpainting" output="screen">
9289
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
9390
<remap from="~/output/objects" to="$(var output/objects)"/>
94-
<param name="trt_precision" value="fp16"/>
95-
<param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
96-
<param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
97-
<param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
98-
<param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
99-
<param from="$(var model_param_path)"/>
91+
<param from="$(var model_param_path)" allow_substs="true"/>
10092
<param from="$(var sync_param_path)"/>
10193
<param from="$(var class_remapper_param_path)"/>
10294
<param name="rois_number" value="$(var input/rois_number)"/>
95+
96+
<!-- This parameter shall NOT be included in param file. See the PR from the git blame for details. -->
10397
<param name="build_only" value="$(var build_only)"/>
10498

10599
<!-- rois, camera and info -->

‎perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml

-21
Original file line numberDiff line numberDiff line change
@@ -20,17 +20,6 @@
2020
<arg name="output/pointcloud" default="output/pointcloud"/>
2121
<arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
2222
<arg name="semantic_segmentation_based_filter_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/segmentation_pointcloud_fusion.param.yaml"/>
23-
<!-- debug -->
24-
<!-- cspell: ignore minx, maxx, miny, maxy, minz, maxz -->
25-
<arg name="debug_mode" default="false"/>
26-
27-
<arg name="filter_scope_minx" default="-100"/>
28-
<arg name="filter_scope_maxx" default="100"/>
29-
<arg name="filter_scope_miny" default="-100"/>
30-
<arg name="filter_scope_maxy" default="100"/>
31-
<arg name="filter_scope_minz" default="-100"/>
32-
<arg name="filter_scope_maxz" default="100"/>
33-
<arg name="image_buffer_size" default="15"/>
3423
<arg name="input/image0" default="/image_raw0"/>
3524
<arg name="input/image1" default="/image_raw1"/>
3625
<arg name="input/image2" default="/image_raw2"/>
@@ -72,16 +61,6 @@
7261
<param name="input/rois7" value="$(var input/mask7)"/>
7362
<param name="input/camera_info7" value="$(var input/camera_info7)"/>
7463
<param name="input/image7" value="$(var input/image7)"/>
75-
76-
<!-- debug -->
77-
<param name="debug_mode" value="$(var debug_mode)"/>
78-
<param name="filter_scope_minx" value="$(var filter_scope_minx)"/>
79-
<param name="filter_scope_maxx" value="$(var filter_scope_maxx)"/>
80-
<param name="filter_scope_miny" value="$(var filter_scope_miny)"/>
81-
<param name="filter_scope_maxy" value="$(var filter_scope_maxy)"/>
82-
<param name="filter_scope_minz" value="$(var filter_scope_minz)"/>
83-
<param name="filter_scope_maxz" value="$(var filter_scope_maxz)"/>
84-
<param name="image_buffer_size" value="$(var image_buffer_size)"/>
8564
</node>
8665
</group>
8766
</launch>

‎perception/map_based_prediction/config/map_based_prediction.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not
2020
speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value
2121
acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds
22+
use_crosswalk_signal: true
2223
# parameter for shoulder lane prediction
2324
prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
2425

‎perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

+13
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
3030
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
3131
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
32+
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
3233
#include <geometry_msgs/msg/pose.hpp>
3334
#include <geometry_msgs/msg/pose_stamped.hpp>
3435
#include <geometry_msgs/msg/twist.hpp>
@@ -42,6 +43,7 @@
4243
#include <algorithm>
4344
#include <deque>
4445
#include <memory>
46+
#include <optional>
4547
#include <string>
4648
#include <unordered_map>
4749
#include <utility>
@@ -107,6 +109,9 @@ using autoware_auto_perception_msgs::msg::TrackedObject;
107109
using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
108110
using autoware_auto_perception_msgs::msg::TrackedObjects;
109111
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
112+
using autoware_perception_msgs::msg::TrafficSignal;
113+
using autoware_perception_msgs::msg::TrafficSignalArray;
114+
using autoware_perception_msgs::msg::TrafficSignalElement;
110115
using tier4_autoware_utils::StopWatch;
111116
using tier4_debug_msgs::msg::StringStamped;
112117
using TrajectoryPoints = std::vector<TrajectoryPoint>;
@@ -122,6 +127,7 @@ class MapBasedPredictionNode : public rclcpp::Node
122127
rclcpp::Publisher<StringStamped>::SharedPtr pub_calculation_time_;
123128
rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
124129
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
130+
rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_signals_;
125131

126132
// Object History
127133
std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
@@ -131,6 +137,8 @@ class MapBasedPredictionNode : public rclcpp::Node
131137
std::shared_ptr<lanelet::routing::RoutingGraph> routing_graph_ptr_;
132138
std::shared_ptr<lanelet::traffic_rules::TrafficRules> traffic_rules_ptr_;
133139

140+
std::unordered_map<lanelet::Id, TrafficSignal> traffic_signal_id_map_;
141+
134142
// parameter update
135143
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
136144
rcl_interfaces::msg::SetParametersResult onParam(
@@ -181,11 +189,14 @@ class MapBasedPredictionNode : public rclcpp::Node
181189
double speed_limit_multiplier_;
182190
double acceleration_exponential_half_life_;
183191

192+
bool use_crosswalk_signal_;
193+
184194
// Stop watch
185195
StopWatch<std::chrono::milliseconds> stop_watch_;
186196

187197
// Member Functions
188198
void mapCallback(const HADMapBin::ConstSharedPtr msg);
199+
void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);
189200
void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects);
190201

191202
bool doesPathCrossAnyFence(const PredictedPath & predicted_path);
@@ -249,6 +260,8 @@ class MapBasedPredictionNode : public rclcpp::Node
249260
const LaneletsData & lanelets_data);
250261
bool isDuplicated(
251262
const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths);
263+
std::optional<lanelet::Id> getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet);
264+
std::optional<TrafficSignalElement> getTrafficSignalElement(const lanelet::Id & id);
252265

253266
visualization_msgs::msg::Marker getDebugMarker(
254267
const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num);

0 commit comments

Comments
 (0)