diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 131388b3764b5..69d42b8509bb5 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -52,6 +52,8 @@
+
+
@@ -153,6 +155,8 @@
+
+
diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
index ea50101719d65..4dae46cbacb0a 100644
--- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
@@ -2,84 +2,138 @@
+
-
+
-
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
-
+
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
+
+
+
+
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
+
+
+
+
diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
index 964008dafa5cd..8b3d15f2cff95 100644
--- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
@@ -40,6 +40,10 @@ def add_launch_arg(name: str, default_value=None, description=None):
add_launch_arg("enable_image_decompressor", "True")
add_launch_arg("enable_fine_detection", "True")
add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw")
+ add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois")
+ add_launch_arg(
+ "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals"
+ )
# traffic_light_fine_detector
add_launch_arg(
@@ -89,6 +93,7 @@ def create_parameter_dict(*args):
package="traffic_light_classifier",
plugin="traffic_light::TrafficLightClassifierNodelet",
name="traffic_light_classifier",
+ namespace="classification",
parameters=[
create_parameter_dict(
"approximate_sync",
@@ -102,7 +107,7 @@ def create_parameter_dict(*args):
],
remappings=[
("~/input/image", LaunchConfiguration("input/image")),
- ("~/input/rois", "rois"),
+ ("~/input/rois", LaunchConfiguration("output/rois")),
("~/output/traffic_signals", "classified/traffic_signals"),
],
extra_arguments=[
@@ -116,9 +121,9 @@ def create_parameter_dict(*args):
parameters=[create_parameter_dict("enable_fine_detection")],
remappings=[
("~/input/image", LaunchConfiguration("input/image")),
- ("~/input/rois", "rois"),
- ("~/input/rough/rois", "rough/rois"),
- ("~/input/traffic_signals", "traffic_signals"),
+ ("~/input/rois", LaunchConfiguration("output/rois")),
+ ("~/input/rough/rois", "detection/rough/rois"),
+ ("~/input/traffic_signals", LaunchConfiguration("output/traffic_signals")),
("~/output/image", "debug/rois"),
("~/output/image/compressed", "debug/rois/compressed"),
("~/output/image/compressedDepth", "debug/rois/compressedDepth"),
@@ -138,11 +143,12 @@ def create_parameter_dict(*args):
package="crosswalk_traffic_light_estimator",
plugin="traffic_light::CrosswalkTrafficLightEstimatorNode",
name="crosswalk_traffic_light_estimator",
+ namespace="classification",
remappings=[
("~/input/vector_map", "/map/vector_map"),
("~/input/route", "/planning/mission_planning/route"),
("~/input/classified/traffic_signals", "classified/traffic_signals"),
- ("~/output/traffic_signals", "traffic_signals"),
+ ("~/output/traffic_signals", "estimated/traffic_signals"),
],
extra_arguments=[{"use_intra_process_comms": False}],
),
@@ -157,11 +163,10 @@ def create_parameter_dict(*args):
package="topic_tools",
plugin="topic_tools::RelayNode",
name="classified_signals_relay",
- namespace="",
+ namespace="classification",
parameters=[
{"input_topic": "classified/traffic_signals"},
- {"output_topic": "traffic_signals"},
- {"type": "autoware_auto_perception_msgs/msg/TrafficSignalArray"},
+ {"output_topic": "estimated/traffic_signals"},
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -209,12 +214,13 @@ def create_parameter_dict(*args):
package="traffic_light_fine_detector",
plugin="traffic_light::TrafficLightFineDetectorNodelet",
name="traffic_light_fine_detector",
+ namespace="detection",
parameters=[fine_detector_param],
remappings=[
("~/input/image", LaunchConfiguration("input/image")),
("~/input/rois", "rough/rois"),
("~/expect/rois", "expect/rois"),
- ("~/output/rois", "rois"),
+ ("~/output/rois", LaunchConfiguration("output/rois")),
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml
index 1140d643a7083..40fc632a33f45 100644
--- a/launch/tier4_perception_launch/package.xml
+++ b/launch/tier4_perception_launch/package.xml
@@ -32,9 +32,12 @@
probabilistic_occupancy_grid_map
shape_estimation
tensorrt_yolo
+ topic_tools
traffic_light_classifier
+ traffic_light_fine_detector
traffic_light_map_based_detector
- traffic_light_ssd_fine_detector
+ traffic_light_multi_camera_fusion
+ traffic_light_occlusion_predictor
traffic_light_visualization
ament_lint_auto
diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp
index d7e82af4af079..892b6be62ef9d 100644
--- a/perception/traffic_light_multi_camera_fusion/src/node.cpp
+++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp
@@ -149,8 +149,8 @@ MultiCameraFusion::MultiCameraFusion(const rclcpp::NodeOptions & node_options)
is_approximate_sync_ = this->declare_parameter("approximate_sync", false);
message_lifespan_ = this->declare_parameter("message_lifespan", 0.09);
for (const std::string & camera_ns : camera_namespaces) {
- std::string signal_topic = camera_ns + "/traffic_signals";
- std::string roi_topic = camera_ns + "/rois";
+ std::string signal_topic = camera_ns + "/classification/traffic_signals";
+ std::string roi_topic = camera_ns + "/detection/rois";
std::string cam_info_topic = camera_ns + "/camera_info";
roi_subs_.emplace_back(
new mf::Subscriber(this, roi_topic, rclcpp::QoS{1}.get_rmw_qos_profile()));