3d extrinsics

.dockerignore

@@ -0,0 +1,18 @@
+# runtime outputs (your issue)
+src/swarm_lio/Log/
+src/swarm_lio/PCD/
+bags/
+
+# common ROS/colcon build outputs
+**/build/
+**/install/
+**/log/
+
+# random artifacts
+**/*.log
+**/*.pcd
+**/*.bag
+**/*.db
+**/*.tmp
+**/.DS_Store
+**/.git/

.gitignore

@@ -48,3 +48,6 @@ docker-compose.override.yml
 # MATLAB
 *.asv
 *.m~
+
+# Point clouds
+*.pcd

docker/Dockerfile.ros2

@@ -69,15 +69,25 @@ ENV DEBIAN_FRONTEND=noninteractive
 ENV ROS_WS=/opt/swarm_lio_ws
 
 # Create colcon workspace
-RUN mkdir -p $ROS_WS/src
-WORKDIR $ROS_WS/src
-
-# Copy Swarm-LIO2 source code
-COPY src/ $ROS_WS/src/
-
-# Build the workspace
+RUN mkdir -p $ROS_WS
 WORKDIR $ROS_WS
-RUN /bin/bash -c "source /opt/ros/humble/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release"
+
+# ---- copy+build swarm_msgs ----
+COPY src/swarm_msgs/ $ROS_WS/src/swarm_msgs/
+RUN /bin/bash -lc "source /opt/ros/humble/setup.bash && \
+  colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select swarm_msgs"
+
+# ---- copy+build udp_bridge ----
+COPY src/udp_bridge/ $ROS_WS/src/udp_bridge/
+RUN /bin/bash -lc "source /opt/ros/humble/setup.bash && \
+  source $ROS_WS/install/setup.bash && \
+  colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select udp_bridge"
+
+# ---- copy+build swarm_lio ----
+COPY src/swarm_lio/ $ROS_WS/src/swarm_lio/
+RUN /bin/bash -lc "source /opt/ros/humble/setup.bash && \
+  source $ROS_WS/install/setup.bash && \
+  colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select swarm_lio"
 
 # Source ROS and workspace by default
 RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc

src/swarm_lio/CMakeLists.txt

@@ -114,15 +114,25 @@ rosidl_target_interfaces(swarm_lio
   "rosidl_typesupport_cpp"
 )
 
-add_executable(global_map_node src/global_map_node.cpp)
-ament_target_dependencies(global_map_node
+add_executable(robot_extrinsic_publisher src/robot_extrinsic_publisher.cpp)
+ament_target_dependencies(robot_extrinsic_publisher
   rclcpp
-  sensor_msgs
+  geometry_msgs
+  swarm_msgs
+  tf2
+  tf2_ros
+)
+install(TARGETS robot_extrinsic_publisher
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+add_executable(gt_extrinsic_initializer src/gt_extrinsic_initializer.cpp)
+ament_target_dependencies(gt_extrinsic_initializer
+  rclcpp
+  nav_msgs
   swarm_msgs
-  pcl_conversions
-  PCL
 )
-install(TARGETS global_map_node
+install(TARGETS gt_extrinsic_initializer
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/swarm_lio/config/simulation.yaml

@@ -1,4 +1,6 @@
 ros__parameters:
+  bypass_initialization: true
+
   common:
       lid_topic: None  # to be replaced in launch file with drone_id
       imu_topic: None  # to be replaced in launch file with drone_id 
@@ -8,20 +10,22 @@ ros__parameters:
   multiuav:
       cluster_extraction_in_predict_region: true
       inten_threshold: 1000
-      ground_height_threshold: -0.6  # remove all points below this height (meter)
-      min_high_inten_cluster_size: 4
+      ground_height_threshold: -5.0  # remove all points below this height (meter). Use -5 for 3D flight
+      mo_force_extrinsic_se2: false  # true: 2D SE2 mode; false: 3D mode
+      mo_ignore_z_meas: false        # true: ignore z in mutual observation (2D); false: use full 3D
+      min_high_inten_cluster_size: 2
       min_cluster_size: 3 # Unused
       predict_region_radius: 0.7
       temp_predict_region_radius: 0.7
-      traj_matching_start_thresh: 15.0
+      traj_matching_start_thresh: 8.0
       ave_match_error_thresh: 0.2
       pos_num_in_traj: 200
       reset_tracker_thresh: 5.0
       valid_cluster_size_thresh: 0.15
       valid_cluster_dist_thresh: 0.35
       mutual_observe_noise: 0.01
       frame_num_in_sliding_window: 1
-      actual_uav_num: 4
+      actual_uav_num: None  # set dynamicly in launch file
       degeneration_thresh: 3.0
 
   imu_propagate:
@@ -53,7 +57,7 @@ ros__parameters:
       b_acc_cov: 0.001
       b_gyr_cov: 0.001
       det_range: 20.0
-      LI_extrinsic_T: [ 0.0, 0.0, -0.08 ]
+      LI_extrinsic_T: [ 0.0, 0.0, -0.095 ]
       LI_extrinsic_R: [ 1.0, 0.0, 0.0,
                         0.0, 1.0, 0.0,
                         0.0, 0.0, 1.0]
@@ -65,9 +69,10 @@ ros__parameters:
 
   publish:
       path_en:  true
-      scan_publish_en:  true       # false: close all the point cloud output
+      scan_publish_en:  true        # false: close all the point cloud output
       dense_publish_en: false       # false: low down the points number in a global-frame point clouds scan.
       scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU body frame
+      local_map_pub_en: false       # true: publish the local map point cloud
 
   pcd_save:
       pcd_save_en: true

src/swarm_lio/config_description.md

@@ -6,7 +6,7 @@ References are workspace-relative links to the exact source lines that read or a
 - `lid_topic` (default `livox/points`; [`laserMapping.cpp#L1238`](src/laserMapping.cpp#L1238), [`laserMapping.cpp#L1378`](src/laserMapping.cpp#L1378)): LiDAR subscription topic. Wrong topic leaves `standard_pcl_cbk` without data, so no frames enter the pipeline.
 - `imu_topic` (default `livox/imu`; [`laserMapping.cpp#L1240`](src/laserMapping.cpp#L1240), [`laserMapping.cpp#L1383`](src/laserMapping.cpp#L1383)): IMU subscription. A mismatch stops undistortion and propagation.
 - `time_offset_lidar_imu` (default `0.0`; [`laserMapping.cpp#L1242`](src/laserMapping.cpp#L1242), [`laserMapping.cpp#L558`](src/laserMapping.cpp#L558)): Offset subtracted from IMU timestamps. Wrong values misalign IMU/LiDAR, producing motion distortion; can also trigger “offset time” drops in preprocess.
-- `drone_id` (default `1`; [`laserMapping.cpp#L1244`](src/laserMapping.cpp#L1244), [`laserMapping.cpp#L1297`](src/laserMapping.cpp#L1297), [`MultiUAV.cpp#L35`](src/MultiUAV.cpp#L35)): Swarm identity and topic prefix (simulation adds `quad{id}/`). Duplicate IDs make `QuadstateCbk` ignore peer messages, disabling mutual observations.
+- `drone_id` (default `1`; [`laserMapping.cpp#L1244`](src/laserMapping.cpp#L1244), [`laserMapping.cpp#L1297`](src/laserMapping.cpp#L1297), [`MultiUAV.cpp#L35`](src/MultiUAV.cpp#L35)): Swarm identity and topic prefix (simulation adds `bot{id}/`). Duplicate IDs make `QuadstateCbk` ignore peer messages, disabling mutual observations.
 
 ## multiuav
 - `cluster_extraction_in_predict_region` (default `true`; [`MultiUAV.cpp#L41`](src/MultiUAV.cpp#L41), [`MultiUAV.cpp#L702`](src/MultiUAV.cpp#L702)): Enables ROI clustering around predicted teammate/temporary tracker positions. False skips ROI extraction, delaying/weakening mutual localization.

src/swarm_lio/include/common_lib.h

@@ -17,7 +17,7 @@
 
 using namespace std;
 using namespace Eigen;
-#define MAX_DRONE_ID (5) //集群中最大的飞机编号
+#define MAX_DRONE_ID (20) //集群中最大的飞机编号
 
 // #define DEBUG_PRINT
 #define DEPLOY

src/swarm_lio/launch/simulation.launch.py

@@ -5,6 +5,7 @@
 from ament_index_python.packages import get_package_share_directory
 import os
 import tempfile
+import yaml
 
 
 def make_rviz_config(template_path: str, prefix: str, bot_id: int) -> str:
@@ -24,26 +25,69 @@ def make_rviz_config(template_path: str, prefix: str, bot_id: int) -> str:
 
 
 def generate_launch_description():
+    pkg_share = get_package_share_directory('swarm_lio')
+    launch_dir = os.path.join(pkg_share, 'launch')
+    rviz_cfg_dir = os.path.join(pkg_share, 'rviz_cfg')
+    rviz_template = os.path.join(rviz_cfg_dir, 'ros2_generic.rviz')
+
+    # Derive default bypass flag from simulation.yaml, fallback to "false"
+    bypass_default = "false"
+    try:
+        params_file = os.path.join(pkg_share, 'config', 'simulation.yaml')
+        with open(params_file, 'r') as f:
+            yaml_data = yaml.safe_load(f) or {}
+        bypass_val = yaml_data.get('ros__parameters', {}).get('bypass_initialization', None)
+        if isinstance(bypass_val, bool):
+            bypass_default = "true" if bypass_val else "false"
+    except Exception:
+        pass
+
+    declare_bot_count = DeclareLaunchArgument(
+        "bot_count",
+        default_value="4",
+        description="Number of bots to simulate (starting at 1). Ignored if bot_list is provided.",
+    )
+
     declare_bot_list = DeclareLaunchArgument(
         "bot_list",
-        default_value="1,2,3,4",
-        description="Comma-separated list of bot IDs to simulate",
+        default_value="",
+        description="Comma-separated list of bot IDs to simulate. Overrides bot_count when set.",
     )
 
     declare_rviz_list = DeclareLaunchArgument(
         "rviz_list",
         default_value="",
         description="Comma-separated list of bot IDs to visualize in RViz (defaults to first bot only)",
     )
-
-    pkg_share = get_package_share_directory('swarm_lio')
-    launch_dir = os.path.join(pkg_share, 'launch')
-    rviz_cfg_dir = os.path.join(pkg_share, 'rviz_cfg')
-    rviz_template = os.path.join(rviz_cfg_dir, 'ros2_generic.rviz')
+    declare_use_sim_time = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="true",
+        description="Use simulation time from /clock"
+    )
+    declare_bypass_initialization = DeclareLaunchArgument(
+        "bypass_initialization",
+        default_value=bypass_default,
+        description="If true, publish ground-truth extrinsics to bypass trajectory matching."
+    )
 
     def launch_everything(context, *args, **kwargs):
         actions = []
-        bot_ids = [int(x) for x in context.launch_configurations["bot_list"].split(',')]
+        bot_list_raw = context.launch_configurations.get("bot_list", "").strip()
+        bot_count_raw = context.launch_configurations.get("bot_count", "0").strip()
+
+        if bot_list_raw:
+            bot_ids = [int(x) for x in bot_list_raw.split(',') if x]
+        else:
+            try:
+                bot_count = int(bot_count_raw) if bot_count_raw else 0
+            except ValueError:
+                bot_count = 0
+            bot_ids = list(range(1, max(bot_count, 0) + 1))
+
+        use_sim_time_str = context.launch_configurations.get('use_sim_time', 'true')
+        use_sim_time_bool = str(use_sim_time_str).lower() in ('true', '1', 'yes')
+        bypass_init_str = context.launch_configurations.get('bypass_initialization', 'false')
+        bypass_init_bool = str(bypass_init_str).lower() in ('true', '1', 'yes')
 
         # --- Launch sim for each bot ---
         for bot_id in bot_ids:
@@ -55,10 +99,29 @@ def launch_everything(context, *args, **kwargs):
                     ),
                     launch_arguments={
                         'drone_id': str(bot_id),
-                        'output_mode': output_mode
+                        'output_mode': output_mode,
+                        'use_sim_time': use_sim_time_str,
+                        'actual_uav_num': str(len(bot_ids)),
+                        'bypass_initialization': bypass_init_str,
                     }.items()
                 )
             )
+            actions.append(
+                Node(
+                    package='tf2_ros',
+                    executable='static_transform_publisher',
+                    name=f'bot{bot_id}_aft_mapped_to_base_link_static_tf',
+                    arguments=[ # Identity transform
+                        '--x', '0', '--y', '0', '--z', '0',
+                        '--roll', '0', '--pitch', '0', '--yaw', '0',
+                        '--frame-id', f'bot{bot_id}/aft_mapped',
+                        '--child-frame-id', f'bot{bot_id}/base_link',
+                    ],
+                    parameters=[{'use_sim_time': use_sim_time_bool}],
+                    output='log',
+                )
+            )
+
 
         # --- Decide which bots get RViz ---
         rviz_list_str = context.launch_configurations.get('rviz_list', '').strip()
@@ -67,9 +130,45 @@ def launch_everything(context, *args, **kwargs):
         else:
             rviz_ids = [bot_ids[0]] if bot_ids else []
 
+        # --- Publish extrinsics between robots ---
+        if bot_ids:
+            actions.append(
+                Node(
+                    package='swarm_lio',
+                    executable='robot_extrinsic_publisher',
+                    name='robot_extrinsic_publisher',
+                    parameters=[{
+                        'root_id': int(bot_ids[0]),
+                        'robot_ids': bot_ids,
+                        'robot_prefix': 'bot',
+                        'robot_frame_suffix': 'world',
+                        'use_sim_time': use_sim_time_bool,
+                    }],
+                    output='log',
+                )
+            )
+        if bypass_init_bool and bot_ids:
+            actions.append(
+                Node(
+                    package='swarm_lio',
+                    executable='gt_extrinsic_initializer',
+                    name='gt_extrinsic_initializer',
+                    parameters=[{
+                        'bypass_initialization': True,
+                        'robot_ids': bot_ids,
+                        'robot_prefix': 'bot',
+                        'odom_suffix': '/gt/odom',
+                        'publish_topic': '/global_extrinsic_from_teammate',
+                        'publish_rate_hz': 5.0,
+                        'use_sim_time': use_sim_time_bool,
+                    }],
+                    output='log',
+                )
+            )
+
         # --- Launch one RViz per requested bot ---
         for bot_id in rviz_ids:
-            bot_prefix = f'/quad{bot_id}'
+            bot_prefix = f'/bot{bot_id}'
             rviz_config = make_rviz_config(rviz_template, bot_prefix, bot_id)
 
             actions.append(
@@ -95,7 +194,10 @@ def launch_everything(context, *args, **kwargs):
         return actions
 
     return LaunchDescription([
+        declare_bot_count,
         declare_bot_list,
         declare_rviz_list,
+        declare_use_sim_time,
+        declare_bypass_initialization,
         OpaqueFunction(function=launch_everything),
     ])

src/swarm_lio/launch/single_drone_sim.launch.py

@@ -21,6 +21,16 @@ def launch_setup(context, *args, **kwargs):
     # Resolve launch arguments in the current context
     drone_id = LaunchConfiguration('drone_id').perform(context)
     output_mode = LaunchConfiguration('output_mode').perform(context)
+    use_sim_time_raw = LaunchConfiguration('use_sim_time').perform(context)
+    actual_uav_num_raw = LaunchConfiguration('actual_uav_num').perform(context)
+    bypass_initialization_raw = LaunchConfiguration('bypass_initialization').perform(context)
+    use_sim_time = str(use_sim_time_raw).lower() in ('true', '1', 'yes', 'on')
+    try:
+        actual_uav_num = int(actual_uav_num_raw)
+    except ValueError:
+        actual_uav_num = 1
+    # default to YAML value unless an explicit launch arg is provided
+    bypass_initialization = None
 
     pkg_share = get_package_share_directory('swarm_lio')
 
@@ -34,11 +44,19 @@ def launch_setup(context, *args, **kwargs):
     params['common/drone_id'] = int(drone_id)
     params['common/lid_topic'] = f'bot{drone_id}/lidar_points/points'
     params['common/imu_topic'] = f'bot{drone_id}/imu'
-    params['sub_gt_pose_topic'] = f"/quad{drone_id}/gt/odom"
+    params['sub_gt_pose_topic'] = f"/bot{drone_id}/gt/odom"
+    params['use_sim_time'] = use_sim_time
+    params['multiuav/actual_uav_num'] = actual_uav_num
+    if bypass_initialization_raw:
+        bypass_initialization = str(bypass_initialization_raw).lower() in ('true', '1', 'yes', 'on')
+    else:
+        bypass_initialization = params.get('bypass_initialization', False)
+    params['bypass_initialization'] = bypass_initialization
 
     print(
         f"Launching drone {drone_id} with LIDAR topic {params['common/lid_topic']} "
-        f"and IMU topic {params['common/imu_topic']}"
+        f"and IMU topic {params['common/imu_topic']} "
+        f"(actual_uav_num={actual_uav_num})"
     )
 
     node = Node(
@@ -57,5 +75,8 @@ def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument('drone_id'),
         DeclareLaunchArgument('output_mode', default_value='screen'),
+        DeclareLaunchArgument('use_sim_time', default_value='true'),
+        DeclareLaunchArgument('actual_uav_num', default_value='4'),
+        DeclareLaunchArgument('bypass_initialization', default_value=''),
         OpaqueFunction(function=launch_setup),
     ])