checked out right branch

removed witmotion driver
Merge branch 'master' of https://git.anxietyprime.de/SHERPA/sherpa_ros2_ws
2026-01-11 16:29:31 +01:00 · 2026-01-11 16:07:10 +01:00 · 2026-01-11 16:00:08 +01:00 · 2026-01-11 15:59:50 +01:00 · 2026-01-11 15:51:19 +01:00 · 2026-01-11 15:50:34 +01:00
9 changed files with 145 additions and 216 deletions
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,20 +1,10 @@
-[submodule "src/witmotion_ros"]
-	path = src/witmotion_ros
-	url = https://github.com/ElettraSciComp/witmotion_IMU_ros.git
-	branch = ros2
-[submodule "src/Livox-SDK2"]
-	path = src/Livox-SDK2
-	url = https://github.com/tu-darmstadt-ros-pkg/Livox-SDK2.git
-	branch = jazzy
-[submodule "src/livox_ros_driver2"]
-	path = src/livox_ros_driver2
-	url = https://github.com/tu-darmstadt-ros-pkg/livox_ros_driver2
-	branch = jazzy
-[submodule "src/FAST_LIO"]
-	path = src/FAST_LIO
-	url = https://github.com/Ericsii/FAST_LIO.git
-	branch = ros2
 [submodule "src/livox_ros2_driver"]
 	path = src/livox_ros2_driver
 	url = https://github.com/Livox-SDK/livox_ros2_driver
 	branch = fix_build_error
+[submodule "src/scans_merger"]
+	path = src/scans_merger
+	url = https://github.com/LCAS/scans_merger.git
+[submodule "src/micro-ROS-Agent"]
+	path = src/micro-ROS-Agent
+	url = https://github.com/micro-ROS/micro-ROS-Agent.git
--- a/src/micro-ROS-Agent
+++ b/src/micro-ROS-Agent
--- a/src/scans_merger
+++ b/src/scans_merger
--- a/src/sherpa/launch/tracking.launch.py
+++ b/src/sherpa/launch/tracking.launch.py
@@ -64,71 +64,62 @@ def generate_launch_description():
        ],
    )

-    # ----- pointcloud downsampling -----
-    downsample_vlp16_node = Node(
-        package='target_tracking',
-        executable='cloud_preprocessing_node',
-        parameters=[
-            {"topic_in": "/lidar/vlp16/points"},
-            {"topic_out": "/lidar/vlp16/downsampled"},
-            {"x_min": 0.0},
-            {"x_max": 100.0},
-            {"z_min": -5.0},
-            {"z_max": 10.0},
-            {"tan_h_fov": math.pi}, # 180°  
-            {"tan_v_fov": math.pi / 5}, # ~36°  
-        ],
-    )
-
-    downsample_mid40_node = Node(
-        package='target_tracking',
-        executable='cloud_preprocessing_node',
-        parameters=[
-            {"topic_in": "/lidar/mid40/points"},
-            {"topic_out": "/lidar/mid40/downsampled"},
-            {"x_min": 0.0},
-            {"x_max": 100.0},
-            {"z_min": -5.0},
-            {"z_max": 10.0},
-            {"tan_h_fov": math.pi / 4}, # 45°  
-            {"tan_v_fov": math.pi / 4}, # 45°  
-        ],
-    )
-
    # ----- pointcloud merging -----
    pointcloud_merger_node = Node(
        package='scans_merger',
        executable='scans_merger',
        parameters=[
            {"destination_frame": "lidar_base"},
-            {"input_cloud_1": "/lidar/vlp16/downsampled"},
-            {"input_cloud_2": "/lidar/mid40/downsampled"},
+            {"input_cloud_1": "/lidar/vlp16/points"},
+            {"input_cloud_2": "/lidar/mid40/points"},
            {"merged_cloud": "/lidar/merged/points"},
        ],
    )

+     # ----- pointcloud downsampling -----
+    downsample_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/merged/points"},
+            {"topic_out": "/lidar/merged/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi}, # 180°  
+            {"tan_v_fov": math.pi / 4}, # ~45°  
+        ],
+    )
+
    # ----- tracking -----
    cloud_clustering_node = Node(
        package='target_tracking',
        executable='cloud_clustering_node',
        parameters=[
-            {"topic_in": "/lidar/merged/points"},
+            {"topic_in": "/lidar/merged/downsampled"},
            {"frame_id": 'lidar_base'},
            {"z_dim_scale": 1.0},
            #{"cluster_tolerance": 0.065},
            #{"min_cluster_size": 450},
            #{"max_cluster_size": 1650},
-            {"min_cluster_size": 125},
-            {"max_cluster_size": 2250},
-            {"cluster_tolerance": 0.15},
+            {"min_cluster_size": 50},
+            {"max_cluster_size": 7500},
+            {"cluster_tolerance": 0.065},
+            {"min_width": 0.3},
+            {"min_height": 1.0},
+            {"min_length": 0.3},
+            {"max_width": 2.0},
+            {"max_height": 2.5},
+            {"max_length": 2.0},
        ],
        remappings=[
-            ('/cluster5', '/tracking/cluster0'),
-            ('/cluster4', '/tracking/cluster1'),
-            ('/cluster3', '/tracking/cluster2'),
-            ('/cluster2', '/tracking/cluster3'),
-            ('/cluster1', '/tracking/cluster4'),
-            ('/cluster0', '/tracking/cluster5'),
+            ('/cluster0', '/tracking/cluster0'),
+            ('/cluster1', '/tracking/cluster1'),
+            ('/cluster2', '/tracking/cluster2'),
+            ('/cluster3', '/tracking/cluster3'),
+            ('/cluster4', '/tracking/cluster4'),
+            ('/cluster5', '/tracking/cluster5'),
        ],
    )

@@ -150,9 +141,8 @@ def generate_launch_description():
    ld.add_action(vlp16_driver_node)
    ld.add_action(vlp16_pointcloud_node)
    ld.add_action(mid40_driver_node)
-    ld.add_action(downsample_vlp16_node)
-    ld.add_action(downsample_mid40_node)
    ld.add_action(pointcloud_merger_node)
+    ld.add_action(downsample_node)
    ld.add_action(cloud_clustering_node)
    ld.add_action(servo_tracking_node)

--- a/src/sherpa_lidar_utils/src/servo_angle_tf.py
+++ b/src/sherpa_lidar_utils/src/servo_angle_tf.py
@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import rclpy
 from rclpy.node import Node
+from rclpy.duration import Duration
 from std_msgs.msg import Float32
 from geometry_msgs.msg import TransformStamped
 import tf2_ros
@@ -46,7 +47,7 @@ class ServoTfNode(Node):

        t = TransformStamped()
        #t.header.stamp = (self.get_clock().now() - rclpy.duration.Duration(seconds=0.1)).to_msg()
-        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.stamp = (self.get_clock().now() + Duration(seconds=0.05)).to_msg()
        t.header.frame_id = "livox_base"
        t.child_frame_id = "frame_default"

--- a/src/sherpa_lidar_utils/src/track_cluster.py
+++ b/src/sherpa_lidar_utils/src/track_cluster.py
@@ -42,8 +42,12 @@ class Cluster5Tracker(Node):
            self.get_logger().warn("No points in cluster5 PointCloud2")
            return

-        arr = np.array(points, dtype=np.float32)
-        centroid = np.mean(arr, axis=0)
+        if len(points) == 1:
+            self.get_logger().warn("Only one point found. Defaulting to search target")
+            centroid = (3, 0, 0)
+        else:
+            arr = np.array(points, dtype=np.float32)
+            centroid = np.mean(arr, axis=0)

        pt = PointStamped()
        pt.header.frame_id = msg.header.frame_id
@@ -68,14 +72,15 @@ class Cluster5Tracker(Node):

        # Compute yaw angle in degrees
        x, y = pt_transformed.point.x, pt_transformed.point.y
-        yaw_deg = -math.degrees(math.atan2(y, x))
+        yaw_deg = math.degrees(math.atan2(y, x))

        self.buf.append(yaw_deg)
-        if len(self.buf) > 15:
+        if len(self.buf) > 5:
            self.buf.pop(0)
+        target_angle: float = sum(self.buf) / len(self.buf)

        angle_msg = Float32()
-        angle_msg.data = sum(self.buf) / len(self.buf)
+        angle_msg.data = target_angle
        self.angle_pub.publish(angle_msg)

        #self.get_logger().info(f"Centroid: ({x:.2f}, {y:.2f}, {pt_transformed.point.z:.2f}), Yaw: {yaw_deg:.2f}°")
--- a/src/target_tracking/src/cloud_clustering.cpp
+++ b/src/target_tracking/src/cloud_clustering.cpp
@@ -628,63 +628,40 @@ namespace cloud_clustering
      //          { return cluster_vec[a].second < cluster_vec[b].second; });

      std::vector<std::pair<int, float>> kf_intensity;
+kf_intensity.reserve(objID.size());

-      for (int i = 0; i < objID.size(); i++) {
-          int cluster_index = objID[i];
-          float mean_intensity = cluster_vec[cluster_index].second;
-          kf_intensity.push_back(std::make_pair(i, mean_intensity));
-      }
+for (int kf_idx = 0; kf_idx < objID.size(); kf_idx++)
+{
+    int cluster_idx = objID[kf_idx];
+    float intensity = cluster_vec[cluster_idx].second; // already normalized
+    kf_intensity.emplace_back(kf_idx, intensity);
+}

-      std::sort(kf_intensity.begin(), kf_intensity.end(),
-                [](const auto &a, const auto &b) {
-                    return a.second > b.second;
-                });
+std::sort(kf_intensity.begin(), kf_intensity.end(),
+          [](const auto &a, const auto &b)
+          {
+              return a.second > b.second;
+          });

-      for (auto &[kf_idx, intensity] : kf_intensity) {
-        int cluster_index = objID[kf_idx];
-        auto &cluster = cluster_vec[cluster_index].first;
-        switch (i)
-        {
-        case 0:
-        {
-          publish_cloud(pub_cluster0, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        case 1:
-        {
-          publish_cloud(pub_cluster1, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        case 2:
-        {
-          publish_cloud(pub_cluster2, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        case 3:
-        {
-          publish_cloud(pub_cluster3, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        case 4:
-        {
-          publish_cloud(pub_cluster4, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
+      int pub_idx = 0;

-        case 5:
-        {
-          publish_cloud(pub_cluster5, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        default:
-          break;
-        }
+      for (const auto &[kf_idx, intensity] : kf_intensity)
+      {
+          int cluster_idx = objID[kf_idx];
+          auto &cluster = cluster_vec[cluster_idx].first;
+
+          switch (pub_idx)
+          {
+          case 0: publish_cloud(pub_cluster0, cluster); break;
+          case 1: publish_cloud(pub_cluster1, cluster); break;
+          case 2: publish_cloud(pub_cluster2, cluster); break;
+          case 3: publish_cloud(pub_cluster3, cluster); break;
+          case 4: publish_cloud(pub_cluster4, cluster); break;
+          case 5: publish_cloud(pub_cluster5, cluster); break;
+          default: break;
+          }
+
+          pub_idx++;
      }
    }
  }
--- a/src/witmotion_ros
+++ b/src/witmotion_ros
--- a/tracking.rviz
+++ b/tracking.rviz
@@ -6,11 +6,9 @@ Panels:
      Expanded:
        - /Global Options1
        - /Status1
-        - /PointCloud29/Topic1
-        - /PointStamped1
-        - /PointStamped1/Topic1
+        - /Merged1/Topic1
      Splitter Ratio: 0.5
-    Tree Height: 720
+    Tree Height: 789
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
@@ -28,7 +26,7 @@ Panels:
    Experimental: false
    Name: Time
    SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: Cluster0
 Visualization Manager:
  Class: ""
  Displays:
@@ -79,7 +77,7 @@ Visualization Manager:
      Max Intensity: 100
      Min Color: 0; 0; 0
      Min Intensity: 1
-      Name: PointCloud2
+      Name: Cluster0
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
@@ -112,7 +110,7 @@ Visualization Manager:
      Max Intensity: 0
      Min Color: 0; 0; 0
      Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster1
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
@@ -145,7 +143,7 @@ Visualization Manager:
      Max Intensity: 0
      Min Color: 0; 0; 0
      Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster2
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
@@ -178,7 +176,7 @@ Visualization Manager:
      Max Intensity: 0
      Min Color: 0; 0; 0
      Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster3
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
@@ -211,7 +209,7 @@ Visualization Manager:
      Max Intensity: 0
      Min Color: 0; 0; 0
      Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster4
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
@@ -235,8 +233,8 @@ Visualization Manager:
      Axis: Z
      Channel Name: intensity
      Class: rviz_default_plugins/PointCloud2
-      Color: 85; 0; 127
-      Color Transformer: FlatColor
+      Color: 255; 255; 255
+      Color Transformer: Intensity
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
@@ -244,7 +242,7 @@ Visualization Manager:
      Max Intensity: 0
      Min Color: 0; 0; 0
      Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster5
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
@@ -274,81 +272,15 @@ Visualization Manager:
      Enabled: false
      Invert Rainbow: false
      Max Color: 255; 255; 255
-      Max Intensity: 166
+      Max Intensity: 255
      Min Color: 0; 0; 0
      Min Intensity: 0
-      Name: PointCloud2
+      Name: Merged
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /lidar/vlp16/downsampled
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 154
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /lidar/mid40/downsampled
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 154
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
+      Style: Points
      Topic:
        Depth: 5
        Durability Policy: Volatile
@@ -356,14 +288,47 @@ Visualization Manager:
        Reliability Policy: Best Effort
        Value: /lidar/merged/points
      Use Fixed Frame: true
-      Use rainbow: true
+      Use rainbow: false
      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Downsampled
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /lidar/merged/downsampled
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/PointStamped
      Color: 255; 255; 255
      Enabled: true
      History Length: 1
-      Name: PointStamped
+      Name: Target
      Radius: 0.10000000149011612
      Topic:
        Depth: 5
@@ -419,33 +384,33 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
-      Distance: 20.069171905517578
+      Distance: 7.419111251831055
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
-        X: 2.547194004058838
-        Y: 0.8107800483703613
-        Z: -0.0785689651966095
+        X: 3.313748836517334
+        Y: -0.11149537563323975
+        Z: -0.589261531829834
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8897970914840698
+      Pitch: 0.635202944278717
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz_default_plugins)
-      Yaw: 3.055371046066284
+      Yaw: 3.1103579998016357
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 1011
+  Height: 1080
  Hide Left Dock: false
  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000359fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000359000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003b40000003efc0100000002fb0000000800540069006d00650100000000000003b40000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002580000035900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000039e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
@@ -454,6 +419,6 @@ Window Geometry:
    collapsed: false
  Views:
    collapsed: false
-  Width: 948
-  X: 1927
-  Y: 62
+  Width: 1920
+  X: 1920
+  Y: 0
Author	SHA1	Message	Date
Timo	6970d94402	checked out right branch	2026-01-11 16:29:31 +01:00
Timo	1c6214d8aa	removed witmotion driver	2026-01-11 16:07:10 +01:00
Timo	2f41ed9475	Merge branch 'master' of https://git.anxietyprime.de/SHERPA/sherpa_ros2_ws	2026-01-11 16:00:08 +01:00
Timo	1662868b77	updated submodules	2026-01-11 15:59:50 +01:00
Timo	edf5da50ed	nein	2026-01-11 15:51:19 +01:00
Timo	cc321533fc	Merge branch 'master' of https://git.anxietyprime.de/SHERPA/sherpa_ros2_ws	2026-01-11 15:50:34 +01:00
Timo	4217434b43	definitly not last minute changes	2026-01-11 15:49:41 +01:00