3D tracking

docker/perception.Dockerfile

@@ -9,10 +9,9 @@ FROM ${BASE_IMAGE} AS source
 WORKDIR ${AMENT_WS}/src
 
 # Copy in source code needed for perception build
-COPY src/perception/perception_bringup perception_bringup
-COPY src/perception/patchwork patchwork
-COPY src/perception/tracking_2d tracking_2d
+COPY src/perception perception
 COPY src/wato_test wato_test
+COPY src/infrastructure/wato_lifecycle_manager infrastructure/wato_lifecycle_manager
 
 ################################# Dependencies ################################
 # NOTE: You should be relying on ROSDEP as much as possible

src/perception/perception_bringup/CMakeLists.txt

@@ -24,4 +24,9 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_package()

src/perception/perception_bringup/launch/perception.launch.py

@@ -18,7 +18,8 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 
 
 def generate_launch_description():
@@ -106,6 +107,50 @@ def generate_launch_description():
         parameters=[LaunchConfiguration("spatial_association_param_file")],
     )
 
+    tracking_pkg = get_package_share_directory("tracking")
+    tracking_param_file = os.path.join(tracking_pkg, "config", "params.yaml")
+    tracking_param = DeclareLaunchArgument(
+        "tracking_param_file",
+        default_value=tracking_param_file,
+        description="Path to config file for tracking node",
+    )
+    tracking_detection_topic = DeclareLaunchArgument(
+        "tracking_detection_topic",
+        default_value="/carla/detections_3d",
+        description="Input detections topic subscribed to by tracking node",
+    )
+    tracking_track_topic = DeclareLaunchArgument(
+        "tracking_track_topic",
+        default_value="/tracked_boxes",
+        description="Tracked boxes output topic published to by tracking node",
+    )
+    tracking_node = ComposableNode(
+        package="tracking",
+        plugin="TrackingNode",
+        name="tracking_node",
+        parameters=[LaunchConfiguration("tracking_param_file")],
+        remappings=[
+            ("input_detections", LaunchConfiguration("tracking_detection_topic")),
+            ("output_tracks", LaunchConfiguration("tracking_track_topic")),
+        ],
+    )
+
+    container = ComposableNodeContainer(
+        name='perception_bringup',
+        namespace='perception',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            camera_object_detection_node,
+            depth_estimation_node,
+            patchwork_node,
+            spatial_association_node,
+            tracking_node,
+        ],
+        output='both'
+    )
+
+
     return LaunchDescription(
         [
             camera_detection_param,
@@ -115,9 +160,9 @@ def generate_launch_description():
             patchwork_ground_topic,
             patchwork_non_ground_topic,
             spatial_association_param,
-            camera_object_detection_node,
-            depth_estimation_node,
-            patchwork_node,
-            spatial_association_node,
+            tracking_param,
+            tracking_detection_topic,
+            tracking_track_topic,
+            container
         ]
     )

src/perception/tracking_2d/bytetrack_cpp_vendor/CMakeLists.txt → src/perception/tracking/bytetrack_cpp_vendor/CMakeLists.txt

@@ -44,7 +44,7 @@ macro(build_bytetrack_cpp)
         CMAKE_ARGS ${extra_cmake_args}
         INSTALL_COMMAND ""
         CONFIGURE_HANDLED_BY_BUILD TRUE
-        PATCH_COMMAND git apply --ignore-whitespace ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-class-id.patch
+        PATCH_COMMAND git apply --ignore-whitespace ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-3d-version-nocv2.patch
     )
     externalproject_get_property(bytetrack BINARY_DIR SOURCE_DIR)