add feature: allocate to used id after full; add feat: input synced obs

输入可以支持同步过的，但还没测试

CMakeLists.txt

@@ -473,6 +473,35 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         message_filters
     )
 
+    add_executable(cloud_reprojection_synced_ros2_node
+        src/cloud_reprojection_synced_ros.cpp
+        src/cloud_reprojection_processing.cpp
+    )
+    target_compile_definitions(cloud_reprojection_synced_ros2_node PRIVATE ROS2)
+    target_link_options(cloud_reprojection_synced_ros2_node PRIVATE "-Wl,--no-as-needed")
+    target_link_libraries(cloud_reprojection_synced_ros2_node
+        cloud_reprojector_ros2
+        ${odin_ros_driver_interfaces_target}
+        ${odin_ros_driver_fastrtps_cpp_target}
+        ${OpenCV_LIBS}
+        ${PCL_LIBRARIES}
+        yaml-cpp
+    )
+    if(ODIN_TARGET_OBSERVATION_AVAILABLE)
+        target_link_libraries(cloud_reprojection_synced_ros2_node target_observation_processing)
+        target_compile_definitions(cloud_reprojection_synced_ros2_node PRIVATE ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION)
+    endif()
+    ament_target_dependencies(cloud_reprojection_synced_ros2_node
+        rclcpp
+        sensor_msgs
+        nav_msgs
+        geometry_msgs
+        std_msgs
+        visualization_msgs
+        cv_bridge
+        pcl_conversions
+    )
+
     add_executable(image_overlay_node src/image_overlay_node.cpp)
     target_compile_definitions(image_overlay_node PRIVATE ROS2)
     target_link_libraries(image_overlay_node
@@ -490,6 +519,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         host_sdk_sample
         pcd2depth_ros2_node
         cloud_reprojection_ros2_node
+        cloud_reprojection_synced_ros2_node
         image_overlay_node
     )
         set_runtime_search_path(${ros2_runtime_target} "\$ORIGIN/..")
@@ -514,6 +544,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         host_sdk_sample 
         pcd2depth_ros2_node
         cloud_reprojection_ros2_node
+        cloud_reprojection_synced_ros2_node
         image_overlay_node
         EXPORT export_${PROJECT_NAME}
         ARCHIVE DESTINATION lib

config/odin_ros2.rviz

@@ -41,7 +41,7 @@ Visualization Manager:
       Cell Size: 1
       Class: rviz_default_plugins/Grid
       Color: 160; 160; 164
-      Enabled: false
+      Enabled: true
       Line Style:
         Line Width: 0.029999999329447746
         Value: Lines
@@ -54,7 +54,7 @@ Visualization Manager:
       Plane: XY
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
-      Value: false
+      Value: true
     - Class: rviz_default_plugins/Image
       Enabled: false
       Max Value: 1
@@ -594,7 +594,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/ThirdPersonFollower
-      Distance: 12.054017066955566
+      Distance: 103.81838989257812
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -609,10 +609,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.0747967958450317
+      Pitch: 1.5697963237762451
       Target Frame: odin1_base_link
       Value: ThirdPersonFollower (rviz_default_plugins)
-      Yaw: 3.0954017639160156
+      Yaw: 3.4504027366638184
     Saved: ~
 Window Geometry:
   Displays:

launch_ROS2/odin1_reprojection_synced_ros2.launch.py

@@ -0,0 +1,50 @@
+import os
+import yaml
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def create_nodes(context):
+    package_dir = get_package_share_directory('odin_ros_driver')
+    config_file = LaunchConfiguration('config_file').perform(context)
+
+    with open(config_file, 'r', encoding='utf-8') as stream:
+        reprojection_params = yaml.safe_load(stream) or {}
+
+    reprojection_params.setdefault('synced_cloud_topic', '/odin1/sync/cloud_in_cam')
+    reprojection_params.setdefault('synced_odometry_topic', '/odin1/sync/odometry')
+    reprojection_params.setdefault('synced_wiwc_topic', '/odin1/sync/wiwc')
+    reprojection_params.setdefault('synced_image_topic', '/odin1/sync/image')
+    reprojection_params.setdefault('synced_image_compressed', 1)
+    reprojection_params.setdefault('register_keys.sync_topic_prefix', '/odin1/sync')
+    reprojection_params['calib_file_path'] = os.path.join(package_dir, 'config', 'calib.yaml')
+
+    synced_target_observation_node = Node(
+        package='odin_ros_driver',
+        executable='cloud_reprojection_synced_ros2_node',
+        name='cloud_reprojection_synced_ros2_node',
+        output='screen',
+        parameters=[reprojection_params]
+    )
+
+    return [synced_target_observation_node]
+
+
+def generate_launch_description():
+    package_dir = get_package_share_directory('odin_ros_driver')
+
+    config_file_arg = DeclareLaunchArgument(
+        'config_file',
+        default_value=os.path.join(package_dir, 'config', 'control_command.yaml'),
+        description='Path to the control config YAML file'
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1'))
+    ld.add_action(config_file_arg)
+    ld.add_action(OpaqueFunction(function=lambda context: create_nodes(context)))
+    return ld

src/cloud_reprojection_ros.cpp

@@ -1033,8 +1033,10 @@ void CloudReprojectionRosNode::processSyncedData(
         fill_observation_msg(observation_msg, target_observation);
         target_observation_pub_->publish(observation_msg);
 
-        // Multi-target topic — every active track with a fresh 3D estimate,
-        // including the follow target (marked via is_primary_target).
+        // Multi-target topic — every retained stable track every frame.
+        // valid=true only when this frame has a fresh 3D estimate; otherwise
+        // the row is still published with valid=false so downstream dataset
+        // builders no longer need to synthesize placeholder observations.
         const std::vector<odin_ros_driver::TargetObservation> active_tracks =
             target_observation_processor_->snapshot_active_tracks();
         odin_ros_driver::msg::TargetObservationArray track_array_msg;

src/target_observation_processing.cpp

@@ -942,8 +942,31 @@ std::vector<int> TargetObservationProcessor::bind_and_update_tracks(
             continue;
         }
 
-        // Promoted: allocate stable_id and drop the pending record.
-        const int new_sid = next_stable_id_++;
+        // Promoted: allocate a stable_id and drop the pending record.
+        //
+        // Default behavior stays monotonic at startup: 0,1,2,... until the
+        // configured concurrent-track budget is reached. After that, if some
+        // old entries have already aged out and left holes in [0, cap), reuse
+        // the smallest free slot instead of letting sid numbers grow forever.
+        //
+        // This keeps the palette / RViz display bounded and makes logs easier
+        // to read on long runs, while preserving the simple "low sid first"
+        // behavior in the common small-track case.
+        const int sid_cap = std::max(1, config_.max_concurrent_tracks);
+        int new_sid = -1;
+        if (next_stable_id_ < sid_cap) {
+            new_sid = next_stable_id_++;
+        } else {
+            for (int candidate = 0; candidate < sid_cap; ++candidate) {
+                if (!tracks_.count(candidate)) {
+                    new_sid = candidate;
+                    break;
+                }
+            }
+            if (new_sid < 0) {
+                new_sid = next_stable_id_++;
+            }
+        }
         TrackEntry entry;
         entry.stable_id = new_sid;
         entry.gallery = TrackGallery(