r2-7-12

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "/home/robofish/RC2025/src/rm_msg"
+}

mapping.sh

@@ -2,11 +2,11 @@ source install/setup.bash
 
 commands=(
     "ros2 launch rm_nav_bringup bringup_real.launch.py \
-    world:=RC2026 \
+    world:=1 \
     mode:=mapping \
     lio:=fastlio \
     localization:=icp \
-    lio_rviz:=false \
+    lio_rviz:=true \
     nav_rviz:=true"
 )
 

nav.sh

@@ -1,7 +1,7 @@
+# 备场代码
 source install/setup.bash
 
 commands=(
-   "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
     "ros2 launch rm_nav_bringup bringup_real.launch.py \
     world:=RC2026 \
     mode:=nav \
@@ -10,7 +10,12 @@ commands=(
     lio_rviz:=false \
     nav_rviz:=true"
     "ros2 launch rm_simpal_move simple_move.launch.py"
-    "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.60, y: 3.995, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
+    # "/usr/bin/python3 /home/robofish/RC2025/src/rc_lidar/juxing.py"
+    "/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
+    "/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
+    
 )
 
 for cmd in "${commands[@]}"; do

nav1.sh

@@ -1,16 +1,20 @@
+# 备场代码
 source install/setup.bash
 
 commands=(
-   "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
     "ros2 launch rm_nav_bringup bringup_real.launch.py \
-    world:=RC2025 \
+    world:=map1 \
     mode:=nav \
     lio:=fastlio \
     localization:=icp \
     lio_rviz:=false \
     nav_rviz:=true"
     "ros2 launch rm_simpal_move simple_move.launch.py"
-    "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.56, y: 3.960, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
+    "/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
+    "/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
+    
 )
 
 for cmd in "${commands[@]}"; do

nav2.sh

@@ -0,0 +1,23 @@
+# 备场代码
+source install/setup.bash
+
+commands=(
+    "ros2 launch rm_nav_bringup bringup_real.launch.py \
+    world:=map2 \
+    mode:=nav \
+    lio:=fastlio \
+    localization:=icp \
+    lio_rviz:=false \
+    nav_rviz:=true"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
+    "/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
+    "/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
+    
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done

nav21.rviz

@@ -6,6 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Tree1
+        - /Map2
       Splitter Ratio: 0.5833333134651184
     Tree Height: 462
   - Class: rviz_common/Selection
@@ -66,19 +67,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        dummy:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         livox_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         wheel_1:
           Alpha: 1
           Show Axes: false
@@ -116,9 +109,7 @@ Visualization Manager:
           Value: true
         base_link_fake:
           Value: true
-        dummy:
-          Value: true
-        imu_link:
+        goal_pose:
           Value: true
         lidar_odom:
           Value: true
@@ -143,13 +134,13 @@ Visualization Manager:
       Show Names: false
       Tree:
         map:
+          goal_pose:
+            {}
           odom:
             lidar_odom:
               base_link:
                 base_link_fake:
                   {}
-                imu_link:
-                  {}
                 livox_frame:
                   {}
                 wheel_1:
@@ -208,7 +199,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -229,7 +220,7 @@ Visualization Manager:
         Value: /global_costmap/voxel_grid
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -242,7 +233,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: ""
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -263,7 +254,7 @@ Visualization Manager:
         Value: /mobile_base/sensors/bumper_pointcloud
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 0.6000000238418579
       Class: rviz_default_plugins/Map
       Color Scheme: map
@@ -288,7 +279,7 @@ Visualization Manager:
     - Alpha: 1
       Class: nav2_rviz_plugins/ParticleCloud
       Color: 0; 180; 0
-      Enabled: true
+      Enabled: false
       Max Arrow Length: 0.30000001192092896
       Min Arrow Length: 0.019999999552965164
       Name: Amcl Particle Swarm
@@ -300,7 +291,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Best Effort
         Value: /particle_cloud
-      Value: true
+      Value: false
     - Class: rviz_common/Group
       Displays:
         - Alpha: 0.30000001192092896
@@ -420,7 +411,7 @@ Visualization Manager:
             Reliability Policy: Reliable
             Value: /global_costmap/published_footprint
           Value: false
-      Enabled: true
+      Enabled: false
       Name: Global Planner
     - Class: rviz_common/Group
       Displays:
@@ -532,10 +523,10 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
-      Enabled: true
+      Enabled: false
       Name: Controller
     - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
+      Enabled: false
       Name: MarkerArray
       Namespaces:
         {}
@@ -545,14 +536,14 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /waypoints
-      Value: true
+      Value: false
     - Alpha: 1
       Arrow Length: 0.30000001192092896
       Axes Length: 0.30000001192092896
       Axes Radius: 0.009999999776482582
       Class: rviz_default_plugins/PoseArray
       Color: 255; 25; 0
-      Enabled: true
+      Enabled: false
       Head Length: 0.07000000029802322
       Head Radius: 0.029999999329447746
       Name: PoseArray
@@ -566,9 +557,9 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /teb_poses
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/Marker
-      Enabled: true
+      Enabled: false
       Name: Marker
       Namespaces:
         {}
@@ -579,13 +570,34 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /teb_markers
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/Axes
-      Enabled: true
+      Enabled: false
       Length: 0.5
       Name: base_link_fake
       Radius: 0.07999999821186066
       Reference Frame: base_link_fake
+      Value: false
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /lidar_grid
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /lidar_grid_updates
+      Use Timestamp: false
       Value: true
   Enabled: true
   Global Options:
@@ -625,16 +637,16 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 14.485743522644043
+      Distance: 2.10038423538208
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 3.056168556213379
-        Y: 0.4070683419704437
-        Z: 0.4124468266963959
+        X: -0.5876622200012207
+        Y: 1.5225260257720947
+        Z: 0.4079643487930298
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
@@ -643,7 +655,7 @@ Visualization Manager:
       Pitch: 1.5697963237762451
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz_default_plugins)
-      Yaw: 3.1423678398132324
+      Yaw: 5.280543804168701
     Saved: ~
 Window Geometry:
   Displays:
@@ -661,5 +673,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1545
-  X: 1585
-  Y: 228
+  X: 375
+  Y: 74

src/rc_lidar/caijian.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2, PointField
+import numpy as np
+import struct
+from sklearn.cluster import DBSCAN
+
+class LidarFilterNode(Node):
+    def __init__(self):
+        super().__init__('caijian_node')
+        self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar/pointcloud',
+            self.filter_callback,
+            10)
+        self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
+
+    def filter_callback(self, msg):
+        num_points = msg.width * msg.height
+        data = np.frombuffer(msg.data, dtype=np.uint8)
+        points = np.zeros((num_points, 4), dtype=np.float32)  # x, y, z, intensity
+
+        for i in range(num_points):
+            offset = i * msg.point_step
+            x = struct.unpack_from('f', data, offset)[0]
+            y = struct.unpack_from('f', data, offset + 4)[0]
+            z = struct.unpack_from('f', data, offset + 8)[0]
+            intensity = struct.unpack_from('f', data, offset + 12)[0]
+            points[i] = [x, y, z, intensity]
+
+        z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
+        dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 16.0
+        mask = z_mask & dist_mask
+        filtered_points = points[mask]
+        
+        # 使用DBSCAN去除孤立点
+        if filtered_points.shape[0] > 0:
+            clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
+            core_mask = clustering.labels_ != -1
+            filtered_points = filtered_points[core_mask]
+
+        fields = [
+            PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
+            PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
+            PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
+            PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
+        ]
+        filtered_points_list = filtered_points.tolist()
+        import sensor_msgs_py.point_cloud2 as pc2
+        filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
+        self.publisher_.publish(filtered_msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LidarFilterNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/circlr.py

@@ -0,0 +1,90 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import PointStamped
+from sensor_msgs_py import point_cloud2 as pc2
+import numpy as np
+from sklearn.cluster import DBSCAN
+import time
+
+def statistical_outlier_removal(points, k=20, std_ratio=2.0):
+    from sklearn.neighbors import NearestNeighbors
+    nbrs = NearestNeighbors(n_neighbors=k+1).fit(points)
+    distances, _ = nbrs.kneighbors(points)
+    mean_dist = np.mean(distances[:, 1:], axis=1)
+    threshold = np.mean(mean_dist) + std_ratio * np.std(mean_dist)
+    mask = mean_dist < threshold
+    return points[mask]
+
+class HoopFinder(Node):
+    def __init__(self):
+        super().__init__('find_hoop')
+        self.sub = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.callback,
+            10)
+        self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
+        self.buffer = []
+        self.start_time = None
+        self.hoop_history = []
+
+    def callback(self, msg):
+        # 采集0.4秒内的点云
+        if self.start_time is None:
+            self.start_time = time.time()
+        for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
+            self.buffer.append([p[0], p[1], p[2], p[3]])
+        if time.time() - self.start_time < 0.4:
+            return
+
+        points = np.array(self.buffer)
+        self.buffer = []
+        self.start_time = None
+
+        # 高度滤波
+        filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
+        if len(filtered) == 0:
+            return
+
+        # 统计离群点滤波
+        filtered = statistical_outlier_removal(filtered[:,:3], k=20, std_ratio=2.0)
+
+        # DBSCAN聚类
+        clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered)
+        labels = clustering.labels_
+        unique_labels = set(labels)
+        hoop_pos = None
+        max_cluster_size = 0
+
+        for label in unique_labels:
+            if label == -1:
+                continue
+            cluster = filtered[labels == label]
+            if len(cluster) > max_cluster_size:
+                max_cluster_size = len(cluster)
+                hoop_pos = np.mean(cluster, axis=0)
+
+        # 均值滤波输出
+        if hoop_pos is not None:
+            self.hoop_history.append(hoop_pos)
+            if len(self.hoop_history) > 5:
+                self.hoop_history.pop(0)
+            smooth_pos = np.mean(self.hoop_history, axis=0)
+            pt = PointStamped()
+            pt.header = msg.header
+            pt.point.x = float(smooth_pos[0])
+            pt.point.y = float(smooth_pos[1])
+            pt.point.z = float(smooth_pos[2])
+            self.pub.publish(pt)
+            self.get_logger().info(f"Hoop position (smoothed): {smooth_pos}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = HoopFinder()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/find.py

@@ -0,0 +1,59 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import PointStamped
+from sensor_msgs_py import point_cloud2 as pc2
+import numpy as np
+from sklearn.cluster import DBSCAN
+
+class HoopFinder(Node):
+    def __init__(self):
+        super().__init__('find_hoop')
+        self.sub = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar',
+            self.callback,
+            10)
+        self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
+
+    def callback(self, msg):
+        points = []
+        for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
+            points.append([p[0], p[1], p[2], p[3]])
+        points = np.array(points)
+        filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
+        if len(filtered) == 0:
+            return
+
+        clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered[:,:3])
+        labels = clustering.labels_
+        unique_labels = set(labels)
+        hoop_pos = None
+        max_cluster_size = 0
+
+        for label in unique_labels:
+            if label == -1:
+                continue
+            cluster = filtered[labels == label]
+            if len(cluster) > max_cluster_size:
+                max_cluster_size = len(cluster)
+                hoop_pos = np.mean(cluster[:,:3], axis=0)
+
+        if hoop_pos is not None:
+            pt = PointStamped()
+            pt.header = msg.header
+            pt.point.x = float(hoop_pos[0])
+            pt.point.y = float(hoop_pos[1])
+            pt.point.z = float(hoop_pos[2])
+            self.pub.publish(pt)
+            self.get_logger().info(f"Hoop position: {hoop_pos}")
+            
+def main(args=None):
+    rclpy.init(args=args)
+    node = HoopFinder()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/fliter.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2, PointField
+import numpy as np
+import struct
+from sklearn.cluster import DBSCAN
+
+class LidarFilterNode(Node):
+    def __init__(self):
+        super().__init__('caijian_node')
+        self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar/pointcloud',
+            self.filter_callback,
+            10)
+        self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
+
+    def filter_callback(self, msg):
+        num_points = msg.width * msg.height
+        data = np.frombuffer(msg.data, dtype=np.uint8)
+        points = np.zeros((num_points, 4), dtype=np.float32)  # x, y, z, intensity
+
+        for i in range(num_points):
+            offset = i * msg.point_step
+            x = struct.unpack_from('f', data, offset)[0]
+            y = struct.unpack_from('f', data, offset + 4)[0]
+            z = struct.unpack_from('f', data, offset + 8)[0]
+            intensity = struct.unpack_from('f', data, offset + 12)[0]
+            points[i] = [x, y, z, intensity]
+
+        z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
+        dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 12.0
+        mask = z_mask & dist_mask
+        filtered_points = points[mask]
+        
+        # 使用DBSCAN去除孤立点
+        if filtered_points.shape[0] > 0:
+            clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
+            core_mask = clustering.labels_ != -1
+            filtered_points = filtered_points[core_mask]
+
+        fields = [
+            PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
+            PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
+            PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
+            PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
+        ]
+        filtered_points_list = filtered_points.tolist()
+        import sensor_msgs_py.point_cloud2 as pc2
+        filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
+        self.publisher_.publish(filtered_msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LidarFilterNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/juxing.py

@@ -0,0 +1,340 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2, PointField
+import numpy as np
+import struct
+from sklearn.cluster import DBSCAN
+import cv2
+from visualization_msgs.msg import Marker
+from sklearn.linear_model import RANSACRegressor
+
+
+def ransac_line_3d(points, threshold=0.05, min_inliers=20):
+    best_inliers = []
+    best_line = None
+    N = len(points)
+    if N < min_inliers:
+        return None, []
+    for _ in range(100):
+        idx = np.random.choice(N, 2, replace=False)
+        p1, p2 = points[idx]
+        v = p2 - p1
+        v = v / np.linalg.norm(v)
+        dists = np.linalg.norm(np.cross(points - p1, v), axis=1)
+        inliers = np.where(dists < threshold)[0]
+        if len(inliers) > len(best_inliers):
+            best_inliers = inliers
+            best_line = (p1, p2)
+        if len(best_inliers) > N * 0.5:
+            break
+    return best_line, best_inliers
+
+def fit_rectangle_pca(cluster):
+    # 用PCA找主方向和边界点
+    pts = cluster[:, :3]
+    mean = np.mean(pts, axis=0)
+    cov = np.cov(pts.T)
+    eigvals, eigvecs = np.linalg.eigh(cov)
+    order = np.argsort(eigvals)[::-1]
+    main_dir = eigvecs[:, order[0]]
+    second_dir = eigvecs[:, order[1]]
+    # 投影到主方向和次方向
+    proj_main = np.dot(pts - mean, main_dir)
+    proj_second = np.dot(pts - mean, second_dir)
+    # 找四个角点
+    corners = []
+    for xm in [np.min(proj_main), np.max(proj_main)]:
+        for xs in [np.min(proj_second), np.max(proj_second)]:
+            idx = np.argmin((proj_main - xm)**2 + (proj_second - xs)**2)
+            corners.append(pts[idx])
+    return corners
+
+def rectangle_score(pts):
+    # 评估4个点是否接近矩形
+    d = [np.linalg.norm(pts[i] - pts[(i+1)%4]) for i in range(4)]
+    diag1 = np.linalg.norm(pts[0] - pts[2])
+    diag2 = np.linalg.norm(pts[1] - pts[3])
+    w = max(d)
+    h = min(d)
+    ratio = w / h if h > 0 else 0
+    ideal_ratio = 1.8 / 1.05
+    score = abs(ratio - ideal_ratio) + abs(diag1 - diag2) / max(diag1, diag2)
+    return score
+    
+def classify_lines(lines):
+    # lines: 每条线是 [x, y, z, intensity]
+    # 假设 lines 是8个端点，两两为一条线
+    vertical_lines = []
+    horizontal_lines = []
+    for i in range(0, len(lines), 2):
+        p1 = np.array(lines[i][:3])
+        p2 = np.array(lines[i+1][:3])
+        vec = p2 - p1
+        length = np.linalg.norm(vec)
+        # 计算与地面的夹角（假设地面为z轴为0，垂直为z方向）
+        dz = abs(vec[2])
+        dxy = np.linalg.norm(vec[:2])
+        # 垂直线：z方向分量大，长度约1.05m
+        if dz > dxy and 0.95 < length < 1.15:
+            vertical_lines.append((i, i+1, length))
+        # 水平线：z方向分量小，长度约1.8m
+        elif dz < dxy and 1.7 < length < 1.9:
+            horizontal_lines.append((i, i+1, length))
+    return vertical_lines, horizontal_lines
+
+def find_best_rectangle_from_lines(lines):
+    vertical_lines, horizontal_lines = classify_lines(lines)
+    # 只选出2条垂直线和2条水平线
+    if len(vertical_lines) < 2 or len(horizontal_lines) < 2:
+        return None
+    # 取长度最接近目标的两条
+    vertical_lines = sorted(vertical_lines, key=lambda x: abs(x[2]-1.05))[:2]
+    horizontal_lines = sorted(horizontal_lines, key=lambda x: abs(x[2]-1.8))[:2]
+    # 组合4个端点
+    indices = [vertical_lines[0][0], vertical_lines[0][1],
+                vertical_lines[1][0], vertical_lines[1][1],
+                horizontal_lines[0][0], horizontal_lines[0][1],
+                horizontal_lines[1][0], horizontal_lines[1][1]]
+    # 去重，只保留4个顶点
+    unique_indices = list(set(indices))
+    if len(unique_indices) < 4:
+        return None
+    pts = [lines[idx] for idx in unique_indices[:4]]
+    # 按矩形评分排序
+    from itertools import permutations
+    best_score = float('inf')
+    best_rect = None
+    for order in permutations(range(4)):
+        ordered = [pts[i] for i in order]
+        score = rectangle_score(np.array([p[:3] for p in ordered]))
+        if score < best_score:
+            best_score = score
+            best_rect = ordered
+    return best_rect
+    
+
+class BasketballFrameDetector(Node):
+    def __init__(self):
+        super().__init__('basketball_frame_detector')
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.pointcloud_callback,
+            10
+        )
+        self.publisher = self.create_publisher(
+            PointCloud2,
+            '/basketball_frame_cloud',
+            10
+        )
+        self.marker_pub = self.create_publisher(
+            Marker,
+            '/basketball_frame_lines',
+            10
+        )
+        self.pointcloud_buffer = []
+        self.max_buffer_size = 10  # 减少缓冲帧数，加快响应
+        self.center_buffer = []
+        self.center_buffer_size = 5
+
+    def pointcloud_callback(self, msg):
+        points = self.pointcloud2_to_xyz(msg)
+        if points.shape[0] == 0:
+            return  # 跳过空点云
+        self.pointcloud_buffer.append(points)
+        # 只保留非空点云
+        self.pointcloud_buffer = [arr for arr in self.pointcloud_buffer if arr.shape[0] > 0]
+        if len(self.pointcloud_buffer) > self.max_buffer_size:
+            self.pointcloud_buffer.pop(0)
+        all_points = np.vstack(self.pointcloud_buffer)
+        xy_points = all_points[:, :2]
+
+        if len(xy_points) < 10:
+            self.get_logger().info('点数太少，跳过')
+            return
+        clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
+        labels = clustering.labels_
+        unique_labels = set(labels)
+        found = False
+        for label in unique_labels:
+            if label == -1:
+                continue
+            cluster = all_points[labels == label]
+            if len(cluster) < 30:
+                continue
+            min_x, min_y = np.min(cluster[:, :2], axis=0)
+            max_x, max_y = np.max(cluster[:, :2], axis=0)
+            width = abs(max_x - min_x)
+            height = abs(max_y - min_y)
+            if 1.5 < width < 2.1 and 0.7 < height < 1.3:
+                cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
+                self.publisher.publish(cloud_msg)
+
+                # 用PCA直接找矩形四角
+                corners = fit_rectangle_pca(cluster)
+                from itertools import permutations
+                best_score = float('inf')
+                best_rect = None
+                for order in permutations(range(4)):
+                    ordered = [corners[i] for i in order]
+                    score = rectangle_score(np.array(ordered))
+                    if score < best_score:
+                        best_score = score
+                        best_rect = ordered
+                rect_lines = best_rect
+                if rect_lines is None or len(rect_lines) < 4:
+                    self.get_logger().info('未找到合适矩形')
+                    continue
+
+                # 发布最新的矩形
+                marker = Marker()
+                marker.header = msg.header
+                marker.ns = "basketball_frame"
+                marker.id = 0
+                marker.type = Marker.LINE_LIST
+                marker.action = Marker.ADD
+                marker.scale.x = 0.08
+                marker.color.r = 0.0
+                marker.color.g = 1.0
+                marker.color.b = 0.0
+                marker.color.a = 1.0
+                marker.points = []
+                from geometry_msgs.msg import Point
+                for i in range(4):
+                    p1 = rect_lines[i]
+                    p2 = rect_lines[(i+1)%4]
+                    pt1 = Point(x=float(p1[0]), y=float(p1[1]), z=float(p1[2]))
+                    pt2 = Point(x=float(p2[0]), y=float(p2[1]), z=float(p2[2]))
+                    marker.points.append(pt1)
+                    marker.points.append(pt2)
+                self.marker_pub.publish(marker)
+
+                # 分别发布4条最优边线
+                for i in range(4):
+                    edge_marker = Marker()
+                    edge_marker.header = msg.header
+                    edge_marker.ns = "basketball_frame_edges"
+                    edge_marker.id = i
+                    edge_marker.type = Marker.LINE_STRIP
+                    edge_marker.action = Marker.ADD
+                    edge_marker.scale.x = 0.12
+                    colors = [
+                        (1.0, 0.0, 0.0),
+                        (0.0, 1.0, 0.0),
+                        (0.0, 0.0, 1.0),
+                        (1.0, 1.0, 0.0)
+                    ]
+                    edge_marker.color.r = colors[i][0]
+                    edge_marker.color.g = colors[i][1]
+                    edge_marker.color.b = colors[i][2]
+                    edge_marker.color.a = 1.0
+                    pt1 = Point(x=float(rect_lines[i][0]), y=float(rect_lines[i][1]), z=float(rect_lines[i][2]))
+                    pt2 = Point(x=float(rect_lines[(i+1)%4][0]), y=float(rect_lines[(i+1)%4][1]), z=float(rect_lines[(i+1)%4][2]))
+                    edge_marker.points = [pt1, pt2]
+                    self.marker_pub.publish(edge_marker)
+
+                # 中心点滑动平均
+                center = np.mean(np.array(rect_lines), axis=0)
+                self.center_buffer.append(center)
+                if len(self.center_buffer) > self.center_buffer_size:
+                    self.center_buffer.pop(0)
+                stable_center = np.mean(self.center_buffer, axis=0)
+
+                # 发布中心点Marker
+                center_marker = Marker()
+                center_marker.header = msg.header
+                center_marker.ns = "basketball_frame"
+                center_marker.id = 1
+                center_marker.type = Marker.SPHERE
+                center_marker.action = Marker.ADD
+                center_marker.scale.x = 0.15
+                center_marker.scale.y = 0.15
+                center_marker.scale.z = 0.15
+                center_marker.color.r = 1.0
+                center_marker.color.g = 0.0
+                center_marker.color.b = 0.0
+                center_marker.color.a = 1.0
+                center_marker.pose.position.x = float(stable_center[0])
+                center_marker.pose.position.y = float(stable_center[1])
+                center_marker.pose.position.z = float(stable_center[2])
+                center_marker.pose.orientation.x = 0.0
+                center_marker.pose.orientation.y = 0.0
+                center_marker.pose.orientation.z = 0.0
+                center_marker.pose.orientation.w = 1.0
+                self.marker_pub.publish(center_marker)
+
+                # 计算法向量（篮板主方向，PCA最小特征值方向）
+                pts = np.array(rect_lines)
+                mean = np.mean(pts, axis=0)
+                cov = np.cov(pts.T)
+                eigvals, eigvecs = np.linalg.eigh(cov)
+                order = np.argsort(eigvals)[::-1]
+                normal_vec = eigvecs[:, order[2]]
+                normal_vec = normal_vec / np.linalg.norm(normal_vec)
+                # 向内侧偏移30cm
+                offset_point = stable_center + 0.3 * normal_vec
+
+                # 发布偏移点Marker
+                offset_marker = Marker()
+                offset_marker.header = msg.header
+                offset_marker.ns = "basketball_frame"
+                offset_marker.id = 2
+                offset_marker.type = Marker.SPHERE
+                offset_marker.action = Marker.ADD
+                offset_marker.scale.x = 0.12
+                offset_marker.scale.y = 0.12
+                offset_marker.scale.z = 0.12
+                offset_marker.color.r = 0.0
+                offset_marker.color.g = 0.0
+                offset_marker.color.b = 1.0
+                offset_marker.color.a = 1.0
+                offset_marker.pose.position.x = float(offset_point[0])
+                offset_marker.pose.position.y = float(offset_point[1])
+                offset_marker.pose.position.z = float(offset_point[2])
+                offset_marker.pose.orientation.x = 0.0
+                offset_marker.pose.orientation.y = 0.0
+                offset_marker.pose.orientation.z = 0.0
+                offset_marker.pose.orientation.w = 1.0
+                self.marker_pub.publish(offset_marker)
+
+                found = True
+                break
+        if not found:
+            self.get_logger().info('本帧未找到矩形')
+
+    def pointcloud2_to_xyz(self, cloud_msg):
+        fmt = 'ffff'
+        points = []
+        for i in range(cloud_msg.width):
+            offset = i * cloud_msg.point_step
+            x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
+            points.append([x, y, z, intensity])
+        return np.array(points)
+
+    def xyz_array_to_pointcloud2(self, points, header):
+        msg = PointCloud2()
+        msg.header = header
+        msg.height = 1
+        msg.width = len(points)
+        msg.is_dense = True
+        msg.is_bigendian = False
+        msg.point_step = 16
+        msg.row_step = msg.point_step * msg.width
+        msg.fields = [
+            PointField(name='x', offset=0, datatype=7, count=1),
+            PointField(name='y', offset=4, datatype=7, count=1),
+            PointField(name='z', offset=8, datatype=7, count=1),
+            PointField(name='intensity', offset=12, datatype=7, count=1),
+        ]
+        msg.data = b''.join([struct.pack('ffff', *p) for p in points])
+        return msg
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = BasketballFrameDetector()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/pcd2pgm.py

@@ -0,0 +1,75 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from nav_msgs.msg import OccupancyGrid
+import numpy as np
+import struct
+import time
+
+class PointCloudToGrid(Node):
+    def __init__(self):
+        super().__init__('pointcloud_to_grid')
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.pointcloud_callback,
+            10)
+        self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
+        self.grid_size = 2000
+        self.resolution = 0.02
+        self.origin_x = -20.0
+        self.origin_y = -20.0 
+        self.points_buffer = []
+        self.last_header = None
+        # 定时器每0.5秒触发一次
+        self.timer = self.create_timer(0.5, self.publish_grid)
+ 
+    def pointcloud_callback(self, msg):
+        points = self.pointcloud2_to_xyz_array(msg)
+        self.points_buffer.append(points)
+        self.last_header = msg.header  # 保存最新header用于地图消息
+
+    def publish_grid(self):
+        if not self.points_buffer:
+            return
+        # 合并0.5秒内所有点
+        all_points = np.concatenate(self.points_buffer, axis=0)
+        grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
+        for x, y, z in all_points:
+            if z < 2.0:
+                ix = int((x - self.origin_x) / self.resolution)
+                iy = int((y - self.origin_y) / self.resolution)
+                if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
+                    grid[iy, ix] = 100
+        grid_msg = OccupancyGrid()
+        if self.last_header:
+            grid_msg.header = self.last_header
+        grid_msg.info.resolution = self.resolution
+        grid_msg.info.width = self.grid_size
+        grid_msg.info.height = self.grid_size
+        grid_msg.info.origin.position.x = self.origin_x
+        grid_msg.info.origin.position.y = self.origin_y
+        grid_msg.data = grid.flatten().tolist()
+        self.publisher.publish(grid_msg)
+        self.points_buffer.clear()  # 清空缓存
+
+    def pointcloud2_to_xyz_array(self, cloud_msg):
+        # 解析 PointCloud2 数据为 numpy 数组
+        fmt = 'fff'  # x, y, z
+        point_step = cloud_msg.point_step
+        data = cloud_msg.data
+        points = []
+        for i in range(0, len(data), point_step):
+            x, y, z = struct.unpack_from(fmt, data, i)
+            points.append([x, y, z])
+        return np.array(points)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PointCloudToGrid()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/save_pcd.py

@@ -0,0 +1,55 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+import numpy as np
+import open3d as o3d
+import time
+
+class PointCloudSaver(Node):
+    def __init__(self):
+        super().__init__('pcd_saver')
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.listener_callback,
+            10)
+        self.point_clouds = []
+        self.start_time = time.time()
+        self.timer = self.create_timer(3.0, self.save_and_exit)
+        self.saving = False
+
+    def listener_callback(self, msg):
+        if not self.saving:
+            pc = self.pointcloud2_to_xyz_array(msg)
+            if pc is not None:
+                self.point_clouds.append(pc)
+
+    def pointcloud2_to_xyz_array(self, cloud_msg):
+        # 仅支持 x, y, z 均为 float32 且无 padding 的点云
+        import struct
+        points = []
+        point_step = cloud_msg.point_step
+        for i in range(cloud_msg.width * cloud_msg.height):
+            offset = i * point_step
+            x, y, z = struct.unpack_from('fff', cloud_msg.data, offset)
+            points.append([x, y, z])
+        return np.array(points)
+
+    def save_and_exit(self):
+        if not self.saving:
+            self.saving = True
+            if self.point_clouds:
+                all_points = np.vstack(self.point_clouds)
+                pcd = o3d.geometry.PointCloud()
+                pcd.points = o3d.utility.Vector3dVector(all_points)
+                o3d.io.write_point_cloud("output.pcd", pcd)
+                self.get_logger().info("Saved output.pcd")
+            rclpy.shutdown()
+
+def main(args=None):
+    rclpy.init(args=args)
+    saver = PointCloudSaver()
+    rclpy.spin(saver)
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/simple.py

@@ -0,0 +1,102 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2, PointField
+import numpy as np
+import struct
+from sklearn.cluster import DBSCAN
+
+class BasketballFrameDetector(Node):
+    def __init__(self):
+        super().__init__('basketball_frame_detector')
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.pointcloud_callback,
+            10
+        )
+        self.publisher = self.create_publisher(
+            PointCloud2,
+            '/basketball_frame_cloud',
+            10
+        )
+        self.pointcloud_buffer = []
+
+    def pointcloud_callback(self, msg):
+        points = self.pointcloud2_to_xyz(msg)
+        self.pointcloud_buffer.append(points)
+        self.get_logger().info(f'已保存点云组数: {len(self.pointcloud_buffer)}')
+        if len(self.pointcloud_buffer) < 10:
+            return
+
+        # 合并10组点云
+        all_points = np.vstack(self.pointcloud_buffer)
+        xy_points = all_points[:, :2]
+        self.get_logger().info(f'合并后点数: {xy_points.shape[0]}')
+
+        # 清空缓存，准备下一批
+        self.pointcloud_buffer = []
+
+        # 聚类识别
+        if len(xy_points) < 10:
+            return
+        clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
+        labels = clustering.labels_
+        unique_labels = set(labels)
+        for label in unique_labels:
+            if label == -1:
+                continue
+            cluster = all_points[labels == label]
+            if len(cluster) < 30:
+                continue
+            min_x, min_y = np.min(cluster[:, :2], axis=0)
+            max_x, max_y = np.max(cluster[:, :2], axis=0)
+            width = abs(max_x - min_x)
+            height = abs(max_y - min_y)
+            self.get_logger().info(
+                f'聚类: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
+            )
+            if 1.6 < width < 2.0 and 0.8 < height < 1.2:
+                self.get_logger().info(
+                    f'可能是篮球框: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
+                )
+                # 发布识别到的篮球框点云
+                cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
+                self.publisher.publish(cloud_msg)
+
+    def pointcloud2_to_xyz(self, cloud_msg):
+        fmt = 'ffff'  # x, y, z, intensity
+        points = []
+        for i in range(cloud_msg.width):
+            offset = i * cloud_msg.point_step
+            x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
+            points.append([x, y, z, intensity])
+        return np.array(points)
+
+    def xyz_array_to_pointcloud2(self, points, header):
+        # 构造 PointCloud2 消息
+        msg = PointCloud2()
+        msg.header = header
+        msg.height = 1
+        msg.width = len(points)
+        msg.is_dense = True
+        msg.is_bigendian = False
+        msg.point_step = 16
+        msg.row_step = msg.point_step * msg.width
+        msg.fields = [
+            PointField(name='x', offset=0, datatype=7, count=1),
+            PointField(name='y', offset=4, datatype=7, count=1),
+            PointField(name='z', offset=8, datatype=7, count=1),
+            PointField(name='intensity', offset=12, datatype=7, count=1),
+        ]
+        msg.data = b''.join([struct.pack('ffff', *p) for p in points])
+        return msg
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = BasketballFrameDetector()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/simple_icp.py

@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from sensor_msgs_py import point_cloud2
+from geometry_msgs.msg import PoseStamped
+import open3d as o3d
+import numpy as np
+from transforms3d.quaternions import mat2quat
+
+class PointCloudLocalization(Node):
+    def __init__(self):
+        super().__init__('point_cloud_localizer')
+        
+        # 加载参考点云地图 (PCD文件)
+        self.reference_map = o3d.io.read_point_cloud("/home/robofish/RC2025/lankuang.pcd")  # 替换为你的PCD文件路径
+        if not self.reference_map.has_points():
+            self.get_logger().error("Failed to load reference map!")
+            rclpy.shutdown()
+        
+        # 预处理参考地图
+        self.reference_map = self.reference_map.voxel_down_sample(voxel_size=0.05)
+        self.reference_map.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)[0]
+        
+        # 创建ICP对象
+        self.icp = o3d.pipelines.registration.registration_icp
+        self.threshold = 0.5  # 匹配距离阈值 (米)
+        self.trans_init = np.identity(4)  # 初始变换矩阵
+        
+        # 订阅激光雷达点云
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.lidar_callback,
+            10)
+        
+        # 发布估计位置
+        self.pose_pub = self.create_publisher(PoseStamped, '/estimated_pose', 10)
+        
+        self.get_logger().info("Point Cloud Localization Node Initialized!")
+
+    def ros_pc2_to_o3d(self, ros_cloud):
+        """将ROS PointCloud2转换为Open3D点云"""
+        # 提取xyz坐标
+        points = point_cloud2.read_points(ros_cloud, field_names=("x", "y", "z"), skip_nans=True)
+        xyz = np.array([ [p[0], p[1], p[2]] for p in points ], dtype=np.float32)
+        
+        if xyz.shape[0] == 0:
+            return None
+            
+        # 创建Open3D点云
+        pcd = o3d.geometry.PointCloud()
+        pcd.points = o3d.utility.Vector3dVector(xyz)
+        return pcd
+
+    def preprocess_pointcloud(self, pcd):
+        """点云预处理"""
+        # 降采样
+        pcd = pcd.voxel_down_sample(voxel_size=0.03)
+        
+        # 移除离群点
+        pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
+        
+        # 移除地面 (可选)
+        # plane_model, inliers = pcd.segment_plane(distance_threshold=0.1, ransac_n=3, num_iterations=100)
+        # pcd = pcd.select_by_index(inliers, invert=True)
+        
+        return pcd
+
+    def lidar_callback(self, msg):
+        """处理新的激光雷达数据"""
+        # 转换为Open3D格式
+        current_pcd = self.ros_pc2_to_o3d(msg)
+        if current_pcd is None:
+            self.get_logger().warn("Received empty point cloud!")
+            return
+        
+        # 预处理当前点云
+        current_pcd = self.preprocess_pointcloud(current_pcd)
+        
+        # 执行ICP配准
+        reg_result = self.icp(
+            current_pcd, self.reference_map, self.threshold,
+            self.trans_init,
+            o3d.pipelines.registration.TransformationEstimationPointToPoint(),
+            o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)
+        )
+        
+        # 更新变换矩阵
+        self.trans_init = reg_result.transformation
+        
+        # 提取位置和方向
+        translation = reg_result.transformation[:3, 3]
+        rotation_matrix = reg_result.transformation[:3, :3]
+        
+        # 转换为四元数
+        quaternion = mat2quat(reg_result.transformation[:3, :3])  # 注意返回顺序为 [w, x, y, z]
+        
+        # 发布位姿
+        pose_msg = PoseStamped()
+        pose_msg.header.stamp = self.get_clock().now().to_msg()
+        pose_msg.header.frame_id = "livox_frame"
+        pose_msg.pose.position.x = translation[0]
+        pose_msg.pose.position.y = translation[1]
+        pose_msg.pose.position.z = translation[2]
+        pose_msg.pose.orientation.x = quaternion[1]  # x
+        pose_msg.pose.orientation.y = quaternion[2]  # y
+        pose_msg.pose.orientation.z = quaternion[3]  # z
+        pose_msg.pose.orientation.w = quaternion[0]  # w
+        
+        self.pose_pub.publish(pose_msg)
+        self.get_logger().info(f"Estimated Position: x={translation[0]:.2f}, y={translation[1]:.2f}, z={translation[2]:.2f}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PointCloudLocalization()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/xiamian.py

@@ -0,0 +1,75 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from nav_msgs.msg import OccupancyGrid
+import numpy as np
+import struct
+import time
+
+class PointCloudToGrid(Node):
+    def __init__(self):
+        super().__init__('pointcloud_to_grid')
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.pointcloud_callback,
+            10)
+        self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
+        self.grid_size = 2000
+        self.resolution = 0.02
+        self.origin_x = -20.0
+        self.origin_y = -20.0 
+        self.points_buffer = []
+        self.last_header = None
+        # 定时器每0.5秒触发一次
+        self.timer = self.create_timer(0.5, self.publish_grid)
+ 
+    def pointcloud_callback(self, msg):
+        points = self.pointcloud2_to_xyz_array(msg)
+        self.points_buffer.append(points)
+        self.last_header = msg.header  # 保存最新header用于地图消息
+
+    def publish_grid(self):
+        if not self.points_buffer:
+            return
+        # 合并0.5秒内所有点
+        all_points = np.concatenate(self.points_buffer, axis=0)
+        grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
+        for x, y, z in all_points:
+            if z < 2.0:
+                ix = int((x - self.origin_x) / self.resolution)
+                iy = int((y - self.origin_y) / self.resolution)
+                if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
+                    grid[iy, ix] = 100
+        grid_msg = OccupancyGrid()
+        if self.last_header:
+            grid_msg.header = self.last_header
+        grid_msg.info.resolution = self.resolution
+        grid_msg.info.width = self.grid_size
+        grid_msg.info.height = self.grid_size
+        grid_msg.info.origin.position.x = self.origin_x
+        grid_msg.info.origin.position.y = self.origin_y
+        grid_msg.data = grid.flatten().tolist()
+        self.publisher.publish(grid_msg)
+        self.points_buffer.clear()  # 清空缓存
+
+    def pointcloud2_to_xyz_array(self, cloud_msg):
+        # 解析 PointCloud2 数据为 numpy 数组
+        fmt = 'fff'  # x, y, z
+        point_step = cloud_msg.point_step
+        data = cloud_msg.data
+        points = []
+        for i in range(0, len(data), point_step):
+            x, y, z = struct.unpack_from(fmt, data, i)
+            points.append([x, y, z])
+        return np.array(points)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PointCloudToGrid()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/zhaoban.py

@@ -0,0 +1,51 @@
+import numpy as np
+from sklearn.cluster import DBSCAN
+from scipy.optimize import leastsq
+import matplotlib.pyplot as plt
+
+def fit_circle(x, y):
+    # 拟合圆的函数
+    def calc_R(xc, yc):
+        return np.sqrt((x - xc)**2 + (y - yc)**2)
+    def f(c):
+        Ri = calc_R(*c)
+        return Ri - Ri.mean()
+    center_estimate = np.mean(x), np.mean(y)
+    center, _ = leastsq(f, center_estimate)
+    radius = calc_R(*center).mean()
+    return center[0], center[1], radius
+
+def find_circle(points, eps=0.5, min_samples=10):
+    # 聚类
+    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(points)
+    labels = clustering.labels_
+    # 只取最大簇    
+    unique, counts = np.unique(labels[labels != -1], return_counts=True)
+    if len(unique) == 0:
+        raise ValueError("未找到有效聚类")
+    main_cluster = unique[np.argmax(counts)]
+    cluster_points = points[labels == main_cluster]
+    x, y = cluster_points[:, 0], cluster_points[:, 1]
+    # 拟合圆
+    xc, yc, r = fit_circle(x, y)
+    return xc, yc, r, cluster_points
+
+if __name__ == "__main__":
+    # 示例数据
+    np.random.seed(0)
+    angle = np.linspace(0, 2 * np.pi, 100)
+    x = 5 + 3 * np.cos(angle) + np.random.normal(0, 0.1, 100)
+    y = 2 + 3 * np.sin(angle) + np.random.normal(0, 0.1, 100)
+    points = np.vstack((x, y)).T
+
+    xc, yc, r, cluster_points = find_circle(points)
+    print(f"圆心: ({xc:.2f}, {yc:.2f}), 半径: {r:.2f}")
+
+    # 可视化
+    plt.scatter(points[:, 0], points[:, 1], label='所有点')
+    plt.scatter(cluster_points[:, 0], cluster_points[:, 1], label='聚类点')
+    circle = plt.Circle((xc, yc), r, color='r', fill=False, label='拟合圆')
+    plt.gca().add_patch(circle)
+    plt.legend()
+    plt.axis('equal')
+    plt.show()

src/rm_driver/livox_ros_driver2/src/config/MID360_config.json

@@ -25,7 +25,7 @@
   },
   "lidar_configs" : [
     {
-      "ip" : "192.168.1.137",
+      "ip" : "192.168.1.176",
       "pcl_data_type" : 1,
       "pattern_mode" : 0,
       "extrinsic_parameter" : {

src/rm_driver/rm_serial_driver/script/R2_Serial.py

@@ -0,0 +1,251 @@
+import rclpy
+from rclpy.node import Node
+import serial
+import struct
+from rm_msgs.msg import DataAim  
+from geometry_msgs.msg import TransformStamped
+import tf2_ros
+import tf2_geometry_msgs
+import time
+
+class AimDataSerial(Node):
+    def __init__(self):
+        super().__init__('aim_data_serial')
+        
+        # 串口配置
+        self.serial_port_r2 = '/dev/r2'
+        self.serial_port_r1 = '/dev/ttyACM0'
+        self.baud_rate = 115200
+        
+        # 重连计数器
+        self.r1_reconnect_count = 0
+        self.r2_reconnect_count = 0
+        self.max_reconnect_attempts = 5
+        
+        # 初始化串口连接
+        self.init_serial_connections()
+        
+        # TF2监听器，用于获取baselink在map上的坐标
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+
+        # 订阅话题
+        self.subscription = self.create_subscription(
+            DataAim,
+            '/chassis/data_aim',
+            self.aim_callback,
+            10
+        )
+        
+        # 定时器，定期获取和发送坐标数据
+        self.timer = self.create_timer(0.1, self.position_timer_callback)  # 10Hz
+        
+        # 计数器，用于减少日志输出
+        self.position_counter = 0
+        
+        self.get_logger().info('Aim data serial node started')
+    
+    def init_serial_connections(self):
+        """初始化串口连接"""
+        # 初始化r2串口（瞄准数据）
+        self.serial_conn_r2 = self.open_serial_port(self.serial_port_r2, "r2")
+        
+        # 初始化r1串口（坐标数据）
+        self.serial_conn_r1 = self.open_serial_port(self.serial_port_r1, "r1")
+    
+    def open_serial_port(self, port, name):
+        """打开串口，返回串口对象或None"""
+        try:
+            ser = serial.Serial(
+                port=port,
+                baudrate=self.baud_rate,
+                timeout=1
+            )
+            self.get_logger().info(f'Serial port {port} ({name}) opened successfully')
+            return ser
+        except Exception as e:
+            self.get_logger().error(f'Failed to open serial port {name}: {e}')
+            return None
+    
+    def reconnect_serial(self, port, name, current_conn, reconnect_count_attr):
+        """尝试重新连接串口"""
+        reconnect_count = getattr(self, reconnect_count_attr)
+        if reconnect_count >= self.max_reconnect_attempts:
+            return None
+            
+        self.get_logger().warn(f'Attempting to reconnect {name} (attempt {reconnect_count + 1})')
+        
+        # 关闭现有连接
+        if current_conn and current_conn.is_open:
+            current_conn.close()
+        
+        # 等待一下再重连
+        time.sleep(0.5)
+        
+        # 尝试重新连接
+        new_conn = self.open_serial_port(port, name)
+        setattr(self, reconnect_count_attr, reconnect_count + 1)
+        
+        if new_conn:
+            # 重连成功，重置计数器
+            setattr(self, reconnect_count_attr, 0)
+            self.get_logger().info(f'{name} reconnected successfully')
+        
+        return new_conn
+    
+    def safe_serial_write(self, serial_conn, data, port_name):
+        """安全的串口写入，包含错误处理和重连"""
+        if not serial_conn:
+            return False
+            
+        try:
+            serial_conn.write(data)
+            return True
+        except Exception as e:
+            self.get_logger().error(f'Write error on {port_name}: {e}')
+            
+            # 尝试重连
+            if port_name == "r1":
+                self.serial_conn_r1 = self.reconnect_serial(
+                    self.serial_port_r1, "r1", self.serial_conn_r1, "r1_reconnect_count"
+                )
+            elif port_name == "r2":
+                self.serial_conn_r2 = self.reconnect_serial(
+                    self.serial_port_r2, "r2", self.serial_conn_r2, "r2_reconnect_count"
+                )
+            
+            return False
+    
+    def aim_callback(self, msg):
+        if not self.serial_conn_r2:
+            return
+            
+        try:
+            # 提取yaw和distance数据
+            yaw = msg.yaw
+            distance = msg.distance
+            
+            self.get_logger().info(f'Received - yaw: {yaw}, distance: {distance}')
+            
+            # 构建发送数据包
+            # 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
+            packet = bytearray()
+            packet.append(0xFF)  # 包头
+            packet.extend(struct.pack('<f', yaw))      # yaw (小端序float)
+            packet.extend(struct.pack('<f', distance)) # distance (小端序float)
+            packet.append(0xFE)  # 包尾
+            
+            # 打印发送的两个数据
+            self.get_logger().info(f'发送到R2的数据: yaw={yaw}, distance={distance}')
+            
+            # 发送数据到r2串口
+            if self.safe_serial_write(self.serial_conn_r2, packet, "r2"):
+                self.get_logger().info(f'Sent aim packet to r2: {packet.hex()}')
+            
+        except Exception as e:
+            self.get_logger().error(f'Error in aim_callback: {e}')
+    
+    def position_timer_callback(self):
+        if not self.serial_conn_r1:
+            return
+            
+        try:
+            # 获取baselink相对于map的变换
+            transform = self.tf_buffer.lookup_transform(
+                'map',  # 目标坐标系
+                'base_link',  # 源坐标系
+                rclpy.time.Time()  # 最新时间
+            )
+            
+            # 提取位置坐标
+            x = transform.transform.translation.x
+            y = transform.transform.translation.y
+            z = transform.transform.translation.z
+            
+            # 打印发送的三个数据
+            self.get_logger().info(f'发送到R1的三个坐标数据: x={x:.3f}, y={y:.3f}, z={z:.3f}')
+            
+            # 构建坐标数据包
+            # 格式: 包头(0xAA) + x(4字节float) + y(4字节float) + z(4字节float) + 包尾(0xBB)
+            packet = bytearray()
+            packet.append(0xAA)  # 包头
+            packet.extend(struct.pack('<f', x))  # x坐标 (小端序float)
+            packet.extend(struct.pack('<f', y))  # y坐标 (小端序float)
+            packet.extend(struct.pack('<f', z))  # z坐标 (小端序float)
+            packet.append(0xBB)  # 包尾
+
+            # 发送数据到r1串口
+            if self.safe_serial_write(self.serial_conn_r1, packet, "r1"):
+                self.get_logger().info(f'Sent position packet to r1: {packet.hex()}')
+            
+        except tf2_ros.TransformException as e:
+            # TF变换异常，可能是坐标系不存在或时间不同步
+            if self.position_counter % 100 == 0:  # 减少TF异常日志频率
+                self.get_logger().warn(f'Transform exception: {e}')
+        except Exception as e:
+            self.get_logger().error(f'Error in position_timer_callback: {e}')
+    # def position_timer_callback(self):
+    #     if not self.serial_conn_r1:
+    #         return
+            
+    #     try:
+    #         # 获取baselink相对于map的变换
+    #         transform = self.tf_buffer.lookup_transform(
+    #             'map',  # 目标坐标系
+    #             'base_link',  # 源坐标系
+    #             rclpy.time.Time()  # 最新时间
+    #         )
+            
+    #         # 提取位置坐标
+    #         x = transform.transform.translation.x
+    #         y = transform.transform.translation.y
+    #         z = transform.transform.translation.z
+            
+    #         # 减少日志输出频率
+    #         self.position_counter += 1
+    #         if self.position_counter % 50 == 0:  # 每5秒打印一次
+    #             self.get_logger().info(f'Position - x: {x:.3f}, y: {y:.3f}, z: {z:.3f}')
+            
+    #         # 构建坐标数据包
+    #         # 格式: 包头(0xAA) + x(4字节float) + y(4字节float) + z(4字节float) + 包尾(0xBB)
+    #         packet = bytearray()
+    #         packet.append(0xAA)  # 包头
+    #         packet.extend(struct.pack('<f', x))  # x坐标 (小端序float)
+    #         packet.extend(struct.pack('<f', y))  # y坐标 (小端序float)
+    #         packet.extend(struct.pack('<f', z))  # z坐标 (小端序float)
+    #         packet.append(0xBB)  # 包尾
+
+    #         # 发送数据到r1串口
+    #         if self.safe_serial_write(self.serial_conn_r1, packet, "r1"):
+    #             if self.position_counter % 50 == 0:  # 只在打印位置时也打印发送信息
+    #                 self.get_logger().info(f'Sent position packet to r1: {packet.hex()}')
+            
+    #     except tf2_ros.TransformException as e:
+    #         # TF变换异常，可能是坐标系不存在或时间不同步
+    #         if self.position_counter % 100 == 0:  # 减少TF异常日志频率
+    #             self.get_logger().warn(f'Transform exception: {e}')
+    #     except Exception as e:
+    #         self.get_logger().error(f'Error in position_timer_callback: {e}')
+    
+    def __del__(self):
+        if hasattr(self, 'serial_conn_r2') and self.serial_conn_r2 and self.serial_conn_r2.is_open:
+            self.serial_conn_r2.close()
+            self.get_logger().info('Serial port r2 closed')
+        if hasattr(self, 'serial_conn_r1') and self.serial_conn_r1 and self.serial_conn_r1.is_open:
+            self.serial_conn_r1.close()
+            self.get_logger().info('Serial port r1 closed')
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    try:
+        node = AimDataSerial()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if rclpy.ok():
+            rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py

@@ -1,79 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-import serial
-import struct
-from rm_msgs.msg import DataAim  # 假设使用DataAim消息类型，您可以根据实际消息类型调整
-class AimDataSerial(Node):
-    def __init__(self):
-        super().__init__('aim_data_serial')
-        
-        # 串口配置
-        self.serial_port = '/dev/ttyUSB0'  # 根据实际串口设备调整
-        self.baud_rate = 115200
-        
-        try:
-            self.serial_conn = serial.Serial(
-                port=self.serial_port,
-                baudrate=self.baud_rate,
-                timeout=1
-            )
-            self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
-        except Exception as e:
-            self.get_logger().error(f'Failed to open serial port: {e}')
-            return
-        
-        # 订阅话题
-        self.subscription = self.create_subscription(
-            DataAim,  # 根据实际消息类型调整
-            '/chassis/data_aim',
-            self.aim_callback,
-            10
-        )
-        
-        self.get_logger().info('Aim data serial node started')
-    
-    def aim_callback(self, msg):
-        try:
-            # 提取yaw和distance数据
-            # 根据实际消息结构调整，这里假设使用Point32的x和y字段
-            yaw = msg.yaw
-            distance = msg.distance
-            
-            self.get_logger().info(f'Received - yaw: {yaw}, distance: {distance}')
-            
-            # 构建发送数据包
-            # 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
-            packet = bytearray()
-            packet.append(0xFF)  # 包头
-            packet.extend(struct.pack('<f', yaw))      # yaw (小端序float)
-            packet.extend(struct.pack('<f', distance)) # distance (小端序float)
-            packet.append(0xFE)  # 包尾
-            
-            # 发送数据
-            self.serial_conn.write(packet)
-            self.get_logger().debug(f'Sent packet: {packet.hex()}')
-            
-        except Exception as e:
-            self.get_logger().error(f'Error in aim_callback: {e}')
-    
-    def __del__(self):
-        if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
-            self.serial_conn.close()
-            self.get_logger().info('Serial port closed')
-
-def main(args=None):
-    rclpy.init(args=args)
-    
-    try:
-        node = AimDataSerial()
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        if rclpy.ok():
-            rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/rm_driver/rm_serial_driver/script/pub_aim.py

@@ -1,79 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-import serial
-import struct
-from rm_msgs.msg import DataAim  # 假设使用DataAim消息类型，您可以根据实际消息类型调整
-class AimDataSerial(Node):
-    def __init__(self):
-        super().__init__('aim_data_serial')
-        
-        # 串口配置
-        self.serial_port = '/dev/ttyUSB0'  # 根据实际串口设备调整
-        self.baud_rate = 115200
-        
-        try:
-            self.serial_conn = serial.Serial(
-                port=self.serial_port,
-                baudrate=self.baud_rate,
-                timeout=1
-            )
-            self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
-        except Exception as e:
-            self.get_logger().error(f'Failed to open serial port: {e}')
-            return
-        
-        # 订阅话题
-        self.subscription = self.create_subscription(
-            DataAim,  # 根据实际消息类型调整
-            '/chassis/data_aim',
-            self.aim_callback,
-            10
-        )
-        
-        self.get_logger().info('Aim data serial node started')
-    
-    def aim_callback(self, msg):
-        try:
-            # 提取yaw和distance数据
-            # 根据实际消息结构调整，这里假设使用Point32的x和y字段
-            yaw = msg.yaw
-            distance = msg.distance
-            
-            self.get_logger().info(f'Received - yaw: {yaw}, distance: {distance}')
-            
-            # 构建发送数据包
-            # 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
-            packet = bytearray()
-            packet.append(0xFF)  # 包头
-            packet.extend(struct.pack('<f', yaw))      # yaw (小端序float)
-            packet.extend(struct.pack('<f', distance)) # distance (小端序float)
-            packet.append(0xFE)  # 包尾
-            
-            # 发送数据
-            self.serial_conn.write(packet)
-            self.get_logger().debug(f'Sent packet: {packet.hex()}')
-            
-        except Exception as e:
-            self.get_logger().error(f'Error in aim_callback: {e}')
-    
-    def __del__(self):
-        if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
-            self.serial_conn.close()
-            self.get_logger().info('Serial port closed')
-
-def main(args=None):
-    rclpy.init(args=args)
-    
-    try:
-        node = AimDataSerial()
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        if rclpy.ok():
-            rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/rm_driver/rm_serial_driver/script/pub_goal.py

@@ -0,0 +1,36 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PointStamped
+from rm_msgs.msg import MoveGoal
+
+class ClickedGoalPublisher(Node):
+    def __init__(self):
+        super().__init__('clicked_goal_publisher')
+        self.subscription = self.create_subscription(
+            PointStamped,
+            '/clicked_point',
+            self.clicked_callback,
+            10
+        )
+        self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
+
+    def clicked_callback(self, msg):
+        goal = MoveGoal()
+        goal.x = msg.point.x
+        goal.y = msg.point.y
+        goal.angle = 0.0
+        goal.max_speed = 0.0
+        goal.tolerance = 0.1
+        goal.rotor = False
+        self.publisher.publish(goal)
+        self.get_logger().info(f'发布目标: x={goal.x:.2f}, y={goal.y:.2f}')
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ClickedGoalPublisher()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rm_driver/rm_serial_driver/script/pub_self.py

@@ -1,101 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-import serial
-import struct
-import tf2_ros
-from geometry_msgs.msg import TransformStamped
-from tf2_geometry_msgs import do_transform_pose
-from geometry_msgs.msg import PoseStamped
-import math
-
-class SelfPositionSerial(Node):
-    def __init__(self):
-        super().__init__('self_position_serial')
-        
-        # 串口配置
-        self.serial_port = '/dev/ttyUSB1'  # 根据实际串口设备调整
-        self.baud_rate = 115200
-        
-        try:
-            self.serial_conn = serial.Serial(
-                port=self.serial_port,
-                baudrate=self.baud_rate,
-                timeout=1
-            )
-            self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
-        except Exception as e:
-            self.get_logger().error(f'Failed to open serial port: {e}')
-            return
-        
-        # TF监听器
-        self.tf_buffer = tf2_ros.Buffer()
-        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
-        
-        # 定时器，定期发送位置信息
-        self.timer = self.create_timer(0.1, self.send_position)  # 10Hz
-        
-        self.get_logger().info('Self position serial node started')
-    
-    def send_position(self):
-        try:
-            # 获取base_link在map下的位置
-            transform = self.tf_buffer.lookup_transform(
-                'map', 'base_link', rclpy.time.Time())
-            
-            # 提取位置和朝向
-            x = transform.transform.translation.x
-            y = transform.transform.translation.y
-            z = transform.transform.translation.z
-            
-            # 计算yaw角（从四元数）
-            quat = transform.transform.rotation
-            yaw = math.atan2(2.0 * (quat.w * quat.z + quat.x * quat.y),
-                           1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z))
-            
-            self.get_logger().debug(f'Position - x: {x:.3f}, y: {y:.3f}, z: {z:.3f}, yaw: {yaw:.3f}')
-            
-            # 构建发送数据包
-            # 格式: 包头(0xFF) + x(4字节) + y(4字节) + z(4字节) + yaw(4字节) + 校验和(1字节) + 包尾(0xFE)
-            packet = bytearray()
-            packet.append(0xFF)  # 包头
-            packet.extend(struct.pack('<f', x))    # x坐标
-            packet.extend(struct.pack('<f', y))    # y坐标
-            packet.extend(struct.pack('<f', z))    # z坐标
-            packet.extend(struct.pack('<f', yaw))  # yaw角
-            
-            # 计算校验和（数据部分的异或校验）
-            checksum = 0
-            for i in range(1, len(packet)):
-                checksum ^= packet[i]
-            packet.append(checksum)
-            
-            packet.append(0xFE)  # 包尾
-            
-            # 发送数据
-            self.serial_conn.write(packet)
-            self.get_logger().debug(f'Sent position packet: {packet.hex()}')
-            
-        except Exception as e:
-            self.get_logger().warn(f'Failed to get transform or send data: {e}')
-    
-    def __del__(self):
-        if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
-            self.serial_conn.close()
-            self.get_logger().info('Serial port closed')
-
-def main(args=None):
-    rclpy.init(args=args)
-    
-    try:
-        node = SelfPositionSerial()
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        if rclpy.ok():
-            rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/rm_driver/rm_serial_driver/script/receive_and_pub.py

@@ -1,241 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-import serial
-import struct
-import tf2_ros
-from geometry_msgs.msg import TransformStamped
-from rm_msgs.msg import DataAim
-import math
-import threading
-from rclpy.executors import MultiThreadedExecutor
-
-class ReceiveAndPubNode(Node):
-    def __init__(self):
-        super().__init__('receive_and_pub')
-        
-        # 串口配置
-        self.receive_port = '/dev/ttyUSB2'  # 接收串口
-        self.send_port = '/dev/ttyUSB3'     # 发送串口
-        self.baud_rate = 115200
-        
-        try:
-            # 接收串口
-            self.receive_serial = serial.Serial(
-                port=self.receive_port,
-                baudrate=self.baud_rate,
-                timeout=0.1
-            )
-            # 发送串口
-            self.send_serial = serial.Serial(
-                port=self.send_port,
-                baudrate=self.baud_rate,
-                timeout=1
-            )
-            self.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port}')
-        except Exception as e:
-            self.get_logger().error(f'Failed to open serial ports: {e}')
-            return
-        
-        # TF广播器和监听器
-        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
-        self.tf_buffer = tf2_ros.Buffer()
-        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
-        
-        # 订阅瞄准数据
-        self.subscription = self.create_subscription(
-            DataAim,
-            '/chassis/data_aim',
-            self.aim_callback,
-            10
-        )
-        
-        # 存储接收到的位置信息和瞄准数据
-        self.received_position = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'valid': False}
-        self.aim_data = {'yaw': 0.0, 'distance': 0.0, 'valid': False}
-        
-        # 启动接收线程
-        self.receive_thread = threading.Thread(target=self.receive_position_thread)
-        self.receive_thread.daemon = True
-        self.receive_thread.start()
-        
-        # 定时发送数据
-        self.send_timer = self.create_timer(0.1, self.send_target_data)  # 10Hz
-        
-        self.get_logger().info('Receive and pub node started')
-    
-    def receive_position_thread(self):
-        """接收位置信息的线程"""
-        buffer = bytearray()
-        while rclpy.ok():
-            try:
-                if self.receive_serial.in_waiting > 0:
-                    data = self.receive_serial.read(self.receive_serial.in_waiting)
-                    buffer.extend(data)
-                    
-                    # 查找完整的数据包
-                    while len(buffer) >= 22:  # 最小包长度：包头(1) + 数据(16) + 校验(1) + 包尾(1) = 19
-                        # 查找包头
-                        start_idx = buffer.find(0xFF)
-                        if start_idx == -1:
-                            break
-                        
-                        # 移除包头前的数据
-                        if start_idx > 0:
-                            buffer = buffer[start_idx:]
-                        
-                        # 检查包长度
-                        if len(buffer) < 22:
-                            break
-                        
-                        # 查找包尾
-                        if buffer[21] == 0xFE:
-                            # 校验数据
-                            checksum = 0
-                            for i in range(1, 20):
-                                checksum ^= buffer[i]
-                            
-                            if checksum == buffer[20]:
-                                # 解析数据
-                                x = struct.unpack('<f', buffer[1:5])[0]
-                                y = struct.unpack('<f', buffer[5:9])[0]
-                                z = struct.unpack('<f', buffer[9:13])[0]
-                                yaw = struct.unpack('<f', buffer[13:17])[0]
-                                
-                                self.received_position = {
-                                    'x': x, 'y': y, 'z': z, 'yaw': yaw, 'valid': True
-                                }
-                                
-                                # 发布tf变换
-                                self.publish_r2_transform(x, y, z, yaw)
-                                
-                                self.get_logger().debug(f'Received position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
-                            else:
-                                self.get_logger().warn('Checksum error')
-                            
-                            # 移除已处理的数据包
-                            buffer = buffer[22:]
-                        else:
-                            # 包尾不匹配，移除包头继续查找
-                            buffer = buffer[1:]
-                            
-            except Exception as e:
-                self.get_logger().error(f'Error in receive thread: {e}')
-    
-    def publish_r2_transform(self, x, y, z, yaw):
-        """发布R2到map的tf变换"""
-        try:
-            t = TransformStamped()
-            t.header.stamp = self.get_clock().now().to_msg()
-            t.header.frame_id = 'map'
-            t.child_frame_id = 'R2'
-            
-            t.transform.translation.x = x
-            t.transform.translation.y = y
-            t.transform.translation.z = z
-            
-            # 将yaw角转换为四元数
-            t.transform.rotation.x = 0.0
-            t.transform.rotation.y = 0.0
-            t.transform.rotation.z = math.sin(yaw / 2.0)
-            t.transform.rotation.w = math.cos(yaw / 2.0)
-            
-            self.tf_broadcaster.sendTransform(t)
-            
-        except Exception as e:
-            self.get_logger().error(f'Error publishing tf: {e}')
-    
-    def aim_callback(self, msg):
-        """接收瞄准数据"""
-        try:
-            self.aim_data = {
-                'yaw': msg.yaw,
-                'distance': msg.distance,
-                'valid': True
-            }
-            self.get_logger().debug(f'Received aim data: yaw={msg.yaw:.3f}, distance={msg.distance:.3f}')
-        except Exception as e:
-            self.get_logger().error(f'Error in aim callback: {e}')
-    
-    def get_r2_target_data(self):
-        """获取R2的瞄准数据"""
-        try:
-            if not self.received_position['valid']:
-                return 0.0, 0.0
-            
-            # 这里可以根据R2的位置计算目标数据
-            # 简化处理：返回R2的yaw角和到原点的距离
-            r2_yaw = self.received_position['yaw']
-            r2_distance = math.sqrt(
-                self.received_position['x']**2 + 
-                self.received_position['y']**2
-            )
-            
-            return r2_yaw, r2_distance
-            
-        except Exception as e:
-            self.get_logger().error(f'Error getting R2 target data: {e}')
-            return 0.0, 0.0
-    
-    def send_target_data(self):
-        """发送目标数据"""
-        try:
-            # 获取原始目标数据
-            if self.aim_data['valid']:
-                original_yaw = self.aim_data['yaw']
-                original_distance = self.aim_data['distance']
-            else:
-                original_yaw = 0.0
-                original_distance = 0.0
-            
-            # 获取R2目标数据
-            r2_yaw, r2_distance = self.get_r2_target_data()
-            
-            # 构建发送数据包
-            # 格式: 包头(0xFF) + original_yaw(4字节) + original_distance(4字节) + r2_yaw(4字节) + r2_distance(4字节) + 校验和(1字节) + 包尾(0xFE)
-            packet = bytearray()
-            packet.append(0xFF)  # 包头
-            packet.extend(struct.pack('<f', original_yaw))      # 原始yaw
-            packet.extend(struct.pack('<f', original_distance)) # 原始distance
-            packet.extend(struct.pack('<f', r2_yaw))            # R2 yaw
-            packet.extend(struct.pack('<f', r2_distance))       # R2 distance
-            
-            # 计算校验和
-            checksum = 0
-            for i in range(1, len(packet)):
-                checksum ^= packet[i]
-            packet.append(checksum)
-            
-            packet.append(0xFE)  # 包尾
-            
-            # 发送数据
-            self.send_serial.write(packet)
-            self.get_logger().debug(f'Sent target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.3f})')
-            
-        except Exception as e:
-            self.get_logger().error(f'Error sending target data: {e}')
-    
-    def __del__(self):
-        if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
-            self.receive_serial.close()
-        if hasattr(self, 'send_serial') and self.send_serial.is_open:
-            self.send_serial.close()
-        self.get_logger().info('Serial ports closed')
-
-def main(args=None):
-    rclpy.init(args=args)
-    
-    try:
-        node = ReceiveAndPubNode()
-        executor = MultiThreadedExecutor()
-        executor.add_node(node)
-        executor.spin()
-    except KeyboardInterrupt:
-        pass
-    finally:
-        if rclpy.ok():
-            rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/rm_driver/rm_serial_driver/script/slect.py

@@ -0,0 +1,65 @@
+import rclpy
+from rclpy.node import Node
+from rm_msgs.msg import MoveGoal
+from geometry_msgs.msg import TransformStamped
+import tf2_ros
+import sys
+
+# 预设多组参数
+MAP_CONFIGS = {
+    "map":  {"split_x": 7.155,   "goal1": (0.65, 3.95),  "goal2": (13.66, 3.73)},
+    "map1": {"split_x": 7.085,   "goal1": (0.63, -3.40),     "goal2": (13.60, -3.42)},
+    "map2": {"split_x": 7.075,   "goal1": (0.57, -3.34),    "goal2": (13.58, -3.35)},
+}
+
+class AimSelector(Node):
+    def __init__(self, config):
+        super().__init__('aim_selector')
+        self.split_x = config["split_x"]
+        self.goal1_x, self.goal1_y = config["goal1"]
+        self.goal2_x, self.goal2_y = config["goal2"]
+        self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        self.timer = self.create_timer(0.05, self.timer_callback)
+
+    def timer_callback(self):
+        try:
+            trans: TransformStamped = self.tf_buffer.lookup_transform(
+                'map', 'base_link', rclpy.time.Time())
+            x = trans.transform.translation.x
+            self.get_logger().info(f'base_link x in map: {x:.2f}')
+            msg = MoveGoal()
+            if x < self.split_x:
+                msg.x = self.goal1_x
+                msg.y = self.goal1_y
+            else:
+                msg.x = self.goal2_x
+                msg.y = self.goal2_y
+            msg.angle = 0.0
+            msg.max_speed = 0.0
+            msg.tolerance = 0.1
+            msg.rotor = False
+            self.publisher.publish(msg)
+        except Exception as e:
+            self.get_logger().warn(f'Failed to get transform: {e}')
+
+def main(args=None):
+    # 读取命令行参数
+    if len(sys.argv) < 2:
+        print("用法: python slect.py [map|map1|map2]")
+        sys.exit(1)
+    map_key = sys.argv[1]
+    if map_key not in MAP_CONFIGS:
+        print(f"未知参数: {map_key}")
+        sys.exit(1)
+    config = MAP_CONFIGS[map_key]
+
+    rclpy.init(args=args)
+    node = AimSelector(config)
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rm_localization/icp_registration/config/icp.yaml

@@ -8,7 +8,7 @@
     odom_frame_id: "odom"
     laser_frame_id: "livox_frame"
     pointcloud_topic: "/livox/lidar/pointcloud"
-    thresh: 0.1
+    thresh: 0.1 
     xy_offset: 0.2
     yaw_offset: 30.0
     yaw_resolution: 10.0

src/rm_nav_bringup/config/reality/fastlio_mid360_real.yaml

@@ -7,7 +7,7 @@
         filter_size_map: 0.5
         cube_side_length: 1000.0
         runtime_pos_log_enable: false
-        map_file_path: "src/rm_nav_bringup/PCD/RC2025.pcd"
+        map_file_path: "src/rm_nav_bringup/PCD/1.pcd"
 
         common:
             lid_topic:  "/livox/lidar"

src/rm_nav_bringup/config/reality/mapper_params_online_async_real.yaml

@@ -1,4 +1,4 @@
-slam_toolbox:
+    slam_toolbox:
   ros__parameters:
 
     # Plugin params

src/rm_nav_bringup/config/reality/measurement_params_real.yaml

@@ -1,3 +1,3 @@
 base_link2livox_frame:
-  xyz: "\"0.037 -0.354 0.41\""
+  xyz: "\"0.00906 -0.33 0.41\""
   rpy: "\"0.0  0.0  0.0\""

src/rm_nav_bringup/config/rm_navigation/costmap_converter/.gitlab-ci.yml

@@ -1,23 +0,0 @@
-build-kinetic:
-  variables:
-    ROSDISTRO: "kinetic"
-    CATKIN_WS: "/root/catkin_ws"
-  stage: build
-  image: osrf/ros:$ROSDISTRO-desktop-full
-  before_script: 
-    - apt-get -qq update
-    - apt-get -qq install git-core python-catkin-tools curl
-    - mkdir -p $CATKIN_WS/src
-    - cp -a . $CATKIN_WS/src/
-    - cd $CATKIN_WS
-    - rosdep update
-    - rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false
-  script: 
-    - source /ros_entrypoint.sh
-    - cd $CATKIN_WS
-    - catkin build -i -s --no-status
-  only:
-    - master
-  tags:
-    - ubuntu
-    - docker

src/rm_nav_bringup/config/rm_navigation/costmap_converter/.travis.yml

@@ -1,109 +0,0 @@
-# Generic .travis.yml file for running continuous integration on Travis-CI with
-# any ROS package.
-#
-# This installs ROS on a clean Travis-CI virtual machine, creates a ROS
-# workspace, resolves all listed dependencies, and sets environment variables
-# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are
-# no compilation errors), and runs all the tests. If any of the compilation/test
-# phases fail, the build is marked as a failure.
-#
-# We handle two types of package dependencies:
-#   - packages (ros and otherwise) available through apt-get. These are installed
-#     using rosdep, based on the information in the ROS package.xml.
-#   - dependencies that must be checked out from source. These are handled by
-#     'wstool', and should be listed in a file named dependencies.rosinstall.
-#
-# There are two variables you may want to change:
-#   - ROS_DISTRO (default is indigo). Note that packages must be available for
-#     ubuntu 14.04 trusty.
-#   - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo
-#     root). This should list all necessary repositories in wstool format (see
-#     the ros wiki). If the file does not exists then nothing happens.
-#
-# See the README.md for more information.
-#
-# Author: Felix Duvallet <felixd@gmail.com>
-
-# NOTE: The build lifecycle on Travis.ci is something like this:
-#    before_install
-#    install
-#    before_script
-#    script
-#    after_success or after_failure
-#    after_script
-#    OPTIONAL before_deploy
-#    OPTIONAL deploy
-#    OPTIONAL after_deploy
-
-################################################################################
-
-# Use ubuntu trusty (14.04) with sudo privileges.
-dist: trusty
-sudo: required
-language:
-  - generic
-cache:
-  - apt
-
-# Configuration variables. All variables are global now, but this can be used to
-# trigger a build matrix for different ROS distributions if desired.
-env:
-  global:
-    - ROS_CI_DESKTOP="`lsb_release -cs`"  # e.g. [precise|trusty|...]
-    - CI_SOURCE_PATH=$(pwd)
-    - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
-    - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
-    - ROS_PARALLEL_JOBS='-j8 -l6'
-  matrix:
-    - ROS_DISTRO=indigo
-    - ROS_DISTRO=jade
-
-
-################################################################################
-
-# Install system dependencies, namely a very barebones ROS setup.
-before_install:
-  - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
-  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-  - sudo apt-get update -qq
-  - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin
-  - source /opt/ros/$ROS_DISTRO/setup.bash
-  # Prepare rosdep to install dependencies.
-  - sudo rosdep init
-  - rosdep update
-
-# Create a catkin workspace with the package under integration.
-install:
-  - mkdir -p ~/catkin_ws/src
-  - cd ~/catkin_ws/src
-  - catkin_init_workspace
-  # Create the devel/setup.bash (run catkin_make with an empty workspace) and
-  # source it to set the path variables.
-  - cd ~/catkin_ws
-  - catkin_make
-  - source devel/setup.bash
-  # Add the package under integration to the workspace using a symlink.
-  - cd ~/catkin_ws/src
-  - ln -s $CI_SOURCE_PATH .
-
-# Install all dependencies, using wstool and rosdep.
-# wstool looks for a ROSINSTALL_FILE defined in the environment variables.
-before_script:
-  # source dependencies: install using wstool.
-  - cd ~/catkin_ws/src
-  - wstool init
-  - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
-  - wstool up
-  # package depdencies: install using rosdep.
-  - cd ~/catkin_ws
-  - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
-
-# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to
-# catkin_make.
-script:
-  - cd ~/catkin_ws
-  - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )
-  # Testing: Use both run_tests (to see the output) and test (to error out).
-  - catkin_make run_tests  # This always returns 0, but looks pretty.
-  - catkin_make test  # This will return non-zero if a test fails.
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/README.md

@@ -1,30 +0,0 @@
-costmap_converter ROS Package
-=============================
-
-A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types
-
-Build status of the *master* branch:
-- ROS Buildfarm Kinetic: [![Kinetic Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__costmap_converter__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__costmap_converter__ubuntu_xenial_amd64/)
-- ROS Buildfarm Indigo: [![Indigo Build Status](http://build.ros.org/buildStatus/icon?job=Idev__costmap_converter__ubuntu_trusty_amd64)](http://build.ros.org/job/Idev__costmap_converter__ubuntu_trusty_amd64/)
-
-
-### Contributors
-
-- Christoph Rösmann
-- Franz Albers (*CostmapToDynamicObstacles* plugin)
-- Otniel Rinaldo
-
-
-### License
-
-The *costmap_converter* package is licensed under the BSD license.
-It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.
-
-Some third-party dependencies are included that are licensed under different terms:
- - *MultitargetTracker*, GNU GPLv3, https://github.com/Smorodov/Multitarget-tracker
-   (partially required for the *CostmapToDynamicObstacles* plugin)
-
-All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.
-
-
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CHANGELOG.rst

@@ -1,105 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package costmap_converter
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.1.2 (2020-06-22)
-------------------
-* OpenCV 4 compatibility fix
-* Contributors: daviddudas
-
-0.1.1 (2020-01-25)
-------------------
-* Fixed ament plugin export
-* Revert release-mode for cmake build
-* Contributors: Christoph Rösmann
-
-0.1.0 (2020-01-23)
-------------------
-* Port to ROS2 (thanks to Vinnam Kim and stevemacenski)
-* Messages moved to a separate package
-* Contributors: Christoph Rösmann, Vinnam Kim, stevemacenski
-
-0.0.12 (2019-12-02)
--------------------
-* CostmapToPolygons: Simplification of the polygon by Douglas-Peucker algorithm (reduces the density of points in the polygon).
-* Bugfixes
-* Contributors: Rainer Kümmerle
-
-0.0.11 (2019-10-26)
--------------------
-* rostest integration to avoid running a roscore separately for unit testing
-* Contributors: Christoph Rösmann
-
-0.0.10 (2019-10-26)
--------------------
-* Runtime improvements for CostmapToPolygonsDBSMCCH (`#12 <https://github.com/rst-tu-dortmund/costmap_converter/issues/12>`_)
-  * Grid lookup for regionQuery
-  * use a grid structure for looking up nearest neighbors
-  * parameters in a struct
-  * guard the parameters by drawing a copy from dynamic reconfigure
-  * Adding some test cases for regionQuery and dbScan
-  * Avoid computing sqrt at the end of convexHull2
-  * Add doxygen comments for the neighbor lookup
-  * Change the param read to one liners
-  * Add test on empty map for dbScan
-* Contributors: Rainer Kümmerle
-
-0.0.9 (2018-05-28)
-------------------
-* Moved plugin loader for static costmap conversion to BaseCostmapToDynamicObstacles.
-  The corresponding ROS parameter `static_converter_plugin` is now defined in the CostmapToDynamicObstacles namespace. 
-* Contributors: Christoph Rösmann
-
-0.0.8 (2018-05-17)
-------------------
-* Standalone converter subscribes now to costmap updates. Fixes `#1 <https://github.com/rst-tu-dortmund/costmap_converter/issues/1>`_
-* Adds radius field for circular obstacles to ObstacleMsg
-* Stacked costmap conversion (`#7 <https://github.com/rst-tu-dortmund/costmap_converter/issues/7>`_).
-  E.g., it is now possible combine a dynamic obstacle and static obstacle converter plugin.
-* Contributors: Christoph Rösmann, Franz Albers
-
-0.0.7 (2017-09-20)
-------------------
-* Fixed some compilation issues (C++11 compiler flags and opencv2 on indigo/jade).
-* Dynamic obstacle plugin: obstacle velocity is now published for both x and y coordinates rather than the absolute value
-
-0.0.6 (2017-09-18)
-------------------
-* This pull request adds the costmap to dynamic obstacles plugin (written by Franz Albers).
-  It detects the dynamic foreground of the costmap (based on the temporal evolution of the costmap)
-  including blobs representing the obstacles. Furthermore, Kalman-based tracking is applied to estimate
-  the current velocity for each obstacle.
-  **Note, this plugin is still experimental.**
-* New message types are introduced: costmap\_converter::ObstacleMsg and costmap\_converter::ObstacleArrayMsg.
-  These types extend the previous polygon representation by additional velocity, orientation and id information.
-* The API has been extended to provide obstacles via the new ObstacleArrayMsg type instead of vector of polygons.
-* Contributors: Franz Albers, Christoph Rösmann
-
-0.0.5 (2016-02-01)
-------------------
-* Major changes regarding the line detection based on the convex hull
-  (it should be much more robust now).
-* Concave hull plugin added.
-* The cluster size can now be limited from above using a specific parameter.
-  This implicitly avoids large clusters forming a 'L' or 'U'.
-* All parameters can now be adjusted using dynamic_reconfigure (rqt_reconfigure).
-* Some parameter names changed.
-* Line plugin based on ransac: line inliers must now be placed inbetween the start and end of a line.
-
-0.0.4 (2016-01-11)
-------------------
-* Fixed conversion from map to world coordinates if the costmap is not quadratic.
-
-0.0.3 (2015-12-23)
-------------------
-* The argument list of the initialize method requires a nodehandle from now on. This facilitates the handling of parameter namespaces for multiple instantiations of the plugin.
-* This change is pushed immediately as a single release to avoid API breaks (since version 0.0.2 is not on the official repos up to now).
-
-0.0.2 (2015-12-22)
-------------------
-* Added a plugin for converting the costmap to lines using ransac
-
-0.0.1 (2015-12-21)
-------------------
-* First release of the package including a pluginlib interface, two plugins (costmap to polygons and costmap to lines) and a standalone conversion node.
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CMakeLists.txt

@@ -1,97 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(costmap_converter)
-
-# Set to Release in order to speed up the program significantly
-set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
-
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-find_package(ament_cmake REQUIRED)
-find_package(class_loader REQUIRED)
-find_package(costmap_converter_msgs REQUIRED)
-find_package(cv_bridge REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(nav2_costmap_2d REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(OpenCV REQUIRED)
-find_package(pluginlib REQUIRED)
-find_package(rclcpp REQUIRED)
-
-set(dependencies
-    class_loader
-    costmap_converter_msgs
-    cv_bridge
-    geometry_msgs
-    nav2_costmap_2d
-    tf2
-    tf2_geometry_msgs
-    tf2_ros
-    OpenCV
-    pluginlib
-    rclcpp
-)
-
-include_directories(
-  include
-)
-
-add_library(costmap_converter SHARED
-  src/costmap_to_polygons.cpp 
-  src/costmap_to_polygons_concave.cpp
-  src/costmap_to_lines_convex_hull.cpp
-  src/costmap_to_lines_ransac.cpp
-  src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp
-  src/costmap_to_dynamic_obstacles/background_subtractor.cpp
-  src/costmap_to_dynamic_obstacles/blob_detector.cpp
-  src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp
-  src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp
-  src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp
-)
-
-ament_target_dependencies(costmap_converter 
-    ${dependencies}
-)
-
-add_executable(standalone_converter src/costmap_converter_node.cpp)
-
-target_link_libraries(standalone_converter
-    costmap_converter
-)
-ament_target_dependencies(standalone_converter
-  ${dependencies}
-)
-
-install(TARGETS costmap_converter
-   DESTINATION lib
-)
-install(TARGETS standalone_converter
-   DESTINATION bin
-)
-
-install(DIRECTORY include/
-   DESTINATION include/
-)
-
-ament_export_include_directories(include)
-ament_export_libraries(costmap_converter)
-ament_export_dependencies(${dependencies})
-pluginlib_export_plugin_description_file(costmap_converter costmap_converter_plugins.xml)
-
-if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
-  ament_add_gtest(test_costmap_polygons test/costmap_polygons.cpp)
-  if(TARGET test_costmap_polygons)
-    target_link_libraries(test_costmap_polygons costmap_converter)  
-  endif()
-  ament_target_dependencies(test_costmap_polygons ${dependencies})
-endif()
-
-ament_package()

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg

@@ -1,136 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-# For integers and doubles:
-#       Name                    Type      Reconfiguration level
-#       Description
-#       Default  Min  Max
-
-##################################################################
-###################### Foreground detection ######################
-gen.add("alpha_slow", double_t, 0,
-	"Foreground detection: Learning rate of the slow filter",
-	0.3, 0.0, 1.0)
-
-gen.add("alpha_fast", double_t, 0,
-	"Foreground detection: Learning rate of the fast filter",
-	0.85, 0.0, 1.0)
-
-gen.add("beta", double_t, 0,
-	"Foreground detection: Weighting coefficient between a pixels value and the mean of its nearest neighbors",
-	0.85, 0.0, 1.0)
-
-gen.add("min_sep_between_slow_and_fast_filter", int_t, 0,
-	"Foreground detection: Minimal difference between the fast and the slow filter to recognize a obstacle as dynamic",
-	80, 0, 255)
-
-gen.add("min_occupancy_probability", int_t, 0,
-	"Foreground detection: Minimal value of the fast filter to recognize a obstacle as dynamic",
-	180, 0, 255)
-
-gen.add("max_occupancy_neighbors", int_t, 0,
-	"Foreground detection: Maximal mean value of the nearest neighbors of a pixel in the slow filter",
-	80, 0, 255)
-
-gen.add("morph_size", int_t, 0,
-	"Foreground detection: Size of the structuring element for the closing operation",
-	1, 0, 10)
-
-gen.add("publish_static_obstacles", bool_t, 0,
-        "Include static obstacles as single-point polygons",
-        True)
-
-############################################################
-###################### Blob detection ######################
-
-# These parameters are commented out, because the input image for the blob detection is already binary -> irrelevant
-#gen.add("threshold_step", double_t, 0,
-#	"Blob detection: Distance between neighboring thresholds",
-#	256.0, 0.0, 256.0)
-#
-#gen.add("min_threshold", double_t, 0,
-#	"Blob detection: Convert the source image to binary images by applying several thresholds, starting at min_threshold",
-#	1, 0, 255)
-#
-#gen.add("max_threshold", double_t, 0,
-#	"Blob detection: Convert the source image to binary images by applying several thresholds, ending at max_threshold",
-#	255, 0, 255)
-#
-#gen.add("min_repeatability", int_t, 0,
-#	"Blob detection: Minimal number of detections of a blob in the several thresholds to be considered as real blob",
-#	1, 1, 10)
-#
-gen.add("min_distance_between_blobs", double_t, 0,
-	"Blob detection: Minimal distance between centers of two blobs to be considered as seperate blobs",
-	10, 0.0, 300.0)
-
-gen.add("filter_by_area", bool_t, 0,
-	"Blob detection: Filter blobs based on number of pixels",
-	True)
-
-gen.add("min_area", int_t, 0,
-	"Blob detection: Minimal number of pixels a blob consists of",
-	3, 0, 300)
-
-gen.add("max_area", int_t, 0,
-	"Blob detection: Maximal number of pixels a blob consists of",
-	300, 0, 300)
-
-gen.add("filter_by_circularity", bool_t, 0,
-	"Blob detection: Filter blobs based on their circularity",
-	True)
-
-gen.add("min_circularity", double_t, 0,
-	"Blob detection: Minimal circularity value (0 in case of a line)",
-	0.2, 0.0, 1.0)
-
-gen.add("max_circularity", double_t, 0,
-	"Blob detection: Maximal circularity value (1 in case of a circle)",
-	1.0, 0.0, 1.0)
-
-gen.add("filter_by_inertia", bool_t, 0,
-	"Blob detection: Filter blobs based on their inertia ratio",
-	True)
-
-gen.add("min_inertia_ratio", double_t, 0,
-	"Blob detection: Minimal inertia ratio",
-	0.2, 0.0, 1.0)
-
-gen.add("max_inertia_ratio", double_t, 0,
-	"Blob detection: Maximal inertia ratio",
-	1.0, 0.0, 1.0)
-
-gen.add("filter_by_convexity", bool_t, 0,
-	"Blob detection: Filter blobs based on their convexity (Blob area / area of its convex hull)",
-	False)
-
-gen.add("min_convexity", double_t, 0,
-	"Blob detection: Minimum convexity ratio",
-	0.0, 0.0, 1.0)
-
-gen.add("max_convexity", double_t, 0,
-	"Blob detection: Maximal convexity ratio",
-	1.0, 0.0, 1.0)
-
-################################################################
-#################### Tracking ##################################
-gen.add("dt", double_t, 0,
-	"Tracking: Time for one timestep of the kalman filter",
-	0.2, 0.1, 3.0)
-
-gen.add("dist_thresh", double_t, 0,
-	"Tracking: Maximum distance between two points to be considered in the assignment problem",
-	20.0, 0.0, 150.0)
-
-gen.add("max_allowed_skipped_frames", int_t, 0,
-	"Tracking: Maximum number of frames a object is tracked while it is not seen",
-	3, 0, 10)
-
-gen.add("max_trace_length", int_t, 0,
-	"Tracking: Maximum number of Points in a objects trace",
-	10, 1, 100)
-
-exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToDynamicObstacles"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg

@@ -1,41 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-# For integers and doubles:
-#       Name                    Type      Reconfiguration level
-#       Description
-#       Default  Min  Max
-
-
-gen.add("cluster_max_distance", double_t, 0, 
-	"Parameter for DB_Scan, maximum distance to neighbors [m]",
-	0.4, 0.0, 10.0)
-
-gen.add("cluster_min_pts", int_t, 0, 
-	"Parameter for DB_Scan: minimum number of points that define a cluster",
-	2, 1, 20)
-
-gen.add("cluster_max_pts", int_t, 0, 
-  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
-  30, 2, 200)
-
-gen.add("convex_hull_min_pt_separation",   double_t,   0,
-	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
-	0.1, 0.0, 10.0)	
-
-gen.add("support_pts_max_dist",   double_t,   0,
-	"Minimum distance from a point to the line to be counted as support point",
-	0.3, 0.0, 10.0)
-
-gen.add("support_pts_max_dist_inbetween",   double_t,   0,
-  "A line is only defined, if the distance between two consecutive support points is less than this treshold. Set to 0 in order to deactivate this check.",
-  1.0, 0.0, 10.0)
-
-gen.add("min_support_pts", int_t, 0, 
-	"Minimum number of support points to represent a line",
-	2, 0, 50)	
-
-exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSMCCH"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg

@@ -1,54 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-# For integers and doubles:
-#       Name                    Type      Reconfiguration level
-#       Description
-#       Default  Min  Max
-
-
-gen.add("cluster_max_distance", double_t, 0, 
-	"Parameter for DB_Scan, maximum distance to neighbors [m]",
-	0.4, 0.0, 10.0)
-
-gen.add("cluster_min_pts", int_t, 0, 
-	"Parameter for DB_Scan: minimum number of points that define a cluster",
-	2, 1, 20)
-
-gen.add("cluster_max_pts", int_t, 0, 
-  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
-  30, 2, 200)
-
-gen.add("ransac_inlier_distance", double_t, 0, 
-	"Maximum distance to the line segment for inliers",
-	0.2, 0.0, 10.0)
-
-gen.add("ransac_min_inliers", int_t, 0, 
-	"Minimum numer of inliers required to form a line",
-	10, 0, 100)
-
-gen.add("ransac_no_iterations", int_t, 0, 
-	"Number of ransac iterations",
-	2000, 1, 10000)
-
-gen.add("ransac_remainig_outliers", int_t, 0, 
-	"Repeat ransac until the number of outliers is as specified here",
-	3, 0, 50)
-
-gen.add("ransac_convert_outlier_pts",   bool_t,   0, 
-	"Convert remaining outliers to single points.",
-	True)
-
-gen.add("ransac_filter_remaining_outlier_pts",   bool_t,   0, 
-	"Filter the interior of remaining outliers and keep only keypoints of their convex hull",
-	False)
-
-gen.add("convex_hull_min_pt_separation",   double_t,   0,
-	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
-	0.1, 0.0, 10.0)	
-
-
-exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSRANSAC"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg

@@ -1,33 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-# For integers and doubles:
-#       Name                    Type      Reconfiguration level
-#       Description
-#       Default  Min  Max
-
-
-gen.add("cluster_max_distance", double_t, 0, 
-	"Parameter for DB_Scan, maximum distance to neighbors [m]",
-	0.4, 0.0, 10.0)
-
-gen.add("cluster_min_pts", int_t, 0, 
-	"Parameter for DB_Scan: minimum number of points that define a cluster",
-	2, 1, 20)
-
-gen.add("cluster_max_pts", int_t, 0, 
-  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
-  30, 2, 200)
-
-gen.add("convex_hull_min_pt_separation",   double_t,   0,
-	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
-	0.1, 0.0, 10.0)	
-
-gen.add("concave_hull_depth",   double_t,   0,
-  "Smaller depth: sharper surface, depth -> high value: convex hull",
-  2.0, 0.0, 100.0) 
-
-exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSConcaveHull"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg

@@ -1,29 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-# For integers and doubles:
-#       Name                    Type      Reconfiguration level
-#       Description
-#       Default  Min  Max
-
-
-gen.add("cluster_max_distance", double_t, 0, 
-	"Parameter for DB_Scan, maximum distance to neighbors [m]",
-	0.4, 0.0, 10.0)
-
-gen.add("cluster_min_pts", int_t, 0, 
-	"Parameter for DB_Scan: minimum number of points that define a cluster",
-	2, 1, 20)
-
-gen.add("cluster_max_pts", int_t, 0, 
-  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
-  30, 2, 200)
-
-gen.add("convex_hull_min_pt_separation",   double_t,   0,
-	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
-	0.1, 0.0, 10.0)	
-
-exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSMCCH"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/costmap_converter_plugins.xml

@@ -1,38 +0,0 @@
-<library path="costmap_converter">
-  
-  <class type="costmap_converter::CostmapToPolygonsDBSMCCH" base_class_type="costmap_converter::BaseCostmapToPolygons">
-  <description>
-      This class converts costmap2d obstacles into a vector of convex polygons. 
-      Clustering is performed with DBScan. Convex polygons are detected using the Monotone Chain Convex Hull algorithm.
-  </description>
-  </class>
-  
-  <class type="costmap_converter::CostmapToLinesDBSMCCH" base_class_type="costmap_converter::BaseCostmapToPolygons">
-  <description>
-      This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). 
-      Clustering is performed with DBScan. Clusters are transformed into convex polygons using the Monotone Chain Convex Hull algorithm.
-      The resulting keypoints are used for possible line candidates.
-      A line is only defined if there exist a specified number of support points between each keypoint pair.
-  </description>
-  </class>
-  
-  <class type="costmap_converter::CostmapToLinesDBSRANSAC" base_class_type="costmap_converter::BaseCostmapToPolygons">
-  <description>
-      This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). 
-      Clustering is performed with DBScan. For each cluster RANSAC is applied sequentally to fit multiple line models.
-  </description>
-  </class>
-  
-  <class type="costmap_converter::CostmapToPolygonsDBSConcaveHull" base_class_type="costmap_converter::BaseCostmapToPolygons">
-  <description>
-      This class converts costmap2d obstacles into a vector of non-convex (concave) polygons. 
-  </description>
-  </class>
-
-  <class type="costmap_converter::CostmapToDynamicObstacles" base_class_type="costmap_converter::BaseCostmapToPolygons">
-  <description>
-      This class detects and tracks obstacles from a sequence of costmaps. 
-  </description>
-  </class>
-  
-</library>

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_converter_interface.h

@@ -1,389 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann, Otniel Rinaldo
- *********************************************************************/
-
-#ifndef COSTMAP_CONVERTER_INTERFACE_H_
-#define COSTMAP_CONVERTER_INTERFACE_H_
-
-//#include <costmap_2d/costmap_2d_ros.h>
-#include <mutex>
-#include <memory>
-
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-#include <nav2_costmap_2d/costmap_2d.hpp>
-#include <nav2_costmap_2d/costmap_2d_ros.hpp>
-#include <geometry_msgs/msg/polygon.hpp>
-#include <costmap_converter_msgs/msg/obstacle_array_msg.hpp>
-
-namespace costmap_converter
-{
-  
-//! Typedef for a shared dynamic obstacle container
-typedef costmap_converter_msgs::msg::ObstacleArrayMsg::SharedPtr ObstacleArrayPtr;
-//! Typedef for a shared dynamic obstacle container (read-only access)
-typedef costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr ObstacleArrayConstPtr;
-
-//! Typedef for a shared polygon container 
-typedef std::shared_ptr<std::vector<geometry_msgs::msg::Polygon>> PolygonContainerPtr;
-//! Typedef for a shared polygon container (read-only access)
-typedef std::shared_ptr<const std::vector<geometry_msgs::msg::Polygon>> PolygonContainerConstPtr;
-  
-
-/**
- * @class BaseCostmapToPolygons
- * @brief This abstract class defines the interface for plugins that convert the costmap into polygon types
- * 
- * Plugins must accept a nav2_costmap_2d::Costmap2D datatype as information source.
- * The interface provides two different use cases:
- * 1. Manual call to conversion routines: setCostmap2D(), compute() and getPolygons() 
- *    (in subsequent calls setCostmap2D() can be substituted by updateCostmap2D())
- * 2. Repeatedly process costmap with a specific rate (startWorker() and stopWorker()):
- *    Make sure that the costmap is valid while the worker is active (you can specify your own spinner or activate a threaded spinner).
- *    Costmaps can be obtained by invoking getPolygons().
- */
-class BaseCostmapToPolygons
-{
-public: 
-  
-    /**
-     * @brief Initialize the plugin
-     * @param nh Nodehandle that defines the namespace for parameters
-     */
-    virtual void initialize(rclcpp::Node::SharedPtr nh) {
-      nh_ = nh;
-    }
-    
-    /**
-     * @brief Destructor
-     */
-    virtual ~BaseCostmapToPolygons() 
-    {
-      stopWorker();
-    }
-
-    
-    /**
-     * @brief Pass a pointer to the costap to the plugin.
-     * @warning The plugin should handle the costmap's mutex locking.
-     * @sa updateCostmap2D
-     * @param costmap Pointer to the costmap2d source
-     */
-    virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap) = 0;
-    
-    /**
-     * @brief Get updated data from the previously set Costmap2D
-     * @warning The plugin should handle the costmap's mutex locking.
-     * @sa setCostmap2D
-     */
-    virtual void updateCostmap2D() = 0;
-    
-     /**
-     * @brief This method performs the actual work (conversion of the costmap to polygons)
-     */
-    virtual void compute() = 0;
-    
-    /**
-     * @brief Get a shared instance of the current polygon container
-     *
-     * If this method is not implemented by any subclass (plugin) the returned shared
-     * pointer is empty.
-     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
-     * @warning The underlying plugin must ensure that this method is thread safe.
-     * @return Shared instance of the current polygon container
-     */
-    virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();}
-
-  /**
-   * @brief Get a shared instance of the current obstacle container
-   * If this method is not overwritten by the underlying plugin, the obstacle container just imports getPolygons().
-   * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
-   * @warning The underlying plugin must ensure that this method is thread safe.
-   * @return Shared instance of the current obstacle container
-   * @sa getPolygons
-   */
-    virtual ObstacleArrayConstPtr getObstacles()
-    {
-      ObstacleArrayPtr obstacles = std::make_shared<costmap_converter_msgs::msg::ObstacleArrayMsg>();
-      PolygonContainerConstPtr polygons = getPolygons();
-      if (polygons)
-      {
-        for (const geometry_msgs::msg::Polygon& polygon : *polygons)
-        {
-          obstacles->obstacles.emplace_back();
-          obstacles->obstacles.back().polygon = polygon;
-        }
-      }
-      return obstacles;
-    }
-
-    /**
-     * @brief Set name of robot's odometry topic
-     *
-     * Some plugins might require odometry information
-     * to compensate the robot's ego motion
-     * @param odom_topic topic name
-     */
-    virtual void setOdomTopic(const std::string& odom_topic) { (void)odom_topic; }
-
-    /**
-     * @brief Determines whether an additional plugin for subsequent costmap conversion is specified
-     *
-     * @return false, since all plugins for static costmap conversion are independent
-     */
-    virtual bool stackedCostmapConversion() {return false;}
-
-     /**
-      * @brief Instantiate a worker that repeatedly coverts the most recent costmap to polygons.
-      * The worker is implemented as a timer event that is invoked at a specific \c rate.
-      * The passed \c costmap pointer must be valid at the complete time and must be lockable.
-      * By specifying the argument \c spin_thread the timer event is invoked in a separate
-      * thread and callback queue or otherwise it is called from the global callback queue (of the
-      * node in which the plugin is used).
-      * @param rate The rate that specifies how often the costmap should be updated
-      * @param costmap Pointer to the underlying costmap (must be valid and lockable as long as the worker is active
-      * @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue)
-     */
-    void startWorker(rclcpp::Rate::SharedPtr rate, nav2_costmap_2d::Costmap2D* costmap, bool spin_thread = false)
-    {
-      setCostmap2D(costmap);
-      
-      if (spin_thread_)
-      {
-        {
-          std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
-          need_to_terminate_ = true;
-        }
-        spin_thread_->join();
-        delete spin_thread_;
-      }
-      
-      if (spin_thread)
-      {
-        RCLCPP_DEBUG(nh_->get_logger(), "Spinning up a thread for the CostmapToPolygons plugin");
-        need_to_terminate_ = false;
-        
-        worker_timer_ = nh_->create_wall_timer(
-                    rate->period(),
-                    std::bind(&BaseCostmapToPolygons::workerCallback, this));
-        spin_thread_ = new std::thread(std::bind(&BaseCostmapToPolygons::spinThread, this));
-      }
-      else
-      {
-        worker_timer_ = nh_->create_wall_timer(
-                    rate->period(),
-                    std::bind(&BaseCostmapToPolygons::workerCallback, this));
-        spin_thread_ = nullptr;
-      }
-    }
-    
-    /**
-     * @brief Stop the worker that repeatedly converts the costmap to polygons
-     */
-    void stopWorker()
-    {
-      if (worker_timer_) worker_timer_->cancel();
-      if (spin_thread_)
-      {
-        {
-          std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
-          need_to_terminate_ = true;
-        }
-        spin_thread_->join();
-        delete spin_thread_;
-      }
-    }
-
-protected:
-  
-    /**
-     * @brief Protected constructor that should be called by subclasses
-     */
-    BaseCostmapToPolygons() : //nh_("~costmap_to_polygons"),
-        nh_(nullptr),
-        spin_thread_(nullptr), need_to_terminate_(false) {}
-    
-    /**
-     * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) 
-     */
-    void spinThread()
-    {
-      rclcpp::WallRate r(10);
-      while (rclcpp::ok())
-      {
-        {
-          std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
-          if (need_to_terminate_)
-            break;
-          rclcpp::spin_some(nh_);
-          r.sleep();
-        }
-      }
-    }
-    
-    /**
-     * @brief The callback of the worker that performs the actual work (updating the costmap and converting it to polygons)
-     */
-    void workerCallback()
-    {
-      updateCostmap2D();
-      compute();
-    }
-
-    rclcpp::Logger getLogger() const
-    {
-        return nh_->get_logger();
-    }
-
-    rclcpp::Time now() const
-    {
-        return nh_->now();
-    }
-    
-private:
-  rclcpp::TimerBase::SharedPtr worker_timer_;
-  rclcpp::Node::SharedPtr nh_;
-  std::thread* spin_thread_;
-  std::mutex terminate_mutex_;
-  bool need_to_terminate_;
-};    
-
-
-/**
- * @class BaseCostmapToDynamicPolygons
- * @brief This class extends the BaseCostmapToPolygongs class for dynamic costmap conversion plugins in order to incorporate a subsequent costmap converter plugin for static obstacles
- *
- * This class should not be instantiated.
- */
-class BaseCostmapToDynamicObstacles : public BaseCostmapToPolygons
-{
-public:
-
-  /**
-   * @brief Load underlying static costmap conversion plugin via plugin-loader
-   * @param plugin_name Exact class name of the plugin to be loaded, e.g.
-   *                    costmap_converter::CostmapToPolygonsDBSMCCH
-   * @param nh_parent   NodeHandle which is extended by the namespace of the static conversion plugin
-   */
-  void loadStaticCostmapConverterPlugin(const std::string& plugin_name, rclcpp::Node::SharedPtr nh_parent)
-  {
-    try
-    {
-      static_costmap_converter_ = static_converter_loader_.createSharedInstance(plugin_name);
-
-      if(std::dynamic_pointer_cast<BaseCostmapToDynamicObstacles>(static_costmap_converter_))
-      {
-        throw pluginlib::PluginlibException("The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin.");
-      }
-//      std::string raw_plugin_name = static_converter_loader_.getName(plugin_name);
-      static_costmap_converter_->initialize(nh_parent);
-      setStaticCostmapConverterPlugin(static_costmap_converter_);
-      RCLCPP_INFO(getLogger(), "CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles %s loaded.", plugin_name.c_str());
-    }
-    catch(const pluginlib::PluginlibException& ex)
-    {
-      RCLCPP_WARN(getLogger(), "CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. "
-                               "Continuing without subsequent conversion of static obstacles. Error message: %s", ex.what());
-      static_costmap_converter_.reset();
-    }
-  }
-
-  /**
-   * @brief Set the underlying plugin for subsequent costmap conversion of the static background of the costmap
-   * @param static_costmap_converter shared pointer to the static costmap conversion plugin
-   */
-  void setStaticCostmapConverterPlugin(std::shared_ptr<BaseCostmapToPolygons> static_costmap_converter)
-  {
-    static_costmap_converter_ = static_costmap_converter;
-  }
-
-  /**
-   * @brief Set the costmap for the underlying plugin
-   * @param static_costmap Costmap2D, which contains the static part of the original costmap
-   */
-  void setStaticCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> static_costmap)
-  {
-    static_costmap_converter_->setCostmap2D(static_costmap.get());
-  }
-
-  /**
-   * @brief Apply the underlying plugin for static costmap conversion
-   */
-  void convertStaticObstacles()
-  {
-    static_costmap_converter_->compute();
-  }
-
-  /**
-   * @brief Get the converted polygons from the underlying plugin
-   * @return Shared instance of the underlying plugins polygon container
-   */
-  PolygonContainerConstPtr getStaticPolygons()
-  {
-    return static_costmap_converter_->getPolygons();
-  }
-
-  /**
-   * @brief Determines whether an additional plugin for subsequent costmap conversion is specified
-   *
-   * @return true, if a plugin for subsequent costmap conversion is specified
-   */
-  bool stackedCostmapConversion()
-  {
-    if(static_costmap_converter_)
-      return true;
-    else
-      return false;
-  }
-
-protected:
-  /**
-   * @brief Protected constructor that should be called by subclasses
-   */
-  BaseCostmapToDynamicObstacles() : static_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), static_costmap_converter_() {}
-
-private:
-  pluginlib::ClassLoader<BaseCostmapToPolygons> static_converter_loader_;
-  std::shared_ptr<BaseCostmapToPolygons> static_costmap_converter_;
-};
-
-
-}
-
-
-
-#endif // end COSTMAP_CONVERTER_INTERFACE_H_

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h

@@ -1,115 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2017
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following code makes use of the OpenCV library.
- * OpenCV is licensed under the terms of the 3-clause BSD License.
- *
- * Authors: Franz Albers, Christoph Rösmann
- *********************************************************************/
-
-#ifndef BACKGROUNDSUBTRACTOR_H_
-#define BACKGROUNDSUBTRACTOR_H_
-
-#include <cv_bridge/cv_bridge.h>
-
-/**
- * @class BackgroundSubtractor
- * @brief Perform background subtraction to extract the "moving" foreground
- *
- * This class is based on OpenCV's background subtraction class cv::BackgroundSubtractor.
- * It has been modified in order to incorporate a specialized bandpass filter.
- *
- * See http://docs.opencv.org/3.2.0/d7/df6/classcv_1_1BackgroundSubtractor.html for the original class.
- */
-class BackgroundSubtractor
-{
-public:
-  struct Params
-  {
-    double alpha_slow; //!< Filter constant (learning rate) of the slow filter part
-    double alpha_fast; //!< Filter constant (learning rate) of the fast filter part
-    double beta;
-    double min_sep_between_fast_and_slow_filter;
-    double min_occupancy_probability;
-    double max_occupancy_neighbors;
-    int morph_size;
-  };
-
-  //! Constructor that accepts a specific parameter configuration
-  BackgroundSubtractor(const Params& parameters);
-
-  /**
-   * @brief Computes a foreground mask
-   * @param[in]  image    Next video frame
-   * @param[out] fg_mask  Foreground mask as an 8-bit binary image
-   * @param[in]  shift_x  Translation along the x axis between the current and previous image
-   * @param[in]  shift_y  Translation along the y axis between the current and previous image
-   */
-  void apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x = 0, int shift_y = 0);
-
-  /**
-   * @brief OpenCV Visualization
-   * @param name  Id/name of the opencv window
-   * @param image Image to be visualized
-   */
-  void visualize(const std::string& name, const cv::Mat& image);
-
-  /**
-   * @brief Export vector of matrices to yaml file
-   * @remarks This method is intended for debugging purposes
-   * @param filename   Desired filename including path and excluding file suffix
-   * @param mat_vec    Vector of cv::Mat to be exported
-   */
-  void writeMatToYAML(const std::string& filename, const std::vector<cv::Mat>& mat_vec);
-
-  //! Update internal parameters
-  void updateParameters(const Params& parameters);
-
-private:
-  //! Transform/shift all internal matrices/grids according to a given translation vector
-  void transformToCurrentFrame(int shift_x, int shift_y);
-
-  cv::Mat occupancy_grid_fast_;
-  cv::Mat occupancy_grid_slow_;
-  cv::Mat current_frame_;
-
-  int previous_shift_x_;
-  int previous_shift_y_;
-
-  Params params_;
-};
-
-#endif // BACKGROUNDSUBTRACTOR_H_

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h

@@ -1,111 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2017
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following code makes use of the OpenCV library.
- * OpenCV is licensed under the terms of the 3-clause BSD License.
- *
- * Authors: Franz Albers, Christoph Rösmann
- *********************************************************************/
-
-#ifndef BLOBDETECTOR_H_
-#define BLOBDETECTOR_H_
-
-// Basically the OpenCV SimpleBlobDetector, extended with getContours()
-
-#include <opencv2/features2d/features2d.hpp>
-
-/**
- * @class BlobDetector
- * @brief Detect blobs in image (specialized for dynamic obstacles in the costmap)
- *
- * This class is based on OpenCV's blob detector cv::SimpleBlobDetector.
- * It has been modified and specialized for dynamic obstacle tracking in the costmap:
- * -> The modified version also returns contours of the blob.
- *
- * See http://docs.opencv.org/trunk/d0/d7a/classcv_1_1SimpleBlobDetector.html for the original class.
- */
-class BlobDetector : public cv::SimpleBlobDetector
-{
-public:
-  //! Default constructor which optionally accepts custom parameters
-  BlobDetector(const cv::SimpleBlobDetector::Params& parameters = cv::SimpleBlobDetector::Params());
-
-  //! Create shared instance of the blob detector with given parameters
-  static cv::Ptr<BlobDetector> create(const BlobDetector::Params& params);
-
-  /**
-   * @brief Detects keypoints in an image and extracts contours
-   *
-   * In contrast to the original detect method, this extended version
-   * also extracts contours. Contours can be accessed by getContours()
-   * after invoking this method.
-   *
-   * @todo The mask option is currently not implemented.
-   *
-   * @param image     image
-   * @param keypoints The detected keypoints.
-   * @param mask      Mask specifying where to look for keypoints (optional). It must be a 8-bit integer
-   *                  matrix with non-zero values in the region of interest.
-   */
-  virtual void detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,
-                      const cv::Mat& mask = cv::Mat());
-
-  /**
-   * @brief Access contours extracted during detection stage
-   * @return Read-only reference to the contours set of the previous detect() run
-   */
-  const std::vector<std::vector<cv::Point>>& getContours() { return contours_; }
-
-  //! Update internal parameters
-  void updateParameters(const cv::SimpleBlobDetector::Params& parameters);
-
-protected:
-  struct Center
-  {
-    cv::Point2d location;
-    double radius;
-    double confidence;
-  };
-
-  virtual void findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
-                         std::vector<std::vector<cv::Point>>& cur_contours) const;
-
-  std::vector<std::vector<cv::Point>> contours_;
-
-  Params params_;
-};
-
-#endif // BLOBDETECTOR_H_

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h

@@ -1,212 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2017
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following code makes use of the OpenCV library.
- * OpenCV is licensed under the terms of the 3-clause BSD License.
- *
- * Authors: Franz Albers, Christoph Rösmann
- *********************************************************************/
-
-#ifndef COSTMAP_TO_DYNAMIC_OBSTACLES_H_
-#define COSTMAP_TO_DYNAMIC_OBSTACLES_H_
-
-// ROS
-#include <costmap_converter/costmap_converter_interface.h>
-#include <nav_msgs/msg/odometry.hpp>
-#include <pluginlib/class_loader.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-// OpenCV
-#include <cv_bridge/cv_bridge.h>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/video/tracking.hpp>
-
-// dynamic reconfigure
-//#include <costmap_converter/CostmapToDynamicObstaclesConfig.h>
-//#include <dynamic_reconfigure/server.h>
-
-// Own includes
-#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
-#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
-#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
-
-// STL
-#include <memory>
-
-namespace costmap_converter {
-
-/**
- * @class CostmapToDynamicObstacles
- * @brief This class converts the costmap_2d into dynamic obstacles.
- *
- * Static obstacles are treated as point obstacles.
- */
-class CostmapToDynamicObstacles : public BaseCostmapToDynamicObstacles
-{
-public:
-  /**
-   * @brief Constructor
-   */
-  CostmapToDynamicObstacles();
-
-  /**
-   * @brief Destructor
-   */
-  virtual ~CostmapToDynamicObstacles();
-
-  /**
-   * @brief Initialize the plugin
-   * @param nh Nodehandle that defines the namespace for parameters
-   */
-  virtual void initialize(rclcpp::Node::SharedPtr nh);
-
-  /**
-   * @brief This method performs the actual work (conversion of the costmap to
-   * obstacles)
-   */
-  virtual void compute();
-
-  /**
-   * @brief Pass a pointer to the costmap to the plugin.
-   * @sa updateCostmap2D
-   * @param costmap Pointer to the costmap2d source
-   */
-  virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap);
-
-  /**
-   * @brief Get updated data from the previously set Costmap2D
-   * @sa setCostmap2D
-   */
-  virtual void updateCostmap2D();
-
-  /**
-   * @brief Get a shared instance of the current obstacle container
-   * @remarks If compute() or startWorker() has not been called before, this
-   * method returns an empty instance!
-   * @return Shared instance of the current obstacle container
-   */
-  ObstacleArrayConstPtr getObstacles();
-
-  /**
-   * @brief Set name of robot's odometry topic
-   *
-   * @warning The method must be called before initialize()
-   *
-   * Some plugins might require odometry information
-   * to compensate the robot's ego motion
-   * @param odom_topic topic name
-   */
-  virtual void setOdomTopic(const std::string& odom_topic)
-  {
-    odom_topic_ = odom_topic;
-  }
-
-  /**
-   * @brief OpenCV Visualization
-   * @param name  Id/name of the opencv window
-   * @param image Image to be visualized
-   */
-  void visualize(const std::string& name, const cv::Mat& image);
-
-protected:
-  /**
-   * @brief Converts the estimated velocity of a tracked object to m/s and
-   * returns it
-   * @param idx Index of the tracked object in the tracker
-   * @return Point_t, which represents velocity in [m/s] of object(idx) in x,y,z
-   * coordinates
-   */
-  Point_t getEstimatedVelocityOfObject(unsigned int idx);
-
-  /**
-   * @brief Gets the last observed contour of a object and converts it from [px]
-   * to [m]
-   * @param[in]  idx Index of the tracked object in the tracker
-   * @param[out] contour vector of Point_t, which represents the last observed contour in [m]
-   *             in x,y,z coordinates
-   */
-  void getContour(unsigned int idx, std::vector<Point_t>& contour);
-
-  /**
-   * @brief Thread-safe update of the internal obstacle container (that is
-   * shared using getObstacles() from outside this
-   * class)
-   * @param obstacles Updated obstacle container
-   */
-  void updateObstacleContainer(ObstacleArrayPtr obstacles);
-
-private:
-  std::mutex mutex_;
-  nav2_costmap_2d::Costmap2D* costmap_;
-  cv::Mat costmap_mat_;
-  ObstacleArrayPtr obstacles_;
-  cv::Mat fg_mask_;
-  std::unique_ptr<BackgroundSubtractor> bg_sub_;
-  cv::Ptr<BlobDetector> blob_det_;
-  std::vector<cv::KeyPoint> keypoints_;
-  std::unique_ptr<CTracker> tracker_;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
-  Point_t ego_vel_;
-
-  std::string odom_topic_ = "/odom";
-  bool publish_static_obstacles_ = true;
-
-//  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>*
-//      dynamic_recfg_; //!< Dynamic reconfigure server to allow config
-//                       //! modifications at runtime
-
-  /**
-   * @brief Callback for the odometry messages of the observing robot.
-   *
-   * Used to convert the velocity of obstacles to the /map frame.
-   * @param msg The Pointer to the nav_msgs::Odometry of the observing robot
-   */
-  void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
-
-  /**
-   * @brief Callback for the dynamic_reconfigure node.
-   *
-   * This callback allows to modify parameters dynamically at runtime without
-   * restarting the node
-   * @param config Reference to the dynamic reconfigure config
-   * @param level Dynamic reconfigure level
-   */
-//  void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level);
-};
-
-} // end namespace costmap_converter
-
-#endif /* COSTMAP_TO_DYNAMIC_OBSTACLES_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h

@@ -1,96 +0,0 @@
-// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
-// Refer to README.md in this directory.
-
-#pragma once
-#include "Kalman.h"
-#include "HungarianAlg.h"
-#include "defines.h"
-#include <iostream>
-#include <vector>
-#include <memory>
-#include <array>
-
-// --------------------------------------------------------------------------
-class CTrack
-{
-public:
-  CTrack(const Point_t& p, const std::vector<cv::Point>& contour, track_t dt, size_t trackID)
-      : track_id(trackID),
-        skipped_frames(0),
-        prediction(p),
-        lastContour(contour),
-        KF(p, dt)
-  {
-  }
-
-  track_t CalcDist(const Point_t& p)
-  {
-    Point_t diff = prediction - p;
-    return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
-  }
-
-  void Update(const Point_t& p, const std::vector<cv::Point>& contour, bool dataCorrect, size_t max_trace_length)
-  {
-    KF.Prediction();
-    prediction = KF.Update(p, dataCorrect);
-
-    if (dataCorrect)
-    {
-      lastContour = contour;
-    }
-
-    if (trace.size() > max_trace_length)
-    {
-      trace.erase(trace.begin(), trace.end() - max_trace_length);
-    }
-
-    trace.push_back(prediction);
-  }
-
-  // Returns contour in [px], not in [m]!
-  std::vector<cv::Point> getLastContour() const
-  {
-    return lastContour;
-  }
-
-  // Returns velocity in [px/s], not in [m/s]!
-  Point_t getEstimatedVelocity() const
-  {
-    return KF.LastVelocity;
-  }
-
-  std::vector<Point_t> trace;
-  size_t track_id;
-  size_t skipped_frames;
-
-private:
-  Point_t prediction;
-  std::vector<cv::Point> lastContour; // Contour liegt immer auf ganzen Pixeln -> Integer Punkt
-  TKalmanFilter KF;
-};
-
-// --------------------------------------------------------------------------
-class CTracker
-{
-public:
-  struct Params{
-    track_t dt; // time for one step of the filter
-    track_t dist_thresh;// distance threshold. Pairs of points with higher distance are not considered in the assignment problem
-    int max_allowed_skipped_frames; // Maximum number of frames the track is maintained without seeing the object in the measurement data
-    int max_trace_length; // Maximum trace length
-  };
-
-  CTracker(const Params& parameters);
-  ~CTracker(void);
-
-  std::vector<std::unique_ptr<CTrack>> tracks;
-  void Update(const std::vector<Point_t>& detectedCentroid, const std::vector<std::vector<cv::Point> > &contour);
-
-  void updateParameters(const Params &parameters);
-
-private:
-  // Aggregated parameters for the tracker object
-  Params params;
-  // ID for the upcoming CTrack object
-  size_t NextTrackID;
-};

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h

@@ -1,60 +0,0 @@
-// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
-// Refer to README.md in this directory.
-
-#include <vector>
-#include <iostream>
-#include <limits>
-#include <time.h>
-#include "defines.h"
-
-// http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm
-
-typedef std::vector<int> assignments_t;
-typedef std::vector<track_t> distMatrix_t;
-
-class AssignmentProblemSolver
-{
-private:
-  // --------------------------------------------------------------------------
-  // Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
-  // --------------------------------------------------------------------------
-  void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
-                         size_t nOfColumns);
-  void buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows, size_t nOfColumns);
-  void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn,
-                             size_t nOfRows);
-  void step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
-              bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
-  void step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
-              bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
-  void step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
-             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
-  void step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
-             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row,
-             size_t col);
-  void step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
-             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
-  // --------------------------------------------------------------------------
-  // Computes a suboptimal solution. Good for cases with many forbidden assignments.
-  // --------------------------------------------------------------------------
-  void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
-                             size_t nOfColumns);
-  // --------------------------------------------------------------------------
-  // Computes a suboptimal solution. Good for cases with many forbidden assignments.
-  // --------------------------------------------------------------------------
-  void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
-                             size_t nOfColumns);
-
-public:
-  enum TMethod
-  {
-    optimal,
-    many_forbidden_assignments,
-    without_forbidden_assignments
-  };
-
-  AssignmentProblemSolver();
-  ~AssignmentProblemSolver();
-  track_t Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t& assignment,
-                TMethod Method = optimal);
-};

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h

@@ -1,20 +0,0 @@
-// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
-// Refer to README.md in this directory.
-
-#pragma once
-#include "defines.h"
-#include <opencv2/opencv.hpp>
-
-// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
-class TKalmanFilter
-{
-public:
-  TKalmanFilter(Point_t p, track_t deltatime = 0.2);
-  ~TKalmanFilter();
-  void Prediction();
-  Point_t Update(Point_t p, bool DataCorrect);
-  cv::KalmanFilter* kalman;
-  track_t dt;
-  Point_t LastPosition; // contour in [px]
-  Point_t LastVelocity; // velocity in [px/s]
-};

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md

@@ -1,13 +0,0 @@
-Multitarget Tracker
-===================
-
-This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].
-It is utilized for the *CostmapToDynamicObstacles* plugin.
-
-The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].
-The package itself depends on other third party packages with different licenses (refer to the package repository).
-
-TODO: Include the whole package as external CMake project.
-
-
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h

@@ -1,9 +0,0 @@
-// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
-// Refer to README.md in this directory.
-
-#pragma once
-#include <opencv2/opencv.hpp>
-
-typedef float track_t;
-typedef cv::Point3_<track_t> Point_t;
-#define Mat_t CV_32FC

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_convex_hull.h

@@ -1,137 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann, Otniel Rinaldo
- *********************************************************************/
-
-#ifndef COSTMAP_TO_LINES_CONVEX_HULL_H_
-#define COSTMAP_TO_LINES_CONVEX_HULL_H_
-
-#include <costmap_converter/costmap_converter_interface.h>
-#include <costmap_converter/costmap_to_polygons.h>
-
-// dynamic reconfigure
-//#include <costmap_converter/CostmapToLinesDBSMCCHConfig.h>
-
-namespace costmap_converter
-{
-  
-/**
- * @class CostmapToLinesDBSMCCH
- * @brief This class converts the costmap_2d into a set of lines (and points)
- * 
- * The conversion is performed in three stages:
- * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
- *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
- *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
- *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
- *    
- * 2. In the subsequent stage, the convex hull of each cluster is determined using the monotone chain algorithm aka Andrew's algorithm:
- *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
- *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
- * 
- * 3. In the third step extract lines from each polygon (convex hull) if there exist at least a predefined number of support points.
- * 
- * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)
- */
-  class CostmapToLinesDBSMCCH : public CostmapToPolygonsDBSMCCH
-  {
-  public:
-   
-    /**
-     * @brief Constructor
-     */
-    CostmapToLinesDBSMCCH();
-       
-    /**
-     * @brief Destructor
-     */
-    virtual ~CostmapToLinesDBSMCCH();
-    
-    /**
-     * @brief Initialize the plugin
-     * @param nh Nodehandle that defines the namespace for parameters
-     */
-    virtual void initialize(rclcpp::Node::SharedPtr nh);
-    
-    /**
-     * @brief This method performs the actual work (conversion of the costmap to polygons)
-     */
-    virtual void compute();   
-    
-    
-  protected:
-    
-    /**
-     * @brief Extract points and lines from a given cluster by checking all keypoint pairs of the convex hull for a minimum number of support points
-     * @param cluster list of points in the cluster
-     * @param polygon convex hull of the cluster \c cluster
-     * @param[out] lines back_inserter object to a sequence of polygon msgs (new lines will be pushed back)
-     */
-    void extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::msg::Polygon& polygon, std::back_insert_iterator< std::vector<geometry_msgs::msg::Polygon> > lines);
-
-    
-    
-  protected:
-      
-    double support_pts_max_dist_inbetween_;
-    double support_pts_max_dist_;
-    int min_support_pts_;
-   
-  private:
-    
-    /**
-     * @brief Callback for the dynamic_reconfigure node.
-     * 
-     * This callback allows to modify parameters dynamically at runtime without restarting the node
-     * @param config Reference to the dynamic reconfigure config
-     * @param level Dynamic reconfigure level
-     */
-//    void reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level);
-    
-//    dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
- 
-  };  
-  
-  
-  
-  
-
-
-  
-  
-} //end namespace teb_local_planner
-
-#endif /* COSTMAP_TO_LINES_CONVEX_HULL_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_ransac.h

@@ -1,187 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef COSTMAP_TO_LINES_RANSAC_H_
-#define COSTMAP_TO_LINES_RANSAC_H_
-
-#include <costmap_converter/costmap_converter_interface.h>
-#include <costmap_converter/costmap_to_polygons.h>
-#include <costmap_converter/misc.h>
-
-#include <random>
-
-//#include <costmap_converter/CostmapToLinesDBSRANSACConfig.h>
-
-namespace costmap_converter
-{
-  
-/**
- * @class CostmapToLinesDBSRANSAC
- * @brief This class converts the costmap_2d into a set of lines (and points)
- * 
- * The conversion is performed in two stages:
- * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
- *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
- *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
- *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
- *    
- * 2. The RANSAC algorithm is used to find straight line segment models (https://en.wikipedia.org/wiki/RANSAC)
- *    RANSAC is called repeatedly to find multiple lines per cluster until the number of inliners is below a specific threshold.
- *    In that case the remaining outliers are used as points or keypoints of their convex hull are used as points (depending on a paramter).
- *    The latter one applies as a filter. The convex assumption is not strict in practice, since the remaining regions/cluster (line inliers are removed)
- *    should be dense and small. For details about the convex hull algorithm, refer to costmap_converter::CostmapToPolygonsDBSMCCH.
- *    Resulting lines of RANSAC are currently not refined by linear regression.
- * 
- * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)
- */
-  class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH
-  {
-  public:
-   
-    /**
-     * @brief Constructor
-     */
-    CostmapToLinesDBSRANSAC();
-       
-    /**
-     * @brief Destructor
-     */
-    virtual ~CostmapToLinesDBSRANSAC();
-    
-    /**
-     * @brief Initialize the plugin
-     * @param nh Nodehandle that defines the namespace for parameters
-     */
-    virtual void initialize(rclcpp::Node::SharedPtr nh);
-    
-    /**
-     * @brief This method performs the actual work (conversion of the costmap to polygons)
-     */
-    virtual void compute();   
-        
-     
-    /**
-     * @brief Check if the candidate point is an inlier.
-     * @param point generic 2D point type defining the reference point
-     * @param line_start generic 2D point type defining the start of the line
-     * @param line_end generic 2D point type defining the end of the line
-     * @param min_distance minimum distance allowed
-     * @tparam Point generic point type that should provide (writable) x and y member fiels.
-     * @tparam LinePoint generic point type that should provide (writable) x and y member fiels.
-     * @return \c true if inlier, \c false otherwise
-     */
-    template <typename Point, typename LinePoint>
-    static bool isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance);
-    
-  protected:
-    
-    double ransac_inlier_distance_; //!< Maximum distance to the line segment for inliers
-    int ransac_min_inliers_; //!< Minimum numer of inliers required to form a line
-    int ransac_no_iterations_; //!< Number of ransac iterations
-    int ransac_remainig_outliers_; //!< Repeat ransac until the number of outliers is as specified here
-    bool ransac_convert_outlier_pts_; //!< If \c true, convert remaining outliers to single points.
-    bool ransac_filter_remaining_outlier_pts_; //!< If \c true, filter the interior of remaining outliers and keep only keypoints of their convex hull
-   
-   
-   std::mt19937 rnd_generator_; //!< Random number generator for ransac with default seed
-    
-    
-    /**
-     * @brief Find a straight line segment in a point cloud with RANSAC (without linear regression).
-     * @param data set of 2D data points
-     * @param inlier_distance maximum distance that inliers must satisfy
-     * @param no_iterations number of RANSAC iterations
-     * @param min_inliers minimum number of inliers to return true
-     * @param[out] best_model start and end of the best straight line segment 
-     * @param[out] inliers inlier keypoints are written to this container [optional]
-     * @param[out] outliers outlier keypoints are written to this container [optional]
-     * @return \c false, if \c min_inliers is not satisfied, \c true otherwise
-     */
-    bool lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations, int min_inliers,
-                    std::pair<KeyPoint, KeyPoint>& best_model, std::vector<KeyPoint>* inliers = NULL,
-                     std::vector<KeyPoint>* outliers = NULL);
-    
-    /**
-     * @brief Perform a simple linear regression in order to fit a straight line 'y = slope*x + intercept'
-     * @param data set of 2D data points
-     * @param[out] slope The slope of the fitted line 
-     * @param[out] intercept The intercept / offset of the line
-     * @param[out] mean_x_out mean of the x-values of the data [optional]
-     * @param[out] mean_y_out mean of the y-values of the data [optional]
-     * @return \c true, if a valid line has been fitted, \c false otherwise.
-     */
-    bool linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept,
-                          double* mean_x_out = NULL, double* mean_y_out = NULL);
-    
-    
-    
-//     void adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, 
-//                           KeyPoint& line_start, KeyPoint& line_end);
-    
-    private:
-    
-    /**
-     * @brief Callback for the dynamic_reconfigure node.
-     * 
-     * This callback allows to modify parameters dynamically at runtime without restarting the node
-     * @param config Reference to the dynamic reconfigure config
-     * @param level Dynamic reconfigure level
-     */
-//    void reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level);
-    
-//    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
-  };  
-  
-  
-
-template <typename Point, typename LinePoint>
-bool CostmapToLinesDBSRANSAC::isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance)
-{
-  bool is_inbetween = false;
-  double distance = computeDistanceToLineSegment(point, line_start, line_end, &is_inbetween);
-  if (!is_inbetween)
-    return false;
-  if (distance <= min_distance)
-    return true;
-  return false;
-}
-     
-  
-} //end namespace teb_local_planner
-
-#endif /* COSTMAP_TO_LINES_RANSAC_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons.h

@@ -1,335 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann, Otniel Rinaldo
- *********************************************************************/
-
-#ifndef COSTMAP_TO_POLYGONS_H_
-#define COSTMAP_TO_POLYGONS_H_
-
-#include <rclcpp/rclcpp.hpp>
-#include <costmap_converter/costmap_converter_interface.h>
-#include <nav_msgs/msg/occupancy_grid.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <geometry_msgs/msg/polygon.hpp>
-#include <vector>
-#include <algorithm>
-#include <mutex>
-#include <Eigen/Core>
-#include <Eigen/StdVector>
-
-// dynamic reconfigure
-//#include <costmap_converter/CostmapToPolygonsDBSMCCHConfig.h>
-//#include <dynamic_reconfigure/server.h>
-
-
-namespace costmap_converter
-{
-  
-/**
- * @class CostmapToPolygonsDBSMCCH
- * @brief This class converts the costmap_2d into a set of convex polygons (and points)
- * 
- * The conversion is performed in two stages:
- * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
- *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
- *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
- *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
- *    
- * 2. In the subsequent stage, clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:
- *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
- *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
- *  
- * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)
- */
-class CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons
-{
-  public:
-    
-    /**
-     * @struct KeyPoint
-     * @brief Defines a keypoint in metric coordinates of the map 
-     */
-    struct KeyPoint
-    {
-      //! Default constructor
-      KeyPoint() : x(0.0), y(0.0) {}
-      //! Constructor with point initialization
-      KeyPoint(double x_, double y_) : x(x_), y(y_) {}
-      double x; //!< x coordinate [m]
-      double y; //!< y coordinate [m]
-      
-      //! Convert keypoint to geometry_msgs::msg::Point message type
-      void toPointMsg(geometry_msgs::msg::Point& point) const {point.x=x; point.y=y; point.z=0;}
-      //! Convert keypoint to geometry_msgs::msg::Point32 message type
-      void toPointMsg(geometry_msgs::msg::Point32& point) const {point.x=x; point.y=y; point.z=0;}
-    };
-
-    /**
-     * @struct Parameters
-     * @brief Defines the parameters of the algorithm
-     */
-    struct Parameters
-    {
-      Parameters() : max_distance_(0.4), min_pts_(2), max_pts_(30), min_keypoint_separation_(0.1) {}
-      // DBSCAN parameters
-      double max_distance_; //!< Parameter for DB_Scan, maximum distance to neighbors [m]
-      int min_pts_; //!< Parameter for DB_Scan: minimum number of points that define a cluster
-      int max_pts_; //!< Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes)
-      
-      // convex hull parameters
-      double min_keypoint_separation_; //!< Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)
-    };
-    
-    /**
-     * @brief Constructor
-     */
-    CostmapToPolygonsDBSMCCH();
-    
-    /**
-     * @brief Destructor
-     */
-    virtual ~CostmapToPolygonsDBSMCCH();
-    
-    /**
-     * @brief Initialize the plugin
-     * @param nh Nodehandle that defines the namespace for parameters
-     */
-    virtual void initialize(rclcpp::Node::SharedPtr nh) override;
-    
-    /**
-     * @brief This method performs the actual work (conversion of the costmap to polygons)
-     */
-    virtual void compute();
-        
-    /**
-     * @brief Pass a pointer to the costap to the plugin.
-     * @sa updateCostmap2D
-     * @param costmap Pointer to the costmap2d source
-     */
-    virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap);
-    
-    /**
-     * @brief Get updated data from the previously set Costmap2D
-     * @sa setCostmap2D
-     */
-    virtual void updateCostmap2D();
-    
-    
-    /**
-     * @brief Convert a generi point type to a geometry_msgs::msg::Polygon
-     * @param point generic 2D point type
-     * @param[out] polygon already instantiated polygon that will be filled with a single point
-     * @tparam Point generic point type that should provide (writable) x and y member fiels.
-     */
-    template< typename Point>
-    static void convertPointToPolygon(const Point& point, geometry_msgs::msg::Polygon& polygon)
-    {
-      polygon.points.resize(1);
-      polygon.points.front().x = point.x;
-      polygon.points.front().y = point.y;
-      polygon.points.front().z = 0;
-    }
-    
-    /**
-     * @brief Get a shared instance of the current polygon container
-     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
-     * @return Shared instance of the current polygon container
-     */
-    PolygonContainerConstPtr getPolygons();
-
-    
-    
-  protected:
-    
-    /**
-     * @brief DBSCAN algorithm for clustering 
-     * 
-     * Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
-     *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
-     *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
-     *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
-     * 
-     * @param[out] clusters clusters will added to this output-argument (a sequence of keypoints for each cluster)
-     *                      the first cluster (clusters[0]) will contain all noise points (that does not fulfil the min_pts condition 
-     */
-    void dbScan(std::vector< std::vector<KeyPoint> >& clusters);
-    
-    /**
-     * @brief Helper function for dbScan to search for neighboring points 
-     * 
-     * @param curr_index index to the current item in \c occupied_cells
-     * @param[out] neighbor_indices list of neighbors (indices of \c occupied cells)
-     */
-    void regionQuery(int curr_index, std::vector<int>& neighbor_indices);
-
-    /**
-     * @brief helper function for adding a point to the lookup data structures
-     */
-    void addPoint(double x, double y)
-    {
-      int idx = occupied_cells_.size();
-      occupied_cells_.emplace_back(x, y);
-      int cx, cy;
-      pointToNeighborCells(occupied_cells_.back(), cx, cy);
-      int nidx = neighborCellsToIndex(cx, cy);
-      if (nidx >= 0)
-        neighbor_lookup_[nidx].push_back(idx);
-    }
-    
-    /**
-     * @brief Compute the convex hull for a single cluster (monotone chain algorithm)
-     * 
-     * Clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:
-     *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
-     *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
-     * @remarks The algorithm seems to cut edges, thus we give a try to convexHull2().
-     * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)
-     * @remarks The last point is the same as the first one
-     * @param cluster list of keypoints that should be converted into a polygon
-     * @param[out] polygon the resulting convex polygon
-     */  
-    void convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon);
-    
-    /**
-     * @brief Compute the convex hull for a single cluster
-     * 
-     * Clusters are converted into convex polgons using an algorithm provided here:
-     *  https://bitbucket.org/vostreltsov/concave-hull/overview
-     * The convex hull algorithm is part of the concave hull algorithm.
-     * The license is WTFPL 2.0 and permits any usage.
-     * 
-     * @remarks The last point is the same as the first one
-     * @param cluster list of keypoints that should be converted into a polygon
-     * @param[out] polygon the resulting convex polygon
-     * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)
-     */  
-    void convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon);
-
-    /**
-     * @brief Simplify a polygon by removing points.
-     * 
-     * We apply the Douglas-Peucker Algorithm to simplify the edges of the polygon.
-     * Internally, the parameter min_keypoint_separation is used for deciding whether
-     * a point is considered close to an edge and to be merged into the line.
-     */
-    void simplifyPolygon(geometry_msgs::msg::Polygon& polygon);
-    
-   /**
-    * @brief 2D Cross product of two vectors defined by two points and a common origin
-    * @param O Origin
-    * @param A First point
-    * @param B Second point
-    * @tparam P1 2D Point type with x and y member fields
-    * @tparam P2 2D Point type with x and y member fields
-    * @tparam P3 2D Point type with x and y member fields
-    */    
-    template <typename P1, typename P2, typename P3>
-    long double cross(const P1& O, const P2& A, const P3& B)
-    { 
-        return (long double)(A.x - O.x) * (long double)(B.y - O.y) - (long double)(A.y - O.y) * (long double)(B.x - O.x);
-    }
-    
-      
-   /**
-    * @brief Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class)
-    * @param polygons Updated polygon container
-    */
-   void updatePolygonContainer(PolygonContainerPtr polygons);   
-          
-   
-   std::vector<KeyPoint> occupied_cells_; //!< List of occupied cells in the current map (updated by updateCostmap2D())
-   
-    std::vector<std::vector<int> > neighbor_lookup_; //! array of cells for neighbor lookup
-    int neighbor_size_x_; //! size of the neighbour lookup in x (number of cells)
-    int neighbor_size_y_; //! size of the neighbour lookup in y (number of cells)
-    double offset_x_;     //! offset [meters] in x for the lookup grid
-    double offset_y_;     //! offset [meters] in y for the lookup grid
-
-    /**
-      * @brief convert a 2d cell coordinate into the 1D index of the array
-      * @param cx the x index of the cell
-      * @param cy the y index of the cell
-      */
-    int neighborCellsToIndex(int cx, int cy)
-    {
-      if (cx < 0 || cx >= neighbor_size_x_ || cy < 0 || cy >= neighbor_size_y_)
-        return -1;
-      return cy * neighbor_size_x_ + cx;
-    }
-
-    /**
-      * @brief compute the cell indices of a keypoint
-      * @param kp key point given in world coordinates [m, m]
-      * @param cx output cell index in x direction
-      * @param cy output cell index in y direction
-      */
-    void pointToNeighborCells(const KeyPoint& kp, int& cx, int& cy)
-    {
-      cx = int((kp.x - offset_x_) / parameter_.max_distance_);
-      cy = int((kp.y - offset_y_) / parameter_.max_distance_);
-    }
-
-
-    Parameters parameter_;          //< active parameters throughout computation
-    Parameters parameter_buffered_; //< the buffered parameters that are offered to dynamic reconfigure
-    std::mutex parameter_mutex_;  //!< Mutex that keeps track about the ownership of the shared polygon instance
-   
-  private:
-       
-    /**
-     * @brief Callback for the dynamic_reconfigure node.
-     * 
-     * This callback allows to modify parameters dynamically at runtime without restarting the node
-     * @param config Reference to the dynamic reconfigure config
-     * @param level Dynamic reconfigure level
-     */
-    //void reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level);
-    
-    
-    PolygonContainerPtr polygons_; //!< Current shared container of polygons
-    std::mutex mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance
-    
-    //dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
-   
-    nav2_costmap_2d::Costmap2D *costmap_; //!< Pointer to the costmap2d
-   
-}; 
-
-  
-} //end namespace teb_local_planner
-
-#endif /* COSTMAP_TO_POLYGONS_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons_concave.h

@@ -1,202 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef COSTMAP_TO_POLYGONS_CONCAVE_H_
-#define COSTMAP_TO_POLYGONS_CONCAVE_H_
-
-#include <costmap_converter/costmap_to_polygons.h>
-#include <costmap_converter/misc.h>
-
-// dynamic reconfigure
-//#include <costmap_converter/CostmapToPolygonsDBSConcaveHullConfig.h>
-//#include <dynamic_reconfigure/server.h>
-
-
-namespace costmap_converter
-{
-  
-/**
- * @class CostmapToPolygonsDBSConcaveHull
- * @brief This class converts the costmap_2d into a set of non-convex polygons (and points)
- *  
- * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)
- * @todo change the class hierarchy to a clearer and more generic one
- */
-class CostmapToPolygonsDBSConcaveHull : public CostmapToPolygonsDBSMCCH
-{
-  public:
-    
-    
-    
-    /**
-     * @brief Constructor
-     */
-    CostmapToPolygonsDBSConcaveHull();
-    
-    /**
-     * @brief Destructor
-     */
-    virtual ~CostmapToPolygonsDBSConcaveHull();
-    
-    /**
-     * @brief Initialize the plugin
-     * @param nh Nodehandle that defines the namespace for parameters
-     */
-    virtual void initialize(rclcpp::Node::SharedPtr nh);
-   
-    
-    /**
-     * @brief This method performs the actual work (conversion of the costmap to polygons)
-     */
-    virtual void compute();
-    
-  protected:
- 
-   
-    /**
-     * @brief Compute the concave hull for a single cluster
-     * 
-     * @remarks The last point is the same as the first one
-     * @param cluster list of keypoints that should be converted into a polygon
-     * @param depth Smaller depth: sharper surface, depth -> high value: convex hull
-     * @param[out] polygon the resulting convex polygon
-     */  
-    void concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon);
-        
-    void concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon);
-    
-    template <typename PointLine, typename PointCluster, typename PointHull>
-    std::size_t findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found);
-        
-    
-    template <typename Point1, typename Point2, typename Point3, typename Point4>
-    bool checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end);
-    
-    template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>
-    bool checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start, 
-                               const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end);
-    
-    double concave_hull_depth_;
-
-  private:
-       
-    /**
-     * @brief Callback for the dynamic_reconfigure node.
-     * 
-     * This callback allows to modify parameters dynamically at runtime without restarting the node
-     * @param config Reference to the dynamic reconfigure config
-     * @param level Dynamic reconfigure level
-     */
-//    void reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level);
-    
-//    dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
-   
-   
-}; 
-
-
-template <typename PointLine, typename PointCluster, typename PointHull>
-std::size_t CostmapToPolygonsDBSConcaveHull::findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found)
-{
-    std::size_t nearsest_idx = 0;
-    double distance = 0;
-    *found = false;
-
-    for (std::size_t i = 0; i < cluster.size(); ++i)
-    {
-        // Skip points that are already in the hull
-        if (std::find_if( hull.begin(), hull.end(), std::bind(isApprox2d<PointHull, PointCluster>, std::placeholders::_1, std::cref(cluster[i]), 1e-5) ) != hull.end() )
-            continue;
-
-        double dist = computeDistanceToLineSegment(cluster[i], line_start, line_end);
-        bool skip = false;
-        for (int j = 0; !skip && j < (int)hull.size() - 1; ++j) 
-        {
-            double dist_temp = computeDistanceToLineSegment(cluster[i], hull[j], hull[j + 1]);
-            skip |= dist_temp < dist;
-        }
-        if (skip) 
-            continue;
-
-        if (!(*found) || dist < distance) 
-        {
-            nearsest_idx = i;
-            distance = dist;
-            *found = true;
-        }
-    }
-    return nearsest_idx;
-}
-
-
-template <typename Point1, typename Point2, typename Point3, typename Point4>
-bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end)
-{
-    double dx1 = line1_end.x - line1_start.x;
-    double dy1 = line1_end.y - line1_start.y;
-    double dx2 = line2_end.x - line2_start.x;
-    double dy2 = line2_end.y - line2_start.y;
-    double s = (-dy1 * (line1_start.x - line2_start.x) + dx1 * (line1_start.y - line2_start.y)) / (-dx2 * dy1 + dx1 * dy2);
-    double t = ( dx2 * (line1_start.y - line2_start.y) - dy2 * (line1_start.x - line2_start.x)) / (-dx2 * dy1 + dx1 * dy2);
-    return (s > 0 && s < 1 && t > 0 && t < 1);
-}
-
-
-template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>
-bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start, 
-                                                            const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end)
-{
-    for (int i = 0; i < (int)hull.size() - 2; i++) 
-    {
-        if (isApprox2d(current_line_start, hull[i], 1e-5) && isApprox2d(current_line_end, hull[i+1], 1e-5))
-        {
-            continue;
-        }
-
-        if (checkLineIntersection(test_line_start, test_line_end, hull[i], hull[i+1])) 
-        {
-            return true;
-        }
-    }
-    return false;
-} 
-  
-  
-} //end namespace teb_local_planner
-
-#endif /* COSTMAP_TO_POLYGONS_CONCAVE_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/misc.h

@@ -1,157 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef MISC_H_
-#define MISC_H_
-
-#include <algorithm>
-#include <cmath>
-
-namespace costmap_converter
-{
-
-/**
-  * @brief Calculate the distance between a point and a straight line (with infinite length)
-  * @param point generic 2D point type defining the reference point
-  * @param line_pt1 generic 2D point as part of the line
-  * @param line_pt2 generic 2D point as part of the line
-  * @tparam Point generic point type that should provide x and y member fiels.
-  * @tparam LinePoint generic point type that should provide x and y member fiels.
-  * @return (minimum) euclidean distance to the line segment
-  */
-template <typename Point, typename LinePoint>
-inline double computeDistanceToLine(const Point& point, const LinePoint& line_pt1, const LinePoint& line_pt2)
-{
-    double dx = line_pt2.x - line_pt1.x;
-    double dy = line_pt2.y - line_pt1.y;
-    
-    double length = std::sqrt(dx*dx + dy*dy);
-    
-    if (length>0)
-      return std::abs(dy * point.x - dx * point.y + line_pt2.x * line_pt1.y - line_pt2.y * line_pt1.x) / length;
-  
-    return std::sqrt(std::pow(point.x - line_pt1.x, 2) + std::pow(point.y - line_pt1.y, 2));  
-}
-
-
-/**
-  * @brief Calculate the squared distance between a point and a straight line segment
-  * @param point generic 2D point type defining the reference point
-  * @param line_start generic 2D point type defining the start of the line
-  * @param line_end generic 2D point type defining the end of the line
-  * @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional]
-  * @tparam Point generic point type that should provide x and y member fiels.
-  * @tparam LinePoint generic point type that should provide x and y member fiels.
-  * @return (minimum) squared euclidean distance to the line segment
-  */
-template <typename Point, typename LinePoint>
-inline double computeSquaredDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
-{
-    double dx = line_end.x - line_start.x;
-    double dy = line_end.y - line_start.y;
-    
-    double length_sqr = dx*dx + dy*dy;
-
-    double u = 0;
-
-    if (length_sqr > 0)
-      u = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / length_sqr;
-
-    if (is_inbetween)
-      *is_inbetween = (u>=0 && u<=1);
-  
-    if (u <= 0)
-      return std::pow(point.x-line_start.x,2) + std::pow(point.y-line_start.y,2);
-    
-    if (u >= 1)
-      return std::pow(point.x-line_end.x,2) + std::pow(point.y-line_end.y,2);
-    
-    return std::pow(point.x - (line_start.x+u*dx) ,2) + std::pow(point.y - (line_start.y+u*dy),2);
-}
-  
-/**
-  * @brief Calculate the distance between a point and a straight line segment
-  * @param point generic 2D point type defining the reference point
-  * @param line_start generic 2D point type defining the start of the line
-  * @param line_end generic 2D point type defining the end of the line
-  * @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional]
-  * @tparam Point generic point type that should provide x and y member fiels.
-  * @tparam LinePoint generic point type that should provide x and y member fiels.
-  * @return (minimum) euclidean distance to the line segment
-  */
-template <typename Point, typename LinePoint>
-inline double computeDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
-{
-  return std::sqrt(computeSquaredDistanceToLineSegment(point, line_start, line_end, is_inbetween));
-}
-  
-
-/**
-  * @brief Calculate the distance between two 2d points
-  * @param pt1 generic 2D point
-  * @param pt2 generic 2D point
-  * @tparam Point1 generic point type that should provide x and y member fiels.
-  * @tparam Point2 generic point type that should provide x and y member fiels.
-  * @return euclidean distance to the line segment
-  */
-template <typename Point1, typename Point2>  
-inline double norm2d(const Point1& pt1, const Point2& pt2)
-{
-  return std::sqrt( std::pow(pt2.x - pt1.x, 2) + std::pow(pt2.y - pt1.y, 2)  );
-}
-
-/**
-  * @brief Check if two points are approximately defining the same one
-  * @param pt1 generic 2D point
-  * @param pt2 generic 2D point
-  * @param threshold define the minimum threshold |pt1.x-pt2.y| < tresh && |pt1.y-pt2.y| < tresh
-  * @tparam Point1 generic point type that should provide x and y member fiels.
-  * @tparam Point2 generic point type that should provide x and y member fiels.
-  * @return \c true, if similar, \c false otherwise
-  */
-template <typename Point1, typename Point2>  
-inline bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold)
-{
-  return ( std::abs(pt2.x-pt1.x)<threshold && std::abs(pt2.y-pt1.y)<threshold );
-}
-
-
-  
-} //end namespace teb_local_planner
-
-#endif /* MISC_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/package.xml

@@ -1,38 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>costmap_converter</name>
-  <version>0.1.2</version>
-  <description>
-    A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types.
-  </description>
-
-  <maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
-
-  <author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
-  <author email="franz.albers@tu-dortmund.de">Franz Albers</author>
-  <author email="otniel.rinaldo@tu-dortmund.de">Otniel Rinaldo</author> 
-
-  <license>BSD</license>
-
-  <url type="website">http://wiki.ros.org/costmap_converter</url>
-  
-  <buildtool_depend>ament_cmake</buildtool_depend>
-    
-  <depend>class_loader</depend>
-  <depend>costmap_converter_msgs</depend>
-  <depend>cv_bridge</depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav2_costmap_2d</depend>
-  <depend>tf2</depend>
-  <depend>pluginlib</depend>
-  <depend>rclcpp</depend>
-  <!-- <depend>dynamic_reconfigure</depend> -->
-
-  <test_depend>ament_cmake_gtest</test_depend>
-  
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_converter_node.cpp

@@ -1,264 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann, Otniel Rinaldo
- *********************************************************************/
-#include <functional>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <geometry_msgs/msg/polygon_stamped.hpp>
-#include <nav2_costmap_2d/costmap_2d.hpp>
-#include <nav_msgs/msg/occupancy_grid.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-
-#include <costmap_converter/costmap_converter_interface.h>
-#include <pluginlib/class_loader.hpp>
-
-class CostmapStandaloneConversion : public rclcpp::Node {
- public:
-  CostmapStandaloneConversion(const std::string node_name)
-      : rclcpp::Node(node_name),
-        converter_loader_("costmap_converter",
-                          "costmap_converter::BaseCostmapToPolygons") {
-    costmap_ros_ =
-        std::make_shared<nav2_costmap_2d::Costmap2DROS>("converter_costmap");
-    costmap_thread_ = std::make_unique<std::thread>(
-        [](rclcpp_lifecycle::LifecycleNode::SharedPtr node) {
-          rclcpp::spin(node->get_node_base_interface());
-        },
-        costmap_ros_);
-    rclcpp_lifecycle::State state;
-    costmap_ros_->on_configure(state);
-    costmap_ros_->on_activate(state);
-
-    n_ = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *) {});
-    // load converter plugin from parameter server, otherwise set default
-
-    std::string converter_plugin =
-        "costmap_converter::CostmapToPolygonsDBSMCCH";
-
-    declare_parameter("converter_plugin",
-                      rclcpp::ParameterValue(converter_plugin));
-
-    get_parameter_or<std::string>("converter_plugin", converter_plugin,
-                                  converter_plugin);
-
-    try {
-      converter_ = converter_loader_.createSharedInstance(converter_plugin);
-    } catch (const pluginlib::PluginlibException &ex) {
-      RCLCPP_ERROR(get_logger(),
-                   "The plugin failed to load for some reason. Error: %s",
-                   ex.what());
-      rclcpp::shutdown();
-      return;
-    }
-
-    RCLCPP_INFO(get_logger(), "Standalone costmap converter: %s loaded.",
-                converter_plugin.c_str());
-
-    std::string obstacles_topic = "costmap_obstacles";
-    declare_parameter("obstacles_topic",
-                      rclcpp::ParameterValue(obstacles_topic));
-    get_parameter_or<std::string>("obstacles_topic", obstacles_topic,
-                                  obstacles_topic);
-
-    std::string polygon_marker_topic = "costmap_polygon_markers";
-    declare_parameter("polygon_marker_topic",
-                      rclcpp::ParameterValue(polygon_marker_topic));
-    get_parameter_or<std::string>("polygon_marker_topic", polygon_marker_topic,
-                                  polygon_marker_topic);
-
-    obstacle_pub_ =
-        create_publisher<costmap_converter_msgs::msg::ObstacleArrayMsg>(
-            obstacles_topic, 1000);
-    marker_pub_ = create_publisher<visualization_msgs::msg::Marker>(
-        polygon_marker_topic, 10);
-
-    occupied_min_value_ = 100;
-    declare_parameter("occupied_min_value",
-                      rclcpp::ParameterValue(occupied_min_value_));
-    get_parameter_or<int>("occupied_min_value", occupied_min_value_,
-                          occupied_min_value_);
-
-    std::string odom_topic = "/odom";
-    declare_parameter("odom_topic", rclcpp::ParameterValue(odom_topic));
-    get_parameter_or<std::string>("odom_topic", odom_topic, odom_topic);
-
-    if (converter_) {
-      converter_->setOdomTopic(odom_topic);
-      converter_->initialize(shared_from_this());
-      converter_->startWorker(std::make_shared<rclcpp::Rate>(5),
-                              costmap_ros_->getCostmap(), false);
-    }
-
-    pub_timer_ = n_->create_wall_timer(
-        std::chrono::milliseconds(200),
-        std::bind(&CostmapStandaloneConversion::publishCallback, this));
-  }
-
-  void publishCallback() {
-    costmap_converter::ObstacleArrayConstPtr obstacles =
-        converter_->getObstacles();
-
-    if (!obstacles) return;
-
-    obstacle_pub_->publish(*obstacles);
-
-    frame_id_ = costmap_ros_->getGlobalFrameID();
-
-    publishAsMarker(frame_id_, *obstacles);
-  }
-
-  void publishAsMarker(
-      const std::string &frame_id,
-      const std::vector<geometry_msgs::msg::PolygonStamped> &polygonStamped) {
-    visualization_msgs::msg::Marker line_list;
-    line_list.header.frame_id = frame_id;
-    line_list.header.stamp = now();
-    line_list.ns = "Polygons";
-    line_list.action = visualization_msgs::msg::Marker::ADD;
-    line_list.pose.orientation.w = 1.0;
-
-    line_list.id = 0;
-    line_list.type = visualization_msgs::msg::Marker::LINE_LIST;
-
-    line_list.scale.x = 0.1;
-    line_list.color.g = 1.0;
-    line_list.color.a = 1.0;
-
-    for (std::size_t i = 0; i < polygonStamped.size(); ++i) {
-      for (int j = 0; j < (int)polygonStamped[i].polygon.points.size() - 1;
-           ++j) {
-        geometry_msgs::msg::Point line_start;
-        line_start.x = polygonStamped[i].polygon.points[j].x;
-        line_start.y = polygonStamped[i].polygon.points[j].y;
-        line_list.points.push_back(line_start);
-        geometry_msgs::msg::Point line_end;
-        line_end.x = polygonStamped[i].polygon.points[j + 1].x;
-        line_end.y = polygonStamped[i].polygon.points[j + 1].y;
-        line_list.points.push_back(line_end);
-      }
-      // close loop for current polygon
-      if (!polygonStamped[i].polygon.points.empty() &&
-          polygonStamped[i].polygon.points.size() != 2) {
-        geometry_msgs::msg::Point line_start;
-        line_start.x = polygonStamped[i].polygon.points.back().x;
-        line_start.y = polygonStamped[i].polygon.points.back().y;
-        line_list.points.push_back(line_start);
-        if (line_list.points.size() % 2 != 0) {
-          geometry_msgs::msg::Point line_end;
-          line_end.x = polygonStamped[i].polygon.points.front().x;
-          line_end.y = polygonStamped[i].polygon.points.front().y;
-          line_list.points.push_back(line_end);
-        }
-      }
-    }
-    marker_pub_->publish(line_list);
-  }
-
-  void publishAsMarker(
-      const std::string &frame_id,
-      const costmap_converter_msgs::msg::ObstacleArrayMsg &obstacles) {
-    visualization_msgs::msg::Marker line_list;
-    line_list.header.frame_id = frame_id;
-    line_list.header.stamp = now();
-    line_list.ns = "Polygons";
-    line_list.action = visualization_msgs::msg::Marker::ADD;
-    line_list.pose.orientation.w = 1.0;
-
-    line_list.id = 0;
-    line_list.type = visualization_msgs::msg::Marker::LINE_LIST;
-
-    line_list.scale.x = 0.1;
-    line_list.color.g = 1.0;
-    line_list.color.a = 1.0;
-
-    for (const auto &obstacle : obstacles.obstacles) {
-      for (int j = 0; j < (int)obstacle.polygon.points.size() - 1; ++j) {
-        geometry_msgs::msg::Point line_start;
-        line_start.x = obstacle.polygon.points[j].x;
-        line_start.y = obstacle.polygon.points[j].y;
-        line_list.points.push_back(line_start);
-        geometry_msgs::msg::Point line_end;
-        line_end.x = obstacle.polygon.points[j + 1].x;
-        line_end.y = obstacle.polygon.points[j + 1].y;
-        line_list.points.push_back(line_end);
-      }
-      // close loop for current polygon
-      if (!obstacle.polygon.points.empty() &&
-          obstacle.polygon.points.size() != 2) {
-        geometry_msgs::msg::Point line_start;
-        line_start.x = obstacle.polygon.points.back().x;
-        line_start.y = obstacle.polygon.points.back().y;
-        line_list.points.push_back(line_start);
-        if (line_list.points.size() % 2 != 0) {
-          geometry_msgs::msg::Point line_end;
-          line_end.x = obstacle.polygon.points.front().x;
-          line_end.y = obstacle.polygon.points.front().y;
-          line_list.points.push_back(line_end);
-        }
-      }
-    }
-    marker_pub_->publish(line_list);
-  }
-
- private:
-  pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons>
-      converter_loader_;
-  std::shared_ptr<costmap_converter::BaseCostmapToPolygons> converter_;
-
-  rclcpp::Node::SharedPtr n_;
-  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
-  std::unique_ptr<std::thread> costmap_thread_;
-  rclcpp::Publisher<costmap_converter_msgs::msg::ObstacleArrayMsg>::SharedPtr
-      obstacle_pub_;
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;
-  rclcpp::TimerBase::SharedPtr pub_timer_;
-
-  std::string frame_id_;
-  int occupied_min_value_;
-};
-
-int main(int argc, char **argv) {
-  rclcpp::init(argc, argv);
-
-  auto convert_process =
-      std::make_shared<CostmapStandaloneConversion>("costmap_converter");
-
-  rclcpp::spin(convert_process);
-
-  return 0;
-}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/background_subtractor.cpp

@@ -1,115 +0,0 @@
-#include <opencv2/highgui/highgui.hpp>
-//#include <opencv2/cvv/cvv.hpp>
-#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
-
-BackgroundSubtractor::BackgroundSubtractor(const Params &parameters): params_(parameters)
-{
-}
-
-void BackgroundSubtractor::apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x, int shift_y)
-{
-  current_frame_ = image;
-
-  // occupancy grids are empty only once in the beginning -> initialize variables
-  if (occupancy_grid_fast_.empty() && occupancy_grid_slow_.empty())
-  {
-    occupancy_grid_fast_ = current_frame_;
-    occupancy_grid_slow_ = current_frame_;
-    previous_shift_x_ = shift_x;
-    previous_shift_y_ = shift_y;
-    return;
-  }
-
-  // Shift previous occupancy grid to new location (match current_frame_)
-  int shift_relative_to_previous_pos_x_ = shift_x - previous_shift_x_;
-  int shift_relative_to_previous_pos_y_ = shift_y - previous_shift_y_;
-  previous_shift_x_ = shift_x;
-  previous_shift_y_ = shift_y;
-
-  // if(shift_relative_to_previous_pos_x_ != 0 || shift_relative_to_previous_pos_y_ != 0)
-  transformToCurrentFrame(shift_relative_to_previous_pos_x_, shift_relative_to_previous_pos_y_);
-
-  // cvv::debugFilter(occupancy_grid_fast_, currentFrame_, CVVISUAL_LOCATION);
-
-  // Calculate normalized sum (mean) of nearest neighbors
-  int size = 3; // 3, 5, 7, ....
-  cv::Mat nearest_neighbor_mean_fast(occupancy_grid_fast_.size(), CV_8UC1);
-  cv::Mat nearest_neighbor_mean_slow(occupancy_grid_slow_.size(), CV_8UC1);
-  cv::boxFilter(occupancy_grid_fast_, nearest_neighbor_mean_fast, -1, cv::Size(size, size), cv::Point(-1, -1), true,
-                cv::BORDER_REPLICATE);
-  cv::boxFilter(occupancy_grid_slow_, nearest_neighbor_mean_slow, -1, cv::Size(size, size), cv::Point(-1, -1), true,
-                cv::BORDER_REPLICATE);
-  //  cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);
-  //  cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);
-
-  // compute time mean value for each pixel according to learningrate alpha
-  // occupancy_grid_fast_ = beta*(alpha_fast*current_frame_ + (1.0-alpha_fast)*occupancy_grid_fast_) + (1-beta)*nearest_neighbor_mean_fast;
-  cv::addWeighted(current_frame_, params_.alpha_fast, occupancy_grid_fast_, (1 - params_.alpha_fast), 0, occupancy_grid_fast_);
-  cv::addWeighted(occupancy_grid_fast_, params_.beta, nearest_neighbor_mean_fast, (1 - params_.beta), 0, occupancy_grid_fast_);
-  // occupancy_grid_slow_ = beta*(alpha_slow*current_frame_ + (1.0-alpha_slow)*occupancy_grid_slow) + (1-beta)*nearest_neighbor_mean_slow;
-  cv::addWeighted(current_frame_, params_.alpha_slow, occupancy_grid_slow_, (1 - params_.alpha_slow), 0, occupancy_grid_slow_);
-  cv::addWeighted(occupancy_grid_slow_, params_.beta, nearest_neighbor_mean_slow, (1 - params_.beta), 0, occupancy_grid_slow_);
-
-  // 1) Obstacles should be detected after a minimum response of the fast filter
-  //    occupancy_grid_fast_ > min_occupancy_probability
-  cv::threshold(occupancy_grid_fast_, occupancy_grid_fast_, params_.min_occupancy_probability, 0 /*unused*/, cv::THRESH_TOZERO);
-  // 2) Moving obstacles have a minimum difference between the responses of the slow and fast filter
-  //    occupancy_grid_fast_-occupancy_grid_slow_ > min_sep_between_fast_and_slow_filter
-  cv::threshold(occupancy_grid_fast_ - occupancy_grid_slow_, fg_mask, params_.min_sep_between_fast_and_slow_filter, 255,
-                cv::THRESH_BINARY);
-  // 3) Dismiss static obstacles
-  //    nearest_neighbor_mean_slow < max_occupancy_neighbors
-  cv::threshold(nearest_neighbor_mean_slow, nearest_neighbor_mean_slow, params_.max_occupancy_neighbors, 255, cv::THRESH_BINARY_INV);
-  cv::bitwise_and(nearest_neighbor_mean_slow, fg_mask, fg_mask);
-
-  //visualize("Current frame", currentFrame_);
-  cv::Mat setBorderToZero = cv::Mat(current_frame_.size(), CV_8UC1, 0.0);
-  int border = 5;
-  setBorderToZero(cv::Rect(border, border, current_frame_.cols-2*border, current_frame_.rows-2*border)) = 255;
-
-  cv::bitwise_and(setBorderToZero, fg_mask, fg_mask);
-
-  // cv::imwrite("/home/albers/Desktop/currentFrame.png", currentFrame_);
-  // visualize("Foreground mask", fgMask);
-
-  // Closing Operation
-  cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE,
-                                              cv::Size(2*params_.morph_size + 1, 2*params_.morph_size + 1),
-                                              cv::Point(params_.morph_size, params_.morph_size));
-  cv::dilate(fg_mask, fg_mask, element);
-  cv::dilate(fg_mask, fg_mask, element);
-  cv::erode(fg_mask, fg_mask, element);
-}
-
-void BackgroundSubtractor::transformToCurrentFrame(int shift_x, int shift_y)
-{
-  // TODO: initialize with current_frame (first observed image) rather than zeros
-
-  // Translate (shift) image in costmap coordinates
-  // in cv::Mat: shift X -> to the left; shift y -> to the top
-  cv::Mat temp_fast, temp_slow;
-  cv::Mat translation_mat = (cv::Mat_<double>(2, 3, CV_64F) << 1, 0, -shift_x, 0, 1, -shift_y);
-  cv::warpAffine(occupancy_grid_fast_, temp_fast, translation_mat, occupancy_grid_fast_.size()); // can't operate in-place
-  cv::warpAffine(occupancy_grid_slow_, temp_slow, translation_mat, occupancy_grid_slow_.size()); // can't operate in-place
-
-  // cvv::debugFilter(occupancy_grid_fast_, temp_fast);
-
-  occupancy_grid_fast_ = temp_fast;
-  occupancy_grid_slow_ = temp_slow;
-}
-
-void BackgroundSubtractor::visualize(const std::string& name, const cv::Mat& image)
-{
-  if (!image.empty())
-  {
-    cv::Mat im = image.clone();
-    cv::flip(im, im, 0);
-    cv::imshow(name, im);
-    cv::waitKey(1);
-  }
-}
-
-void BackgroundSubtractor::updateParameters(const Params &parameters)
-{
-  params_ = parameters;
-}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/blob_detector.cpp

@@ -1,193 +0,0 @@
-#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
-#include <opencv2/opencv.hpp>
-#include <iostream>
-
-BlobDetector::BlobDetector(const SimpleBlobDetector::Params& parameters) : params_(parameters) {}
-
-cv::Ptr<BlobDetector> BlobDetector::create(const cv::SimpleBlobDetector::Params& params)
-{
-  return cv::Ptr<BlobDetector> (new BlobDetector(params)); // compatibility with older versions
-  //return cv::makePtr<BlobDetector>(params);
-}
-
-void BlobDetector::detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat&)
-{
-  // TODO: support mask
-  contours_.clear();
-
-  keypoints.clear();
-  cv::Mat grayscale_image;
-  if (image.channels() == 3)
-    cv::cvtColor(image, grayscale_image, cv::COLOR_BGR2GRAY);
-  else
-    grayscale_image = image;
-
-  if (grayscale_image.type() != CV_8UC1)
-  {
-    //CV_Error(cv::Error::StsUnsupportedFormat, "Blob detector only supports 8-bit images!");
-    std::cerr << "Blob detector only supports 8-bit images!\n";
-  }
-
-  std::vector<std::vector<Center>> centers;
-  std::vector<std::vector<cv::Point>> contours;
-  for (double thresh = params_.minThreshold; thresh < params_.maxThreshold; thresh += params_.thresholdStep)
-  {
-    cv::Mat binarized_image;
-    cv::threshold(grayscale_image, binarized_image, thresh, 255, cv::THRESH_BINARY);
-
-    std::vector<Center> cur_centers;
-    std::vector<std::vector<cv::Point>> cur_contours, new_contours;
-    findBlobs(grayscale_image, binarized_image, cur_centers, cur_contours);
-    std::vector<std::vector<Center>> new_centers;
-    for (std::size_t i = 0; i < cur_centers.size(); ++i)
-    {
-      bool isNew = true;
-      for (std::size_t j = 0; j < centers.size(); ++j)
-      {
-        double dist = cv::norm(centers[j][centers[j].size() / 2].location - cur_centers[i].location);
-        isNew = dist >= params_.minDistBetweenBlobs && dist >= centers[j][centers[j].size() / 2].radius &&
-                dist >= cur_centers[i].radius;
-        if (!isNew)
-        {
-          centers[j].push_back(cur_centers[i]);
-
-          size_t k = centers[j].size() - 1;
-          while (k > 0 && centers[j][k].radius < centers[j][k - 1].radius)
-          {
-            centers[j][k] = centers[j][k - 1];
-            k--;
-          }
-          centers[j][k] = cur_centers[i];
-
-          break;
-        }
-      }
-      if (isNew)
-      {
-        new_centers.push_back(std::vector<Center>(1, cur_centers[i]));
-        new_contours.push_back(cur_contours[i]);
-      }
-    }
-    std::copy(new_centers.begin(), new_centers.end(), std::back_inserter(centers));
-    std::copy(new_contours.begin(), new_contours.end(), std::back_inserter(contours));
-  }
-
-  for (size_t i = 0; i < centers.size(); ++i)
-  {
-    if (centers[i].size() < params_.minRepeatability)
-      continue;
-    cv::Point2d sum_point(0, 0);
-    double normalizer = 0;
-    for (std::size_t j = 0; j < centers[i].size(); ++j)
-    {
-      sum_point += centers[i][j].confidence * centers[i][j].location;
-      normalizer += centers[i][j].confidence;
-    }
-    sum_point *= (1. / normalizer);
-    cv::KeyPoint kpt(sum_point, (float)(centers[i][centers[i].size() / 2].radius));
-    keypoints.push_back(kpt);
-    contours_.push_back(contours[i]);
-  }
-}
-
-void BlobDetector::findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
-                             std::vector<std::vector<cv::Point>>& cur_contours) const
-{
-  (void)image;
-  centers.clear();
-  cur_contours.clear();
-
-  std::vector<std::vector<cv::Point>> contours;
-  cv::Mat tmp_binary_image = binary_image.clone();
-  cv::findContours(tmp_binary_image, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);
-
-  for (std::size_t contour_idx = 0; contour_idx < contours.size(); ++contour_idx)
-  {
-    Center center;
-    center.confidence = 1;
-    cv::Moments moms = cv::moments(cv::Mat(contours[contour_idx]));
-    if (params_.filterByArea)
-    {
-      double area = moms.m00;
-      if (area < params_.minArea || area >= params_.maxArea)
-        continue;
-    }
-
-    if (params_.filterByCircularity)
-    {
-      double area = moms.m00;
-      double perimeter = cv::arcLength(cv::Mat(contours[contour_idx]), true);
-      double ratio = 4 * CV_PI * area / (perimeter * perimeter);
-      if (ratio < params_.minCircularity || ratio >= params_.maxCircularity)
-        continue;
-    }
-
-    if (params_.filterByInertia)
-    {
-      double denominator = std::sqrt(std::pow(2 * moms.mu11, 2) + std::pow(moms.mu20 - moms.mu02, 2));
-      const double eps = 1e-2;
-      double ratio;
-      if (denominator > eps)
-      {
-        double cosmin = (moms.mu20 - moms.mu02) / denominator;
-        double sinmin = 2 * moms.mu11 / denominator;
-        double cosmax = -cosmin;
-        double sinmax = -sinmin;
-
-        double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin;
-        double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax;
-        ratio = imin / imax;
-      }
-      else
-      {
-        ratio = 1;
-      }
-
-      if (ratio < params_.minInertiaRatio || ratio >= params_.maxInertiaRatio)
-        continue;
-
-      center.confidence = ratio * ratio;
-    }
-
-    if (params_.filterByConvexity)
-    {
-      std::vector<cv::Point> hull;
-      cv::convexHull(cv::Mat(contours[contour_idx]), hull);
-      double area = cv::contourArea(cv::Mat(contours[contour_idx]));
-      double hullArea = cv::contourArea(cv::Mat(hull));
-      double ratio = area / hullArea;
-      if (ratio < params_.minConvexity || ratio >= params_.maxConvexity)
-        continue;
-    }
-
-    if (moms.m00 == 0.0)
-      continue;
-    center.location = cv::Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00);
-
-    if (params_.filterByColor)
-    {
-      if (binary_image.at<uchar>(cvRound(center.location.y), cvRound(center.location.x)) != params_.blobColor)
-        continue;
-    }
-
-    // compute blob radius
-    {
-      std::vector<double> dists;
-      for (std::size_t point_idx = 0; point_idx < contours[contour_idx].size(); ++point_idx)
-      {
-        cv::Point2d pt = contours[contour_idx][point_idx];
-        dists.push_back(cv::norm(center.location - pt));
-      }
-      std::sort(dists.begin(), dists.end());
-      center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.;
-    }
-
-    centers.push_back(center);
-    cur_contours.push_back(contours[contour_idx]);
-  }
-}
-
-void BlobDetector::updateParameters(const Params& parameters)
-{
-  params_ = parameters;
-}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp

@@ -1,489 +0,0 @@
-#include <costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h>
-
-#include <pluginlib/class_list_macros.hpp>
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2/LinearMath/Vector3.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-
-PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToDynamicObstacles, costmap_converter::BaseCostmapToPolygons)
-
-namespace costmap_converter
-{
-
-CostmapToDynamicObstacles::CostmapToDynamicObstacles() : BaseCostmapToDynamicObstacles()
-{
-  ego_vel_.x = ego_vel_.y = ego_vel_.z = 0;
-  costmap_ = nullptr;
-//  dynamic_recfg_ = nullptr;
-}
-
-CostmapToDynamicObstacles::~CostmapToDynamicObstacles()
-{
-//  if(dynamic_recfg_ != nullptr)
-//    delete dynamic_recfg_;
-}
-
-void CostmapToDynamicObstacles::initialize(rclcpp::Node::SharedPtr nh)
-{
-  BaseCostmapToPolygons::initialize(nh);
-  
-  costmap_ = nullptr;
-
-  // We need the odometry from the robot to compensate the ego motion
-//  rclcpp::Node::SharedPtr gn; // create odom topic w.r.t. global node handle
-  odom_sub_ = nh->create_subscription<nav_msgs::msg::Odometry>(
-              odom_topic_,
-              rclcpp::SystemDefaultsQoS(),
-              std::bind(&CostmapToDynamicObstacles::odomCallback, this, std::placeholders::_1));
-
-  nh->get_parameter_or<bool>("publish_static_obstacles", publish_static_obstacles_, publish_static_obstacles_);
-
-  //////////////////////////////////
-  // Foreground detection parameters
-  BackgroundSubtractor::Params bg_sub_params;
-
-  bg_sub_params.alpha_slow = 0.3;
-  nh->get_parameter_or<double>("alpha_slow", bg_sub_params.alpha_slow, bg_sub_params.alpha_slow);
-
-  bg_sub_params.alpha_fast = 0.85;
-  nh->get_parameter_or<double>("alpha_fast", bg_sub_params.alpha_fast, bg_sub_params.alpha_fast);
-
-  bg_sub_params.beta = 0.85;
-  nh->get_parameter_or<double>("beta", bg_sub_params.beta, bg_sub_params.beta);
-
-  bg_sub_params.min_occupancy_probability = 180;
-  nh->get_parameter_or<double>("min_occupancy_probability", bg_sub_params.min_occupancy_probability, bg_sub_params.min_occupancy_probability);
-
-  bg_sub_params.min_sep_between_fast_and_slow_filter = 80;
-  nh->get_parameter_or<double>("min_sep_between_slow_and_fast_filter", bg_sub_params.min_sep_between_fast_and_slow_filter, bg_sub_params.min_sep_between_fast_and_slow_filter);
-
-  bg_sub_params.max_occupancy_neighbors = 100;
-  nh->get_parameter_or<double>("max_occupancy_neighbors", bg_sub_params.max_occupancy_neighbors, bg_sub_params.max_occupancy_neighbors);
-
-  bg_sub_params.morph_size = 1;
-  nh->get_parameter_or<int>("morph_size", bg_sub_params.morph_size, bg_sub_params.morph_size);
-
-  bg_sub_ = std::unique_ptr<BackgroundSubtractor>(new BackgroundSubtractor(bg_sub_params));
-
-  ////////////////////////////
-  // Blob detection parameters
-  BlobDetector::Params blob_det_params;
-
-  blob_det_params.filterByColor = true; // actually filterByIntensity, always true
-  blob_det_params.blobColor = 255;      // Extract light blobs
-  blob_det_params.thresholdStep = 256;  // Input for blob detection is already a binary image
-  blob_det_params.minThreshold = 127;
-  blob_det_params.maxThreshold = 255;
-  blob_det_params.minRepeatability = 1;
-
-  blob_det_params.minDistBetweenBlobs = 10;
-  nh->get_parameter_or<float>("min_distance_between_blobs", blob_det_params.minDistBetweenBlobs, blob_det_params.minDistBetweenBlobs);
-
-  blob_det_params.filterByArea = true;
-  nh->get_parameter_or<bool>("filter_by_area", blob_det_params.filterByArea, blob_det_params.filterByArea);
-
-  blob_det_params.minArea = 3; // Filter out blobs with less pixels
-  nh->get_parameter_or<float>("min_area", blob_det_params.minArea, blob_det_params.minArea);
-
-  blob_det_params.maxArea = 300;
-  nh->get_parameter_or<float>("max_area", blob_det_params.maxArea, blob_det_params.maxArea);
-
-  blob_det_params.filterByCircularity = true; // circularity = 4*pi*area/perimeter^2
-  nh->get_parameter_or<bool>("filter_by_circularity", blob_det_params.filterByCircularity, blob_det_params.filterByCircularity);
-
-  blob_det_params.minCircularity = 0.2;
-  nh->get_parameter_or<float>("min_circularity", blob_det_params.minCircularity, blob_det_params.minCircularity);
-
-  blob_det_params.maxCircularity = 1; // maximal 1 (in case of a circle)
-  nh->get_parameter_or<float>("max_circularity", blob_det_params.maxCircularity, blob_det_params.maxCircularity);
-
-  blob_det_params.filterByInertia = true; // Filter blobs based on their elongation
-  nh->get_parameter_or<bool>("filter_by_intertia", blob_det_params.filterByInertia, blob_det_params.filterByInertia);
-
-  blob_det_params.minInertiaRatio = 0.2;  // minimal 0 (in case of a line)
-  nh->get_parameter_or<float>("min_inertia_ratio", blob_det_params.minInertiaRatio, blob_det_params.minInertiaRatio);
-
-  blob_det_params.maxInertiaRatio = 1;    // maximal 1 (in case of a circle)
-  nh->get_parameter_or<float>("max_intertia_ratio", blob_det_params.maxInertiaRatio, blob_det_params.maxInertiaRatio);
-
-  blob_det_params.filterByConvexity = false; // Area of the Blob / Area of its convex hull
-  nh->get_parameter_or<bool>("filter_by_convexity", blob_det_params.filterByConvexity, blob_det_params.filterByConvexity);
-
-  blob_det_params.minConvexity = 0;          // minimal 0
-  nh->get_parameter_or<float>("min_convexity", blob_det_params.minConvexity, blob_det_params.minConvexity);
-
-  blob_det_params.maxConvexity = 1;          // maximal 1
-  nh->get_parameter_or<float>("max_convexity", blob_det_params.maxConvexity, blob_det_params.maxConvexity);
-
-  blob_det_ = BlobDetector::create(blob_det_params);
-
-  ////////////////////////////////////
-  // Tracking parameters
-  CTracker::Params tracker_params;
-  tracker_params.dt = 0.2;
-  nh->get_parameter_or<float>("dt", tracker_params.dt, tracker_params.dt);
-
-  tracker_params.dist_thresh = 60.0;
-  nh->get_parameter_or<float>("dist_thresh", tracker_params.dist_thresh, tracker_params.dist_thresh);
-
-  tracker_params.max_allowed_skipped_frames = 3;
-  nh->get_parameter_or<int>("max_allowed_skipped_frames", tracker_params.max_allowed_skipped_frames, tracker_params.max_allowed_skipped_frames);
-
-  tracker_params.max_trace_length = 10;
-  nh->get_parameter_or<int>("max_trace_length", tracker_params.max_trace_length, tracker_params.max_trace_length);
-
-  tracker_ = std::unique_ptr<CTracker>(new CTracker(tracker_params));
-
-
-  ////////////////////////////////////
-  // Static costmap conversion parameters
-  std::string static_converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH";
-  nh->get_parameter_or<std::string>("static_converter_plugin", static_converter_plugin, static_converter_plugin);
-  loadStaticCostmapConverterPlugin(static_converter_plugin, nh);
-
-
-// setup dynamic reconfigure
-//  dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>(nh);
-//  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>::CallbackType cb = boost::bind(&CostmapToDynamicObstacles::reconfigureCB, this, _1, _2);
-//  dynamic_recfg_->setCallback(cb);
-}
-
-void CostmapToDynamicObstacles::compute()
-{
-  if (costmap_mat_.empty())
-    return;
-
-  /////////////////////////// Foreground detection ////////////////////////////////////
-  // Dynamic obstacles are separated from static obstacles
-  int origin_x = round(costmap_->getOriginX() / costmap_->getResolution());
-  int origin_y = round(costmap_->getOriginY() / costmap_->getResolution());
-  // ROS_INFO("Origin x  [m]: %f    Origin_y  [m]: %f", costmap_->getOriginX(), costmap_->getOriginY());
-  // ROS_INFO("Origin x [px]: %d \t Origin_y [px]: %d", originX, originY);
-
-  bg_sub_->apply(costmap_mat_, fg_mask_, origin_x, origin_y);
-
-  // if no foreground object is detected, no ObstacleMsgs need to be published
-  if (fg_mask_.empty())
-    return;
-
-  cv::Mat bg_mat;
-  if (publish_static_obstacles_)
-  {
-    // Get static obstacles
-    bg_mat = costmap_mat_ - fg_mask_;
-    // visualize("bg_mat", bg_mat);
-  }
-
-
-  /////////////////////////////// Blob detection /////////////////////////////////////
-  // Centers and contours of Blobs are detected
-  blob_det_->detect(fg_mask_, keypoints_);
-  std::vector<std::vector<cv::Point>> contours = blob_det_->getContours();
-
-
-  ////////////////////////////// Tracking ////////////////////////////////////////////
-  // Objects are assigned to objects from previous frame based on Hungarian Algorithm
-  // Object velocities are estimated using a Kalman Filter
-  std::vector<Point_t> detected_centers(keypoints_.size());
-  for (size_t i = 0; i < keypoints_.size(); i++)
-  {
-    detected_centers.at(i).x = keypoints_.at(i).pt.x;
-    detected_centers.at(i).y = keypoints_.at(i).pt.y;
-    detected_centers.at(i).z = 0; // Currently unused!
-  }
-
-  tracker_->Update(detected_centers, contours);
-
-
-  ///////////////////////////////////// Output ///////////////////////////////////////
-  /*
-  cv::Mat fg_mask_with_keypoints = cv::Mat::zeros(fg_mask.size(), CV_8UC3);
-  cv::cvtColor(fg_mask, fg_mask_with_keypoints, cv::COLOR_GRAY2BGR);
-
-  for (int i = 0; i < (int)tracker_->tracks.size(); ++i)
-    cv::rectangle(fg_mask_with_keypoints, cv::boundingRect(tracker_->tracks[i]->getLastContour()),
-                  cv::Scalar(0, 0, 255), 1);
-
-  //visualize("fgMaskWithKeyPoints", fgMaskWithKeypoints);
-  //cv::imwrite("/home/albers/Desktop/fgMask.png", fgMask);
-  //cv::imwrite("/home/albers/Desktop/fgMaskWithKeyPoints.png", fgMaskWithKeypoints);
-  */
-
-  //////////////////////////// Fill ObstacleContainerPtr /////////////////////////////
-  ObstacleArrayPtr obstacles(new costmap_converter_msgs::msg::ObstacleArrayMsg);
-  // header.seq is automatically filled
-  obstacles->header.stamp = now();
-  obstacles->header.frame_id = "/map"; //Global frame /map
-
-  // For all tracked objects
-  for (unsigned int i = 0; i < (unsigned int)tracker_->tracks.size(); ++i)
-  {
-    geometry_msgs::msg::Polygon polygon;
-
-    // TODO directly create polygon inside getContour and avoid copy
-    std::vector<Point_t> contour;
-    getContour(i, contour); // this method also transforms map to world coordinates
-
-    // convert contour to polygon
-    for (const Point_t& pt : contour)
-    {
-      polygon.points.emplace_back();
-      polygon.points.back().x = pt.x;
-      polygon.points.back().y = pt.y;
-      polygon.points.back().z = 0;
-    }
-
-    obstacles->obstacles.emplace_back();
-    obstacles->obstacles.back().polygon = polygon;
-
-    // Set obstacle ID
-    obstacles->obstacles.back().id = tracker_->tracks.at(i)->track_id;
-
-    // Set orientation
-    geometry_msgs::msg::QuaternionStamped orientation;
-
-    Point_t vel = getEstimatedVelocityOfObject(i);
-    double yaw = std::atan2(vel.y, vel.x);
-    //ROS_INFO("yaw: %f", yaw);
-    tf2::Quaternion q;
-    q.setRPY(0, 0, yaw);
-    obstacles->obstacles.back().orientation = tf2::toMsg(q);
-
-    // Set velocity
-    geometry_msgs::msg::TwistWithCovariance velocities;
-    //velocities.twist.linear.x = std::sqrt(vel.x*vel.x + vel.y*vel.y);
-    //velocities.twist.linear.y = 0;
-    velocities.twist.linear.x = vel.x;
-    velocities.twist.linear.y = vel.y; // TODO(roesmann): don't we need to consider the transformation between opencv's and costmap's coordinate frames?
-    velocities.twist.linear.z = 0;
-    velocities.twist.angular.x = 0;
-    velocities.twist.angular.y = 0;
-    velocities.twist.angular.z = 0;
-
-    // TODO: use correct covariance matrix
-    velocities.covariance = {1, 0, 0, 0, 0, 0,
-                             0, 1, 0, 0, 0, 0,
-                             0, 0, 1, 0, 0, 0,
-                             0, 0, 0, 1, 0, 0,
-                             0, 0, 0, 0, 1, 0,
-                             0, 0, 0, 0, 0, 1};
-
-    obstacles->obstacles.back().velocities = velocities;
-  }
-
-  ////////////////////////// Static obstacles ////////////////////////////
-  if (publish_static_obstacles_)
-  {
-    uchar* img_data = bg_mat.data;
-    int width = bg_mat.cols;
-    int height = bg_mat.rows;
-    int stride = bg_mat.step;
-
-    if (stackedCostmapConversion())
-    {
-      // Create new costmap with static obstacles (background)
-      std::shared_ptr<nav2_costmap_2d::Costmap2D> static_costmap(new nav2_costmap_2d::Costmap2D(costmap_->getSizeInCellsX(),
-                                                                                        costmap_->getSizeInCellsY(),
-                                                                                        costmap_->getResolution(),
-                                                                                        costmap_->getOriginX(),
-                                                                                        costmap_->getOriginY()));
-      for(int i = 0; i < height; i++)
-      {
-        for(int j = 0; j < width; j++)
-        {
-          static_costmap->setCost(j, i, img_data[i * stride + j]);
-        }
-      }
-
-      // Apply static obstacle conversion plugin
-      setStaticCostmap(static_costmap);
-      convertStaticObstacles();
-
-      // Store converted static obstacles for publishing
-      auto static_polygons = getStaticPolygons();
-      for (auto it = static_polygons->begin(); it != static_polygons->end(); ++it)
-      {
-        obstacles->obstacles.emplace_back();
-        obstacles->obstacles.back().polygon = *it;
-        obstacles->obstacles.back().velocities.twist.linear.x = 0;
-        obstacles->obstacles.back().velocities.twist.linear.y = 0;
-        obstacles->obstacles.back().id = -1;
-      }
-    }
-    // Otherwise leave static obstacles point-shaped
-    else
-    {
-      for(int i = 0; i < height; i++)
-      {
-        for(int j = 0; j < width; j++)
-        {
-            uchar value = img_data[i * stride + j];
-            if (value > 0)
-            {
-              // obstacle found
-              obstacles->obstacles.emplace_back();
-              geometry_msgs::msg::Point32 pt;
-              pt.x = (double)j*costmap_->getResolution() + costmap_->getOriginX();
-              pt.y = (double)i*costmap_->getResolution() + costmap_->getOriginY();
-              obstacles->obstacles.back().polygon.points.push_back(pt);
-              obstacles->obstacles.back().velocities.twist.linear.x = 0;
-              obstacles->obstacles.back().velocities.twist.linear.y = 0;
-              obstacles->obstacles.back().id = -1;
-            }
-        }
-      }
-    }
-  }
-
-  updateObstacleContainer(obstacles);
-}
-
-void CostmapToDynamicObstacles::setCostmap2D(nav2_costmap_2d::Costmap2D* costmap)
-{
-  if (!costmap)
-    return;
-
-  costmap_ = costmap;
-
-  updateCostmap2D();
-}
-
-void CostmapToDynamicObstacles::updateCostmap2D()
-{
-  if (!costmap_->getMutex())
-  {
-    RCLCPP_ERROR(getLogger(), "Cannot update costmap since the mutex pointer is null");
-    return;
-  }
-  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap_->getMutex());
-
-  // Initialize costmapMat_ directly with the unsigned char array of costmap_
-  //costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
-  //                      costmap_->getCharMap()).clone(); // Deep copy: TODO
-  costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
-                        costmap_->getCharMap());
-}
-
-ObstacleArrayConstPtr CostmapToDynamicObstacles::getObstacles()
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return obstacles_;
-}
-
-void CostmapToDynamicObstacles::updateObstacleContainer(ObstacleArrayPtr obstacles)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  obstacles_ = obstacles;
-}
-
-Point_t CostmapToDynamicObstacles::getEstimatedVelocityOfObject(unsigned int idx)
-{
-  // vel [px/s] * costmapResolution [m/px] = vel [m/s]
-  Point_t vel = tracker_->tracks.at(idx)->getEstimatedVelocity() * costmap_->getResolution() + ego_vel_;
-
-  //ROS_INFO("vel x: %f, vel y: %f, vel z: %f", vel.x, vel.y, vel.z);
-  // velocity in /map frame
-  return vel;
-}
-
-void CostmapToDynamicObstacles::odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
-{
-  RCLCPP_INFO_ONCE(getLogger(), "CostmapToDynamicObstacles: odom received.");
-
-  tf2::Quaternion pose;
-  tf2::fromMsg(msg->pose.pose.orientation, pose);
-
-  tf2::Vector3 twistLinear;
-  // tf2::fromMsg(msg->twist.twist.linear, twistLinear); // not available in tf2
-  twistLinear.setX(msg->twist.twist.linear.x);
-  twistLinear.setY(msg->twist.twist.linear.y);
-  twistLinear.setZ(msg->twist.twist.linear.z);
-
-
-  // velocity of the robot in x, y and z coordinates
-  tf2::Vector3 vel = tf2::quatRotate(pose, twistLinear);
-  ego_vel_.x = vel.x();
-  ego_vel_.y = vel.y();
-  ego_vel_.z = vel.z();
-}
-
-//void CostmapToDynamicObstacles::reconfigureCB(CostmapToDynamicObstaclesConfig& config, uint32_t level)
-//{
-//  publish_static_obstacles_ = config.publish_static_obstacles;
-
-//  // BackgroundSubtraction Parameters
-//  BackgroundSubtractor::Params bg_sub_params;
-//  bg_sub_params.alpha_slow = config.alpha_slow;
-//  bg_sub_params.alpha_fast = config.alpha_fast;
-//  bg_sub_params.beta = config.beta;
-//  bg_sub_params.min_sep_between_fast_and_slow_filter = config.min_sep_between_slow_and_fast_filter;
-//  bg_sub_params.min_occupancy_probability = config.min_occupancy_probability;
-//  bg_sub_params.max_occupancy_neighbors = config.max_occupancy_neighbors;
-//  bg_sub_params.morph_size = config.morph_size;
-//  bg_sub_->updateParameters(bg_sub_params);
-
-//  // BlobDetector Parameters
-//  BlobDetector::Params blob_det_params;
-//  // necessary, because blobDetParams are otherwise initialized with default values for dark blobs
-//  blob_det_params.filterByColor = true; // actually filterByIntensity, always true
-//  blob_det_params.blobColor = 255;      // Extract light blobs
-//  blob_det_params.thresholdStep = 256;  // Input for blobDetection is already a binary image
-//  blob_det_params.minThreshold = 127;
-//  blob_det_params.maxThreshold = 255;
-//  blob_det_params.minRepeatability = 1;
-//  blob_det_params.minDistBetweenBlobs = config.min_distance_between_blobs; // TODO: Currently not working as expected
-//  blob_det_params.filterByArea = config.filter_by_area;
-//  blob_det_params.minArea = config.min_area;
-//  blob_det_params.maxArea = config.max_area;
-//  blob_det_params.filterByCircularity = config.filter_by_circularity;
-//  blob_det_params.minCircularity = config.min_circularity;
-//  blob_det_params.maxCircularity = config.max_circularity;
-//  blob_det_params.filterByInertia = config.filter_by_inertia;
-//  blob_det_params.minInertiaRatio = config.min_inertia_ratio;
-//  blob_det_params.maxInertiaRatio = config.max_inertia_ratio;
-//  blob_det_params.filterByConvexity = config.filter_by_convexity;
-//  blob_det_params.minConvexity = config.min_convexity;
-//  blob_det_params.maxConvexity = config.max_convexity;
-//  blob_det_->updateParameters(blob_det_params);
-
-//  // Tracking Parameters
-//  CTracker::Params tracking_params;
-//  tracking_params.dt = config.dt;
-//  tracking_params.dist_thresh = config.dist_thresh;
-//  tracking_params.max_allowed_skipped_frames = config.max_allowed_skipped_frames;
-//  tracking_params.max_trace_length = config.max_trace_length;
-//  tracker_->updateParameters(tracking_params);
-//}
-
-void CostmapToDynamicObstacles::getContour(unsigned int idx, std::vector<Point_t>& contour)
-{
-  assert(!tracker_->tracks.empty() && idx < tracker_->tracks.size());
-
-  contour.clear();
-
-  // contour [px] * costmapResolution [m/px] = contour [m]
-  std::vector<cv::Point> contour2i = tracker_->tracks.at(idx)->getLastContour();
-
-  contour.reserve(contour2i.size());
-
-  Point_t costmap_origin(costmap_->getOriginX(), costmap_->getOriginY(), 0);
-
-  for (std::size_t i = 0; i < contour2i.size(); ++i)
-  {
-    contour.push_back((Point_t(contour2i.at(i).x, contour2i.at(i).y, 0.0)*costmap_->getResolution())
-                        + costmap_origin); // Shift to /map
-  }
-
-}
-
-void CostmapToDynamicObstacles::visualize(const std::string& name, const cv::Mat& image)
-{
-  if (!image.empty())
-  {
-    cv::Mat im = image.clone();
-    cv::flip(im, im, 0);
-    cv::imshow(name, im);
-    cv::waitKey(1);
-  }
-}
-}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp

@@ -1,130 +0,0 @@
-// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
-// Refer to README.md in this directory.
-
-#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
-
-// ---------------------------------------------------------------------------
-// Tracker. Manage tracks. Create, remove, update.
-// ---------------------------------------------------------------------------
-CTracker::CTracker(const Params &parameters)
-    : params(parameters),
-      NextTrackID(0)
-{
-}
-// ---------------------------------------------------------------------------
-//
-// ---------------------------------------------------------------------------
-void CTracker::Update(const std::vector<Point_t>& detectedCentroid, const std::vector< std::vector<cv::Point> >& contours)
-{
-  // Each contour has a centroid
-  assert(detectedCentroid.size() == contours.size());
-
-  // -----------------------------------
-  // If there is no tracks yet, then every cv::Point begins its own track.
-  // -----------------------------------
-  if (tracks.size() == 0)
-  {
-    // If no tracks yet
-    for (size_t i = 0; i < detectedCentroid.size(); ++i)
-    {
-      tracks.push_back(
-          std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
-    }
-  }
-
-  size_t N = tracks.size();
-  size_t M = detectedCentroid.size();
-
-  assignments_t assignment;
-
-  if (!tracks.empty())
-  {
-    // Distance matrix of N-th Track to the M-th detectedCentroid
-    distMatrix_t Cost(N * M);
-
-    // calculate distance between the blobs centroids
-    for (size_t i = 0; i < tracks.size(); i++)
-    {
-      for (size_t j = 0; j < detectedCentroid.size(); j++)
-      {
-        Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]);
-      }
-    }
-
-    // -----------------------------------
-    // Solving assignment problem (tracks and predictions of Kalman filter)
-    // -----------------------------------
-    AssignmentProblemSolver APS;
-    APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);
-
-    // -----------------------------------
-    // clean assignment from pairs with large distance
-    // -----------------------------------
-    for (size_t i = 0; i < assignment.size(); i++)
-    {
-      if (assignment[i] != -1)
-      {
-        if (Cost[i + assignment[i] * N] > params.dist_thresh)
-        {
-          assignment[i] = -1;
-          tracks[i]->skipped_frames = 1;
-        }
-      }
-      else
-      {
-        // If track have no assigned detect, then increment skipped frames counter.
-        tracks[i]->skipped_frames++;
-      }
-    }
-
-    // -----------------------------------
-    // If track didn't get detects long time, remove it.
-    // -----------------------------------
-    for (int i = 0; i < static_cast<int>(tracks.size()); i++)
-    {
-      if ((int)tracks[i]->skipped_frames > params.max_allowed_skipped_frames)
-      {
-        tracks.erase(tracks.begin() + i);
-        assignment.erase(assignment.begin() + i);
-        i--;
-      }
-    }
-  }
-
-  // -----------------------------------
-  // Search for unassigned detects and start new tracks for them.
-  // -----------------------------------
-  for (size_t i = 0; i < detectedCentroid.size(); ++i)
-  {
-    if (find(assignment.begin(), assignment.end(), i) == assignment.end())
-    {
-      tracks.push_back(std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
-    }
-  }
-
-  // Update Kalman Filters state
-
-  for (size_t i = 0; i < assignment.size(); i++)
-  {
-    // If track updated less than one time, than filter state is not correct.
-
-    if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
-    {
-      tracks[i]->skipped_frames = 0;
-      tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length);
-    }
-    else // if not continue using predictions
-    {
-      tracks[i]->Update(Point_t(), std::vector<cv::Point>(), false, params.max_trace_length);
-    }
-  }
-}
-
-void CTracker::updateParameters(const Params &parameters)
-{
-  params = parameters;
-}
-// ---------------------------------------------------------------------------
-//
-// ---------------------------------------------------------------------------
-CTracker::~CTracker(void) {}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp

@@ -1,732 +0,0 @@
-// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
-// Refer to README.md in this directory.
-
-#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h>
-#include <limits>
-
-AssignmentProblemSolver::AssignmentProblemSolver() {}
-
-AssignmentProblemSolver::~AssignmentProblemSolver() {}
-
-track_t AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns,
-                                       std::vector<int>& assignment, TMethod Method)
-{
-  assignment.resize(nOfRows, -1);
-
-  track_t cost = 0;
-
-  switch (Method)
-  {
-  case optimal:
-    assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
-    break;
-
-  case many_forbidden_assignments:
-    assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
-    break;
-
-  case without_forbidden_assignments:
-    assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
-    break;
-  }
-
-  return cost;
-}
-// --------------------------------------------------------------------------
-// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost,
-                                                const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
-{
-  // Generate distance cv::Matrix
-  // and check cv::Matrix elements positiveness :)
-
-  // Total elements number
-  size_t nOfElements = nOfRows * nOfColumns;
-  // Memory allocation
-  track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t));
-  // Pointer to last element
-  track_t* distMatrixEnd = distMatrix + nOfElements;
-
-  for (size_t row = 0; row < nOfElements; row++)
-  {
-    track_t value = distMatrixIn[row];
-    assert(value >= 0);
-    distMatrix[row] = value;
-  }
-
-  // Memory allocation
-  bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));
-  bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool));
-  bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool));
-  bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));
-  bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */
-
-  /* preliminary steps */
-  if (nOfRows <= nOfColumns)
-  {
-    for (size_t row = 0; row < nOfRows; row++)
-    {
-      /* find the smallest element in the row */
-      track_t* distMatrixTemp = distMatrix + row;
-      track_t minValue = *distMatrixTemp;
-      distMatrixTemp += nOfRows;
-      while (distMatrixTemp < distMatrixEnd)
-      {
-        track_t value = *distMatrixTemp;
-        if (value < minValue)
-        {
-          minValue = value;
-        }
-        distMatrixTemp += nOfRows;
-      }
-      /* subtract the smallest element from each element of the row */
-      distMatrixTemp = distMatrix + row;
-      while (distMatrixTemp < distMatrixEnd)
-      {
-        *distMatrixTemp -= minValue;
-        distMatrixTemp += nOfRows;
-      }
-    }
-    /* Steps 1 and 2a */
-    for (size_t row = 0; row < nOfRows; row++)
-    {
-      for (size_t col = 0; col < nOfColumns; col++)
-      {
-        if (distMatrix[row + nOfRows * col] == 0)
-        {
-          if (!coveredColumns[col])
-          {
-            starMatrix[row + nOfRows * col] = true;
-            coveredColumns[col] = true;
-            break;
-          }
-        }
-      }
-    }
-  }
-  else /* if(nOfRows > nOfColumns) */
-  {
-    for (size_t col = 0; col < nOfColumns; col++)
-    {
-      /* find the smallest element in the column */
-      track_t* distMatrixTemp = distMatrix + nOfRows * col;
-      track_t* columnEnd = distMatrixTemp + nOfRows;
-      track_t minValue = *distMatrixTemp++;
-      while (distMatrixTemp < columnEnd)
-      {
-        track_t value = *distMatrixTemp++;
-        if (value < minValue)
-        {
-          minValue = value;
-        }
-      }
-      /* subtract the smallest element from each element of the column */
-      distMatrixTemp = distMatrix + nOfRows * col;
-      while (distMatrixTemp < columnEnd)
-      {
-        *distMatrixTemp++ -= minValue;
-      }
-    }
-    /* Steps 1 and 2a */
-    for (size_t col = 0; col < nOfColumns; col++)
-    {
-      for (size_t row = 0; row < nOfRows; row++)
-      {
-        if (distMatrix[row + nOfRows * col] == 0)
-        {
-          if (!coveredRows[row])
-          {
-            starMatrix[row + nOfRows * col] = true;
-            coveredColumns[col] = true;
-            coveredRows[row] = true;
-            break;
-          }
-        }
-      }
-    }
-
-    for (size_t row = 0; row < nOfRows; row++)
-    {
-      coveredRows[row] = false;
-    }
-  }
-  /* move to step 2b */
-  step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
-         nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns);
-  /* compute cost and remove invalid assignments */
-  computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
-  /* free allocated memory */
-  free(distMatrix);
-  free(coveredColumns);
-  free(coveredRows);
-  free(starMatrix);
-  free(primeMatrix);
-  free(newStarMatrix);
-  return;
-}
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows,
-                                                    size_t nOfColumns)
-{
-  for (size_t row = 0; row < nOfRows; row++)
-  {
-    for (size_t col = 0; col < nOfColumns; col++)
-    {
-      if (starMatrix[row + nOfRows * col])
-      {
-        assignment[row] = static_cast<int>(col);
-        break;
-      }
-    }
-  }
-}
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost,
-                                                    const distMatrix_t& distMatrixIn, size_t nOfRows)
-{
-  for (size_t row = 0; row < nOfRows; row++)
-  {
-    const int col = assignment[row];
-    if (col >= 0)
-    {
-      cost += distMatrixIn[row + nOfRows * col];
-    }
-  }
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
-                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
-                                     size_t nOfRows, size_t nOfColumns, size_t minDim)
-{
-  bool* starMatrixTemp, *columnEnd;
-  /* cover every column containing a starred zero */
-  for (size_t col = 0; col < nOfColumns; col++)
-  {
-    starMatrixTemp = starMatrix + nOfRows * col;
-    columnEnd = starMatrixTemp + nOfRows;
-    while (starMatrixTemp < columnEnd)
-    {
-      if (*starMatrixTemp++)
-      {
-        coveredColumns[col] = true;
-        break;
-      }
-    }
-  }
-  /* move to step 3 */
-  step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
-         nOfColumns, minDim);
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
-                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
-                                     size_t nOfRows, size_t nOfColumns, size_t minDim)
-{
-  /* count covered columns */
-  size_t nOfCoveredColumns = 0;
-  for (size_t col = 0; col < nOfColumns; col++)
-  {
-    if (coveredColumns[col])
-    {
-      nOfCoveredColumns++;
-    }
-  }
-  if (nOfCoveredColumns == minDim)
-  {
-    /* algorithm finished */
-    buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
-  }
-  else
-  {
-    /* move to step 3 */
-    step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
-          nOfColumns, minDim);
-  }
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
-                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
-                                    size_t nOfRows, size_t nOfColumns, size_t minDim)
-{
-  bool zerosFound = true;
-  while (zerosFound)
-  {
-    zerosFound = false;
-    for (size_t col = 0; col < nOfColumns; col++)
-    {
-      if (!coveredColumns[col])
-      {
-        for (size_t row = 0; row < nOfRows; row++)
-        {
-          if ((!coveredRows[row]) && (distMatrix[row + nOfRows * col] == 0))
-          {
-            /* prime zero */
-            primeMatrix[row + nOfRows * col] = true;
-            /* find starred zero in current row */
-            size_t starCol = 0;
-            for (; starCol < nOfColumns; starCol++)
-            {
-              if (starMatrix[row + nOfRows * starCol])
-              {
-                break;
-              }
-            }
-            if (starCol == nOfColumns) /* no starred zero found */
-            {
-              /* move to step 4 */
-              step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows,
-                    nOfRows, nOfColumns, minDim, row, col);
-              return;
-            }
-            else
-            {
-              coveredRows[row] = true;
-              coveredColumns[starCol] = false;
-              zerosFound = true;
-              break;
-            }
-          }
-        }
-      }
-    }
-  }
-  /* move to step 5 */
-  step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
-        nOfColumns, minDim);
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
-                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
-                                    size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col)
-{
-  const size_t nOfElements = nOfRows * nOfColumns;
-  /* generate temporary copy of starMatrix */
-  for (size_t n = 0; n < nOfElements; n++)
-  {
-    newStarMatrix[n] = starMatrix[n];
-  }
-  /* star current zero */
-  newStarMatrix[row + nOfRows * col] = true;
-  /* find starred zero in current column */
-  size_t starCol = col;
-  size_t starRow = 0;
-  for (; starRow < nOfRows; starRow++)
-  {
-    if (starMatrix[starRow + nOfRows * starCol])
-    {
-      break;
-    }
-  }
-  while (starRow < nOfRows)
-  {
-    /* unstar the starred zero */
-    newStarMatrix[starRow + nOfRows * starCol] = false;
-    /* find primed zero in current row */
-    size_t primeRow = starRow;
-    size_t primeCol = 0;
-    for (; primeCol < nOfColumns; primeCol++)
-    {
-      if (primeMatrix[primeRow + nOfRows * primeCol])
-      {
-        break;
-      }
-    }
-    /* star the primed zero */
-    newStarMatrix[primeRow + nOfRows * primeCol] = true;
-    /* find starred zero in current column */
-    starCol = primeCol;
-    for (starRow = 0; starRow < nOfRows; starRow++)
-    {
-      if (starMatrix[starRow + nOfRows * starCol])
-      {
-        break;
-      }
-    }
-  }
-  /* use temporary copy as new starMatrix */
-  /* delete all primes, uncover all rows */
-  for (size_t n = 0; n < nOfElements; n++)
-  {
-    primeMatrix[n] = false;
-    starMatrix[n] = newStarMatrix[n];
-  }
-  for (size_t n = 0; n < nOfRows; n++)
-  {
-    coveredRows[n] = false;
-  }
-  /* move to step 2a */
-  step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
-         nOfColumns, minDim);
-}
-
-// --------------------------------------------------------------------------
-//
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
-                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
-                                    size_t nOfRows, size_t nOfColumns, size_t minDim)
-{
-  /* find smallest uncovered element h */
-  float h = std::numeric_limits<track_t>::max();
-  for (size_t row = 0; row < nOfRows; row++)
-  {
-    if (!coveredRows[row])
-    {
-      for (size_t col = 0; col < nOfColumns; col++)
-      {
-        if (!coveredColumns[col])
-        {
-          const float value = distMatrix[row + nOfRows * col];
-          if (value < h)
-          {
-            h = value;
-          }
-        }
-      }
-    }
-  }
-  /* add h to each covered row */
-  for (size_t row = 0; row < nOfRows; row++)
-  {
-    if (coveredRows[row])
-    {
-      for (size_t col = 0; col < nOfColumns; col++)
-      {
-        distMatrix[row + nOfRows * col] += h;
-      }
-    }
-  }
-  /* subtract h from each uncovered column */
-  for (size_t col = 0; col < nOfColumns; col++)
-  {
-    if (!coveredColumns[col])
-    {
-      for (size_t row = 0; row < nOfRows; row++)
-      {
-        distMatrix[row + nOfRows * col] -= h;
-      }
-    }
-  }
-  /* move to step 3 */
-  step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
-        nOfColumns, minDim);
-}
-
-// --------------------------------------------------------------------------
-// Computes a suboptimal solution. Good for cases without forbidden assignments.
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost,
-                                                    const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
-{
-  /* make working copy of distance Matrix */
-  const size_t nOfElements = nOfRows * nOfColumns;
-  float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
-  for (size_t n = 0; n < nOfElements; n++)
-  {
-    distMatrix[n] = distMatrixIn[n];
-  }
-
-  /* recursively search for the minimum element and do the assignment */
-  for (;;)
-  {
-    /* find minimum distance observation-to-track pair */
-    float minValue = std::numeric_limits<track_t>::max();
-    size_t tmpRow = 0;
-    size_t tmpCol = 0;
-    for (size_t row = 0; row < nOfRows; row++)
-    {
-      for (size_t col = 0; col < nOfColumns; col++)
-      {
-        const float value = distMatrix[row + nOfRows * col];
-        if (value != std::numeric_limits<track_t>::max() && (value < minValue))
-        {
-          minValue = value;
-          tmpRow = row;
-          tmpCol = col;
-        }
-      }
-    }
-
-    if (minValue != std::numeric_limits<track_t>::max())
-    {
-      assignment[tmpRow] = static_cast<int>(tmpCol);
-      cost += minValue;
-      for (size_t n = 0; n < nOfRows; n++)
-      {
-        distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
-      }
-      for (size_t n = 0; n < nOfColumns; n++)
-      {
-        distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
-      }
-    }
-    else
-    {
-      break;
-    }
-  }
-
-  free(distMatrix);
-}
-// --------------------------------------------------------------------------
-// Computes a suboptimal solution. Good for cases with many forbidden assignments.
-// --------------------------------------------------------------------------
-void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost,
-                                                    const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
-{
-  /* make working copy of distance Matrix */
-  const size_t nOfElements = nOfRows * nOfColumns;
-  float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
-  for (size_t n = 0; n < nOfElements; n++)
-  {
-    distMatrix[n] = distMatrixIn[n];
-  }
-
-  /* allocate memory */
-  int* nOfValidObservations = (int*)calloc(nOfRows, sizeof(int));
-  int* nOfValidTracks = (int*)calloc(nOfColumns, sizeof(int));
-
-  /* compute number of validations */
-  bool infiniteValueFound = false;
-  bool finiteValueFound = false;
-  for (size_t row = 0; row < nOfRows; row++)
-  {
-    for (size_t col = 0; col < nOfColumns; col++)
-    {
-      if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
-      {
-        nOfValidTracks[col] += 1;
-        nOfValidObservations[row] += 1;
-        finiteValueFound = true;
-      }
-      else
-      {
-        infiniteValueFound = true;
-      }
-    }
-  }
-
-  if (infiniteValueFound)
-  {
-    if (!finiteValueFound)
-    {
-      free(nOfValidTracks);
-      free(nOfValidObservations);
-      free(distMatrix);
-      return;
-    }
-    bool repeatSteps = true;
-
-    while (repeatSteps)
-    {
-      repeatSteps = false;
-
-      /* step 1: reject assignments of multiply validated tracks to singly validated observations		 */
-      for (size_t col = 0; col < nOfColumns; col++)
-      {
-        bool singleValidationFound = false;
-        for (size_t row = 0; row < nOfRows; row++)
-        {
-          if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() &&
-              (nOfValidObservations[row] == 1))
-          {
-            singleValidationFound = true;
-            break;
-          }
-
-          if (singleValidationFound)
-          {
-            for (size_t row = 0; row < nOfRows; row++)
-              if ((nOfValidObservations[row] > 1) &&
-                  distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
-              {
-                distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
-                nOfValidObservations[row] -= 1;
-                nOfValidTracks[col] -= 1;
-                repeatSteps = true;
-              }
-          }
-        }
-      }
-
-      /* step 2: reject assignments of multiply validated observations to singly validated tracks */
-      if (nOfColumns > 1)
-      {
-        for (size_t row = 0; row < nOfRows; row++)
-        {
-          bool singleValidationFound = false;
-          for (size_t col = 0; col < nOfColumns; col++)
-          {
-            if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() && (nOfValidTracks[col] == 1))
-            {
-              singleValidationFound = true;
-              break;
-            }
-          }
-
-          if (singleValidationFound)
-          {
-            for (size_t col = 0; col < nOfColumns; col++)
-            {
-              if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
-              {
-                distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
-                nOfValidObservations[row] -= 1;
-                nOfValidTracks[col] -= 1;
-                repeatSteps = true;
-              }
-            }
-          }
-        }
-      }
-    } /* while(repeatSteps) */
-
-    /* for each multiply validated track that validates only with singly validated  */
-    /* observations, choose the observation with minimum distance */
-    for (size_t row = 0; row < nOfRows; row++)
-    {
-      if (nOfValidObservations[row] > 1)
-      {
-        bool allSinglyValidated = true;
-        float minValue = std::numeric_limits<track_t>::max();
-        size_t tmpCol = 0;
-        for (size_t col = 0; col < nOfColumns; col++)
-        {
-          const float value = distMatrix[row + nOfRows * col];
-          if (value != std::numeric_limits<track_t>::max())
-          {
-            if (nOfValidTracks[col] > 1)
-            {
-              allSinglyValidated = false;
-              break;
-            }
-            else if ((nOfValidTracks[col] == 1) && (value < minValue))
-            {
-              tmpCol = col;
-              minValue = value;
-            }
-          }
-        }
-
-        if (allSinglyValidated)
-        {
-          assignment[row] = static_cast<int>(tmpCol);
-          cost += minValue;
-          for (size_t n = 0; n < nOfRows; n++)
-          {
-            distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
-          }
-          for (size_t n = 0; n < nOfColumns; n++)
-          {
-            distMatrix[row + nOfRows * n] = std::numeric_limits<track_t>::max();
-          }
-        }
-      }
-    }
-
-    // for each multiply validated observation that validates only with singly validated  track, choose the track with
-    // minimum distance
-    for (size_t col = 0; col < nOfColumns; col++)
-    {
-      if (nOfValidTracks[col] > 1)
-      {
-        bool allSinglyValidated = true;
-        float minValue = std::numeric_limits<track_t>::max();
-        size_t tmpRow = 0;
-        for (size_t row = 0; row < nOfRows; row++)
-        {
-          const float value = distMatrix[row + nOfRows * col];
-          if (value != std::numeric_limits<track_t>::max())
-          {
-            if (nOfValidObservations[row] > 1)
-            {
-              allSinglyValidated = false;
-              break;
-            }
-            else if ((nOfValidObservations[row] == 1) && (value < minValue))
-            {
-              tmpRow = row;
-              minValue = value;
-            }
-          }
-        }
-
-        if (allSinglyValidated)
-        {
-          assignment[tmpRow] = static_cast<int>(col);
-          cost += minValue;
-          for (size_t n = 0; n < nOfRows; n++)
-          {
-            distMatrix[n + nOfRows * col] = std::numeric_limits<track_t>::max();
-          }
-          for (size_t n = 0; n < nOfColumns; n++)
-          {
-            distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
-          }
-        }
-      }
-    }
-  } /* if(infiniteValueFound) */
-
-  /* now, recursively search for the minimum element and do the assignment */
-  for (;;)
-  {
-    /* find minimum distance observation-to-track pair */
-    float minValue = std::numeric_limits<track_t>::max();
-    size_t tmpRow = 0;
-    size_t tmpCol = 0;
-    for (size_t row = 0; row < nOfRows; row++)
-    {
-      for (size_t col = 0; col < nOfColumns; col++)
-      {
-        const float value = distMatrix[row + nOfRows * col];
-        if (value != std::numeric_limits<track_t>::max() && (value < minValue))
-        {
-          minValue = value;
-          tmpRow = row;
-          tmpCol = col;
-        }
-      }
-    }
-
-    if (minValue != std::numeric_limits<track_t>::max())
-    {
-      assignment[tmpRow] = static_cast<int>(tmpCol);
-      cost += minValue;
-      for (size_t n = 0; n < nOfRows; n++)
-      {
-        distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
-      }
-      for (size_t n = 0; n < nOfColumns; n++)
-      {
-        distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
-      }
-    }
-    else
-    {
-      break;
-    }
-  }
-
-  /* free allocated memory */
-  free(nOfValidObservations);
-  free(nOfValidTracks);
-  free(distMatrix);
-}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp

@@ -1,99 +0,0 @@
-// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
-// Refer to README.md in this directory.
-
-#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h>
-#include "opencv2/opencv.hpp"
-#include <iostream>
-#include <vector>
-
-//---------------------------------------------------------------------------
-//---------------------------------------------------------------------------
-TKalmanFilter::TKalmanFilter(Point_t pt, track_t deltatime)
-{
-  // time increment (lower values makes target more "massive")
-  dt = deltatime;
-
-  // 6 state variables [x y z xdot ydot zdot], 3 measurements [x y z]
-  kalman = new cv::KalmanFilter(6, 3, 0);
-  // Transition cv::Matrix
-  kalman->transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
-                              1, 0, 0, dt,  0,  0,
-                              0, 1, 0,  0, dt,  0,
-                              0, 0, 1,  0,  0, dt,
-                              0, 0, 0,  1,  0,  0,
-                              0, 0, 0,  0,  1,  0,
-                              0, 0, 0,  0,  0,  1);
-  // init...
-  LastPosition = pt;
-  kalman->statePre.at<track_t>(0) = pt.x;
-  kalman->statePre.at<track_t>(1) = pt.y;
-  kalman->statePre.at<track_t>(2) = pt.z;
-
-  kalman->statePre.at<track_t>(3) = 0;
-  kalman->statePre.at<track_t>(4) = 0;
-  kalman->statePre.at<track_t>(5) = 0;
-
-  kalman->statePost.at<track_t>(0) = pt.x;
-  kalman->statePost.at<track_t>(1) = pt.y;
-  kalman->statePost.at<track_t>(2) = pt.z;
-
-  // Nur x, y und z Koordinaten messbar!
-  kalman->measurementMatrix = (cv::Mat_<track_t>(3, 6) <<
-                               1, 0, 0, 0, 0, 0,
-                               0, 1, 0, 0, 0, 0,
-                               0, 0, 1, 0, 0, 0);
-
-  float sigma = 0.5; // Assume standard deviation for acceleration, todo: dynamic reconfigure
-  float c1 = pow(dt,4.0)/4.0;
-  float c2 = pow(dt,2.0);
-  float c3 = pow(dt,3.0)/2.0;
-  kalman->processNoiseCov = sigma*sigma*(cv::Mat_<float>(6, 6) <<
-                             c1,  0,  0, c3,  0,  0,
-                              0, c1,  0,  0, c3,  0,
-                              0,  0, c1,  0,  0, c3,
-                             c3,  0,  0, c2,  0,  0,
-                              0, c3,  0,  0, c2,  0,
-                              0,  0, c3,  0,  0, c2);
-
-  cv::setIdentity(kalman->measurementNoiseCov, cv::Scalar::all(5e-2));
-
-  cv::setIdentity(kalman->errorCovPost, cv::Scalar::all(1000000));
-}
-//---------------------------------------------------------------------------
-TKalmanFilter::~TKalmanFilter() { delete kalman; }
-
-//---------------------------------------------------------------------------
-void TKalmanFilter::Prediction()
-{
-  cv::Mat prediction = kalman->predict();
-  LastPosition = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
-  LastVelocity = Point_t(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5));
-}
-
-//---------------------------------------------------------------------------
-Point_t TKalmanFilter::Update(Point_t p, bool DataCorrect)
-{
-  cv::Mat measurement(3, 1, Mat_t(1));
-  if (!DataCorrect)
-  {
-    measurement.at<track_t>(0) = LastPosition.x; // update using prediction
-    measurement.at<track_t>(1) = LastPosition.y;
-    measurement.at<track_t>(2) = LastPosition.z;
-  }
-  else
-  {
-    measurement.at<track_t>(0) = p.x; // update using measurements
-    measurement.at<track_t>(1) = p.y;
-    measurement.at<track_t>(2) = p.z;
-  }
-  // Correction
-  cv::Mat estimated = kalman->correct(measurement);
-  LastPosition.x = estimated.at<track_t>(0); // update using measurements
-  LastPosition.y = estimated.at<track_t>(1);
-  LastPosition.z = estimated.at<track_t>(2);
-  LastVelocity.x = estimated.at<track_t>(3);
-  LastVelocity.y = estimated.at<track_t>(4);
-  LastVelocity.z = estimated.at<track_t>(5);
-  return LastPosition;
-}
-//---------------------------------------------------------------------------

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md

@@ -1,13 +0,0 @@
-Multitarget Tracker
-===================
-
-This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].
-It is utilized for the *CostmapToDynamicObstacles* plugin.
-
-The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].
-The package itself depends on other third party packages with different licenses (refer to the package repository).
-
-TODO: Include the whole package as external CMake project.
-
-
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_convex_hull.cpp

@@ -1,303 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann, Otniel Rinaldo
- *********************************************************************/
-
-#include <costmap_converter/costmap_to_lines_convex_hull.h>
-#include <costmap_converter/misc.h>
-#include <pluginlib/class_list_macros.hpp>
-
-PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::BaseCostmapToPolygons)
-
-namespace costmap_converter
-{
-
-CostmapToLinesDBSMCCH::CostmapToLinesDBSMCCH() : CostmapToPolygonsDBSMCCH() 
-{
-//    dynamic_recfg_ = NULL;
-}
-  
-CostmapToLinesDBSMCCH::~CostmapToLinesDBSMCCH() 
-{
-//  if (dynamic_recfg_ != NULL)
-//    delete dynamic_recfg_;
-}
-  
-void CostmapToLinesDBSMCCH::initialize(rclcpp::Node::SharedPtr nh)
-{ 
-    BaseCostmapToPolygons::initialize(nh);
-    
-    // DB SCAN
-    parameter_.max_distance_ = 0.4;
-    nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
-    
-    parameter_.min_pts_ = 2;
-    nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
-    
-    parameter_.max_pts_ = 30;
-    nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
-    
-    // convex hull
-    parameter_.min_keypoint_separation_ = 0.1;
-    nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
-    
-    parameter_buffered_ = parameter_;
-
-    // Line extraction
-    support_pts_max_dist_ = 0.3;
-    nh->get_parameter_or<double>("support_pts_max_dist", support_pts_max_dist_, support_pts_max_dist_);
-    
-    support_pts_max_dist_inbetween_ = 1.0;
-    nh->get_parameter_or<double>("support_pts_max_dist_inbetween", support_pts_max_dist_inbetween_, support_pts_max_dist_inbetween_);
-    
-    min_support_pts_ = 2;
-    nh->get_parameter_or<int>("min_support_pts", min_support_pts_, min_support_pts_);
-    
-    // setup dynamic reconfigure
-//    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>(nh);
-//    dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSMCCH::reconfigureCB, this, _1, _2);
-//    dynamic_recfg_->setCallback(cb);
-    
-    // deprecated
-    rclcpp::Parameter dummy;
-    if (nh->get_parameter("support_pts_min_dist_", dummy) || nh->get_parameter("support_pts_min_dist", dummy))
-      RCLCPP_WARN(nh->get_logger(), "CostmapToLinesDBSMCCH: Parameter 'support_pts_min_dist' is deprecated and not included anymore.");
-    if (nh->get_parameter("min_support_pts_", dummy))
-      RCLCPP_WARN(nh->get_logger(), "CostmapToLinesDBSMCCH: Parameter 'min_support_pts_' is not found. Remove the underscore.");
-}  
-  
-void CostmapToLinesDBSMCCH::compute()
-{
-    std::vector< std::vector<KeyPoint> > clusters;
-    dbScan(clusters);
-    
-    // Create new polygon container
-    PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());  
-    
-    
-    // add convex hulls to polygon container
-    for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
-    {
-      geometry_msgs::msg::Polygon polygon;
-      convexHull2(clusters[i], polygon );
-      
-      // now extract lines of the polygon (by searching for support points in the cluster)  
-      // and add them to the polygon container
-      extractPointsAndLines(clusters[i], polygon, std::back_inserter(*polygons));
-    }
-    
-    // add our non-cluster points to the polygon container (as single points)
-    if (!clusters.empty())
-    {
-      for (size_t i=0; i < clusters.front().size(); ++i)
-      {
-        polygons->push_back( geometry_msgs::msg::Polygon() );
-        convertPointToPolygon(clusters.front()[i], polygons->back());
-      }
-    }
-        
-    // replace shared polygon container
-    updatePolygonContainer(polygons);
-}
-
-typedef CostmapToLinesDBSMCCH CL;
-bool sort_keypoint_x(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) 
-{ return (cluster[i].x<cluster[j].x) || (cluster[i].x == cluster[j].x && cluster[i].y < cluster[j].y); }
-bool sort_keypoint_y(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) 
-{ return (cluster[i].y<cluster[j].y) || (cluster[i].y == cluster[j].y && cluster[i].x < cluster[j].x); }
-
-void CostmapToLinesDBSMCCH::extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::msg::Polygon& polygon,
-                                                  std::back_insert_iterator< std::vector<geometry_msgs::msg::Polygon> > lines)
-{
-   if (polygon.points.empty())
-     return;
-  
-   if (polygon.points.size() < 2)
-   {
-     lines = polygon; // our polygon is just a point, push back onto the output sequence
-     return;
-   }
-   int n = (int)polygon.points.size();
-     
-   for (int i=1; i<n; ++i) // this implemenation requires an explicitly closed polygon (start vertex = end vertex)
-   {
-        const geometry_msgs::msg::Point32* vertex1 = &polygon.points[i-1];
-        const geometry_msgs::msg::Point32* vertex2 = &polygon.points[i];
-
-        // check how many vertices belong to the line (sometimes the convex hull algorithm finds multiple vertices on a line,
-        // in case of small coordinate deviations)
-        double dx = vertex2->x - vertex1->x;
-        double dy = vertex2->y - vertex1->y;
-//         double norm = std::sqrt(dx*dx + dy*dy);
-//         dx /= norm;
-//         dy /= norm;
-//         for (int j=i; j<(int)polygon.points.size() - 2; ++j)
-//         {
-//           const geometry_msgs::msg::Point32* vertex_jp2 = &polygon.points[j+2];
-//           double dx2 = vertex_jp2->x - vertex2->x;
-//           double dy2 = vertex_jp2->y - vertex2->y;
-//           double norm2 = std::sqrt(dx2*dx2 + dy2*dy2);
-//           dx2 /= norm2;
-//           dy2 /= norm2;
-//           if (std::abs(dx*dx2 + dy*dy2) < 0.05) //~3 degree
-//           {
-//             vertex2 = &polygon.points[j+2];
-//             i = j; // DO NOT USE "i" afterwards
-//           }
-//           else break;
-//         }
-       
-        //Search support points
-        std::vector<std::size_t> support_points; // store indices of cluster
-        
-        for (std::size_t k = 0; k < cluster.size(); ++k)
-        {
-            bool is_inbetween = false;
-            double dist_line_to_point = computeDistanceToLineSegment( cluster[k], *vertex1, *vertex2, &is_inbetween );
-
-            if(is_inbetween && dist_line_to_point <= support_pts_max_dist_)
-            {
-              support_points.push_back(k);
-              continue;
-            }
-        }
-
-        // now check if the inlier models a line by checking the minimum distance between all support points (avoid lines over gaps)
-        // and by checking if the minium number of points is reached // deactivate by setting support_pts_max_dist_inbetween_==0
-        bool is_line=true;
-        if (support_pts_max_dist_inbetween_!=0)
-        {
-          if ((int)support_points.size() >= min_support_pts_ + 2) // +2 since start and goal are included
-          {          
-            // sort points
-            if (std::abs(dx) >= std::abs(dy))
-              std::sort(support_points.begin(), support_points.end(), std::bind(sort_keypoint_x, std::placeholders::_1, std::placeholders::_2, std::cref(cluster)));
-            else 
-              std::sort(support_points.begin(), support_points.end(), std::bind(sort_keypoint_y, std::placeholders::_1, std::placeholders::_2, std::cref(cluster)));
-            
-            // now calculate distances
-            for (int k = 1; k < int(support_points.size()); ++k)
-            {
-              double dist_x = cluster[support_points[k]].x - cluster[support_points[k-1]].x;
-              double dist_y = cluster[support_points[k]].y - cluster[support_points[k-1]].y;
-              double dist = std::sqrt( dist_x*dist_x + dist_y*dist_y);
-              if (dist > support_pts_max_dist_inbetween_)
-              {
-                is_line = false;
-                break;
-              }
-            }
-            
-          }
-          else
-            is_line = false;
-        }
-        
-        if (is_line)
-        {
-          // line found:
-          geometry_msgs::msg::Polygon line;
-          line.points.push_back(*vertex1);
-          line.points.push_back(*vertex2);
-          lines = line; // back insertion
-          
-          // remove inlier from list
-          // iterate in reverse order, otherwise indices are not valid after erasing elements
-          std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
-          for (; support_it != support_points.rend(); ++support_it)
-          {
-            cluster.erase(cluster.begin() + *support_it);
-          }
-        }
-        else
-        {
-            // remove goal, since it will be added in the subsequent iteration
-            //support_points.pop_back();
-          // old:
-//             // add vertex 1 as single point
-//             geometry_msgs::msg::Polygon point;
-//             point.points.push_back(*vertex1);
-//             lines = point; // back insertion
-
-           // add complete inlier set as points 
-//            for (int k = 0; k < int(support_points.size()); ++k)
-//            {
-//               geometry_msgs::msg::Polygon polygon;
-//               convertPointToPolygon(cluster[support_points[k]], polygon);
-//               lines = polygon; // back insertion
-//            }
-           
-          // remove inlier from list and add them as point obstacles
-          // iterate in reverse order, otherwise indices are not valid after erasing elements
-          std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
-          for (; support_it != support_points.rend(); ++support_it)
-          {
-            geometry_msgs::msg::Polygon polygon;
-            convertPointToPolygon(cluster[*support_it], polygon);
-            lines = polygon; // back insertion
-            
-            cluster.erase(cluster.begin() + *support_it);
-          }
-        }
-    }
- 
-    // add all remaining cluster points, that do not belong to a line
-    for (int i=0; i<(int)cluster.size();++i)
-    {
-        geometry_msgs::msg::Polygon polygon;
-        convertPointToPolygon(cluster[i], polygon);
-        lines = polygon; // back insertion
-    }
-
-}
-
-//void CostmapToLinesDBSMCCH::reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level)
-//{
-//    boost::mutex::scoped_lock lock(parameter_mutex_);
-//    parameter_buffered_.max_distance_ = config.cluster_max_distance;
-//    parameter_buffered_.min_pts_ = config.cluster_min_pts;
-//    parameter_buffered_.max_pts_ = config.cluster_max_pts;
-//    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
-//    support_pts_max_dist_ = config.support_pts_max_dist;
-//    support_pts_max_dist_inbetween_ = config.support_pts_max_dist_inbetween;
-//    min_support_pts_ = config.min_support_pts;
-//}
-
-
-
-}//end namespace costmap_converter
-
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_ransac.cpp

@@ -1,361 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include <costmap_converter/costmap_to_lines_ransac.h>
-#include <pluginlib/class_list_macros.hpp>
-
-PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::BaseCostmapToPolygons)
-
-namespace costmap_converter
-{
-
-CostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC() : CostmapToPolygonsDBSMCCH() 
-{
-//  dynamic_recfg_ = NULL;
-}
-
-CostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC() 
-{
-//  if (dynamic_recfg_ != NULL)
-//    delete dynamic_recfg_;
-}
-  
-void CostmapToLinesDBSRANSAC::initialize(rclcpp::Node::SharedPtr nh)
-{ 
-    BaseCostmapToPolygons::initialize(nh);
-    
-    // DB SCAN
-    parameter_.max_distance_ = 0.4;
-    nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
-    
-    parameter_.min_pts_ = 2;
-    nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
-    
-    parameter_.max_pts_ = 30;
-    nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
-    
-    // convex hull (only necessary if outlier filtering is enabled)
-    parameter_.min_keypoint_separation_ = 0.1;
-    nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
-
-    parameter_buffered_ = parameter_;
-
-    // ransac
-    ransac_inlier_distance_ = 0.2;
-    nh->get_parameter_or<double>("ransac_inlier_distance", ransac_inlier_distance_, ransac_inlier_distance_);
-    
-    ransac_min_inliers_ = 10;
-    nh->get_parameter_or<int>("ransac_min_inliers", ransac_min_inliers_, ransac_min_inliers_);
-    
-    ransac_no_iterations_ = 2000;
-    nh->get_parameter_or<int>("ransac_no_iterations", ransac_no_iterations_, ransac_no_iterations_);
-   
-    ransac_remainig_outliers_ = 3;
-    nh->get_parameter_or<int>("ransac_remainig_outliers", ransac_remainig_outliers_, ransac_remainig_outliers_);
-    
-    ransac_convert_outlier_pts_ = true;
-    nh->get_parameter_or<bool>("ransac_convert_outlier_pts", ransac_convert_outlier_pts_, ransac_convert_outlier_pts_);
-    
-    ransac_filter_remaining_outlier_pts_ = false;
-    nh->get_parameter_or<bool>("ransac_filter_remaining_outlier_pts", ransac_filter_remaining_outlier_pts_, ransac_filter_remaining_outlier_pts_);
-    
-    // setup dynamic reconfigure
-//    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>(nh);
-//    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSRANSAC::reconfigureCB, this, _1, _2);
-//    dynamic_recfg_->setCallback(cb);
-}  
-  
-void CostmapToLinesDBSRANSAC::compute()
-{
-    std::vector< std::vector<KeyPoint> > clusters;
-    dbScan(clusters);
-    
-    // Create new polygon container
-    PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());  
-    
-    
-    // fit lines using ransac for each cluster
-    for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
-    { 
-      
-      while ((int)clusters[i].size() > ransac_remainig_outliers_)
-      {
-//         std::vector<KeyPoint> inliers;
-        std::vector<KeyPoint> outliers;
-        std::pair<KeyPoint,KeyPoint> model;
-        if (!lineRansac(clusters[i], ransac_inlier_distance_, ransac_no_iterations_, ransac_min_inliers_, model, NULL, &outliers ) )
-          break;
-        
-        // add to polygon container
-        geometry_msgs::msg::Polygon line;
-        line.points.resize(2);
-        model.first.toPointMsg(line.points.front());
-        model.second.toPointMsg(line.points.back());
-        polygons->push_back(line);
-        
-        clusters[i] = outliers;
-      }
-      // create point polygons for remaining outliers
-      if (ransac_convert_outlier_pts_)
-      {
-        if (ransac_filter_remaining_outlier_pts_)
-        {
-          // take edge points of a convex polygon
-          // these points define a cluster and since all lines are extracted,
-          // we remove points from the interior...
-          geometry_msgs::msg::Polygon polygon;
-          convexHull2(clusters[i], polygon);
-          for (int j=0; j < (int)polygon.points.size(); ++j)
-          {
-            polygons->push_back(geometry_msgs::msg::Polygon());
-            convertPointToPolygon(polygon.points[j], polygons->back());
-          }
-        }
-        else
-        {
-          for (int j = 0; j < (int)clusters[i].size(); ++j)
-          {
-            polygons->push_back(geometry_msgs::msg::Polygon());
-            convertPointToPolygon(clusters[i][j], polygons->back());           
-          }
-        }
-
-      }
-      
-      
-    }
-    
-    // add our non-cluster points to the polygon container (as single points)
-    if (!clusters.empty())
-    {
-      for (size_t i=0; i < clusters.front().size(); ++i)
-      {
-        polygons->push_back( geometry_msgs::msg::Polygon() );
-        convertPointToPolygon(clusters.front()[i], polygons->back());
-      }
-    }
-        
-    // replace shared polygon container
-    updatePolygonContainer(polygons);
-}
-
-
-bool CostmapToLinesDBSRANSAC::lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations,
-                                         int min_inliers, std::pair<KeyPoint, KeyPoint>& best_model, 
-                                         std::vector<KeyPoint>* inliers, std::vector<KeyPoint>* outliers)
-{
-  if ((int)data.size() < 2 ||  (int)data.size() < min_inliers)
-  {
-    return false;
-  }
-  
-  std::uniform_int_distribution<> distribution(0, data.size()-1);
-  
-  std::pair<int, int> best_model_idx;
-  int best_no_inliers = -1;
-  
-  
-  for (int i=0; i < no_iterations; ++i)
-  {
-    // choose random points to define a line candidate
-    int start_idx = distribution(rnd_generator_);
-    int end_idx = start_idx;
-    while (end_idx == start_idx)
-      end_idx = distribution(rnd_generator_);
-    
-    
-    // compute inliers
-    int no_inliers = 0;
-    for (int j=0; j<(int)data.size(); ++j)
-    {
-      if ( isInlier(data[j], data[start_idx], data[end_idx], inlier_distance) )
-        ++no_inliers;
-    }
-    
-    if (no_inliers > best_no_inliers)
-    {
-      best_no_inliers = no_inliers;
-      best_model_idx.first = start_idx;
-      best_model_idx.second = end_idx;
-    }
-  }
-  
-  best_model.first = data[best_model_idx.first];
-  best_model.second = data[best_model_idx.second];
-  
-  if (best_no_inliers < min_inliers)
-    return false;
-  
-  // Now repeat the calculation for the best model in order to obtain the inliers and outliers set
-  // This might be faster if no_iterations is large, but TEST
-  if (inliers || outliers)
-  {
-    if (inliers)
-      inliers->clear();
-    if (outliers)
-      outliers->clear();
-    
-//    int no_inliers = 0;
-    for (int i=0; i<(int)data.size(); ++i)
-    {
-        if ( isInlier( data[i], best_model.first, best_model.second, inlier_distance ) )
-        {
-          if (inliers)
-            inliers->push_back( data[i] );
-        }
-        else
-        {
-          if (outliers)
-            outliers->push_back( data[i] );
-        }
-    }
-  }
-  
-  return true;
-}
-
-bool CostmapToLinesDBSRANSAC::linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept, 
-                                               double* mean_x_out, double* mean_y_out)
-{
-  if (data.size() < 2)
-  {
-    RCLCPP_ERROR(getLogger(), "CostmapToLinesDBSRANSAC: at least 2 data points required for linear regression");
-    return false;
-  }
-  
-  double mean_x = 0;
-  double mean_y = 0;
-  for (int i=0; i<(int)data.size(); ++i)
-  {
-    mean_x += data[i].x;
-    mean_y += data[i].y;
-  }
-  mean_x /= double(data.size());
-  mean_y /= double(data.size());
-  
-  if (mean_x_out)
-    *mean_x_out = mean_x;
- 
-  if (mean_y_out)
-    *mean_y_out = mean_y;
-  
-  double numerator = 0.0;
-  double denominator = 0.0;
-
-  for(int i=0; i<(int)data.size(); ++i)
-  {
-      double dx = data[i].x - mean_x;
-      numerator += dx * (data[i].y - mean_y);
-      denominator += dx*dx;
-  }
-  
-  if (denominator == 0)
-  {
-    RCLCPP_ERROR(getLogger(), "CostmapToLinesDBSRANSAC: linear regression failed, denominator 0");
-    return false;
-  }
-  else
-    slope = numerator / denominator;
-  
-  intercept = mean_y - slope * mean_x;
-  return true;
-}
-
-
-//void CostmapToLinesDBSRANSAC::reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level)
-//{
-//    boost::mutex::scoped_lock lock(parameter_mutex_);
-//    parameter_buffered_.max_distance_ = config.cluster_max_distance;
-//    parameter_buffered_.min_pts_ = config.cluster_min_pts;
-//    parameter_buffered_.max_pts_ = config.cluster_max_pts;
-//    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
-//    ransac_inlier_distance_ = config.ransac_inlier_distance;
-//    ransac_min_inliers_ = config.ransac_min_inliers;
-//    ransac_no_iterations_ = config.ransac_no_iterations;
-//    ransac_remainig_outliers_ = config.ransac_remainig_outliers;
-//    ransac_convert_outlier_pts_ = config.ransac_convert_outlier_pts;
-//    ransac_filter_remaining_outlier_pts_ = config.ransac_filter_remaining_outlier_pts;
-//}
-
-
-/*
-void CostmapToLinesDBSRANSAC::adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, 
-                                               KeyPoint& line_start, KeyPoint& line_end)
-{
-  line_start = linept1; 
-  line_end = linept2;
-
-  // infinite line is defined by linept1 and linept2
-  double dir_x = line_end.x - line_start.x;
-  double dir_y = line_end.y - line_start.y;
-  double norm = std::sqrt(dir_x*dir_x + dir_y*dir_y);
-  dir_x /= norm;
-  dir_y /= norm;
-  
-  // project data onto the line and check if the distance is increased in both directions
-  for (int i=0; i < (int) data.size(); ++i)
-  {
-    double dx = data[i].x - line_start.x;
-    double dy = data[i].y - line_start.y;
-    // check scalar product at start
-    double extension = dx*dir_x + dy*dir_y;
-    if (extension<0)
-    {
-      line_start.x -=  dir_x*extension;
-      line_start.y -=  dir_y*extension;
-    }
-    else
-    {
-      dx = data[i].x - line_end.x;
-      dy = data[i].y - line_end.y;
-      // check scalar product at end
-      double extension = dx*dir_x + dy*dir_y;
-      if (extension>0)
-      {
-        line_end.x +=  dir_x*extension;
-        line_end.y +=  dir_y*extension;
-      }
-    }
-  }
-  
-}*/
-
-
-}//end namespace costmap_converter
-
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons.cpp

@@ -1,514 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann, Otniel Rinaldo
- *********************************************************************/
-
-#include <costmap_converter/costmap_to_polygons.h>
-#include <costmap_converter/misc.h>
-#include <pluginlib/class_list_macros.hpp>
-
-PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)
-
-namespace
-{
-
-/**
- * @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points
- *
- * Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
- *
- * @param begin iterator pointing to the begin of the range of points
- * @param end interator pointing to the end of the range of points
- * @param epsilon distance criteria for removing points if it is closer to the line segment than this
- * @param result the simplified polygon
- */
-std::vector<geometry_msgs::msg::Point32> douglasPeucker(std::vector<geometry_msgs::msg::Point32>::iterator begin,
-  std::vector<geometry_msgs::msg::Point32>::iterator end, double epsilon)
-{
-  if (std::distance(begin, end) <= 2)
-  {
-    return std::vector<geometry_msgs::msg::Point32>(begin, end);
-  }
-
-  // Find the point with the maximum distance from the line [begin, end)
-  double dmax = std::numeric_limits<double>::lowest();
-  std::vector<geometry_msgs::msg::Point32>::iterator max_dist_it;
-  std::vector<geometry_msgs::msg::Point32>::iterator last = std::prev(end);
-  for (auto it = std::next(begin); it != last; ++it)
-  {
-    double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last);
-    if (d > dmax)
-    {
-      max_dist_it = it;
-      dmax = d;
-    }
-  }
-
-  if (dmax < epsilon * epsilon)
-  { // termination criterion reached, line is good enough
-    std::vector<geometry_msgs::msg::Point32> result;
-    result.push_back(*begin);
-    result.push_back(*last);
-    return result;
-  }
-
-  // Recursive calls for the two splitted parts
-  auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon);
-  auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon);
-
-  // Combine the two lines into one line and return the merged line.
-  // Note that we have to skip the first point of the second line, as it is duplicated above.
-  firstLineSimplified.insert(firstLineSimplified.end(),
-    std::make_move_iterator(std::next(secondLineSimplified.begin())),
-    std::make_move_iterator(secondLineSimplified.end()));
-  return firstLineSimplified;
-}
-
-} // end namespace
-
-namespace costmap_converter
-{
-
-CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()
-{
-  costmap_ = NULL;
-  // dynamic_recfg_ = NULL;
-  neighbor_size_x_ = neighbor_size_y_ = -1;
-  offset_x_ = offset_y_ = 0.;
-}
-
-CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH()
-{
-//  if (dynamic_recfg_ != NULL)
-//    delete dynamic_recfg_;
-}
-
-void CostmapToPolygonsDBSMCCH::initialize(rclcpp::Node::SharedPtr nh)
-{
-    BaseCostmapToPolygons::initialize(nh);
-
-    costmap_ = NULL;
-
-    parameter_.max_distance_ = 0.4;
-    nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
-
-    parameter_.min_pts_ = 2;
-    nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
-
-    parameter_.max_pts_ = 30;
-    nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
-
-    parameter_.min_keypoint_separation_ = 0.1;
-    nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
-
-    parameter_buffered_ = parameter_;
-
-    // setup dynamic reconfigure
-//    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);
-//    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);
-//    dynamic_recfg_->setCallback(cb);
-}
-
-
-void CostmapToPolygonsDBSMCCH::compute()
-{
-    std::vector< std::vector<KeyPoint> > clusters;
-    dbScan(clusters);
-
-    // Create new polygon container
-    PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());
-
-
-    // add convex hulls to polygon container
-    for (std::size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
-    {
-      polygons->push_back( geometry_msgs::msg::Polygon() );
-      convexHull2(clusters[i], polygons->back() );
-    }
-
-    // add our non-cluster points to the polygon container (as single points)
-    if (!clusters.empty())
-    {
-      for (std::size_t i=0; i < clusters.front().size(); ++i)
-      {
-        polygons->push_back( geometry_msgs::msg::Polygon() );
-        convertPointToPolygon(clusters.front()[i], polygons->back());
-      }
-    }
-
-    // replace shared polygon container
-    updatePolygonContainer(polygons);
-}
-
-void CostmapToPolygonsDBSMCCH::setCostmap2D(nav2_costmap_2d::Costmap2D *costmap)
-{
-    if (!costmap)
-      return;
-
-    costmap_ = costmap;
-
-    updateCostmap2D();
-}
-
-void CostmapToPolygonsDBSMCCH::updateCostmap2D()
-{
-      occupied_cells_.clear();
-
-      if (!costmap_->getMutex())
-      {
-        RCLCPP_ERROR(getLogger(), "Cannot update costmap since the mutex pointer is null");
-        return;
-      }
-
-      // TODO: currently dynamic reconigure is not supported in ros2
-      { // get a copy of our parameters from dynamic reconfigure
-        std::lock_guard<std::mutex> lock(parameter_mutex_);
-        parameter_ = parameter_buffered_;
-      }
-
-      std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap_->getMutex());
-
-      // allocate neighbor lookup
-      int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;
-      int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;
-
-      if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) {
-        neighbor_size_x_ = cells_x;
-        neighbor_size_y_ = cells_y;
-        neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);
-      }
-      offset_x_ = costmap_->getOriginX();
-      offset_y_ = costmap_->getOriginY();
-      for (auto& n : neighbor_lookup_)
-        n.clear();
-      
-      auto size_x = costmap_->getSizeInCellsX();
-      auto size_y = costmap_->getSizeInCellsY();
-
-      // get indices of obstacle cells
-      for(std::size_t i = 0; i < size_x; i++)
-      {
-        for(std::size_t j = 0; j < size_y; j++)
-        {
-          int value = costmap_->getCost(i,j);
-          if(value >= nav2_costmap_2d::LETHAL_OBSTACLE)
-          {
-            double x, y;
-            costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
-            addPoint(x, y);
-          }
-        }
-      }
-}
-
-
-void CostmapToPolygonsDBSMCCH::dbScan(std::vector< std::vector<KeyPoint> >& clusters)
-{
-  std::vector<bool> visited(occupied_cells_.size(), false);
-
-  clusters.clear();
-
-  //DB Scan Algorithm
-  int cluster_id = 0; // current cluster_id
-  clusters.push_back(std::vector<KeyPoint>());
-  for(int i = 0; i< (int)occupied_cells_.size(); i++)
-  {
-    if(!visited[i]) //keypoint has not been visited before
-    {
-      visited[i] = true; // mark as visited
-      std::vector<int> neighbors;
-      regionQuery(i, neighbors); //Find neighbors around the keypoint
-      if((int)neighbors.size() < parameter_.min_pts_) //If not enough neighbors are found, mark as noise
-      {
-        clusters[0].push_back(occupied_cells_[i]);
-      }
-      else
-      {
-        ++cluster_id; // increment current cluster_id
-        clusters.push_back(std::vector<KeyPoint>());
-
-        // Expand the cluster
-        clusters[cluster_id].push_back(occupied_cells_[i]);
-        for(int j = 0; j<(int)neighbors.size(); j++)
-        {
-          if ((int)clusters[cluster_id].size() == parameter_.max_pts_)
-            break;
-
-          if(!visited[neighbors[j]]) //keypoint has not been visited before
-          {
-            visited[neighbors[j]] = true;  // mark as visited
-            std::vector<int> further_neighbors;
-            regionQuery(neighbors[j], further_neighbors); //Find more neighbors around the new keypoint
-//             if(further_neighbors.size() < min_pts_)
-//             {
-//               clusters[0].push_back(occupied_cells[neighbors[j]]);
-//             }
-//             else
-            if ((int)further_neighbors.size() >= parameter_.min_pts_)
-            {
-              // neighbors found
-              neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end());  //Add these newfound P' neighbour to P neighbour vector "nb_indeces"
-              clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]);
-            }
-          }
-        }
-      }
-    }
-  }
-}
-
-void CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector<int>& neighbors)
-{
-    neighbors.clear();
-
-    double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_;
-    const KeyPoint& kp = occupied_cells_[curr_index];
-    int cx, cy;
-    pointToNeighborCells(kp, cx,cy);
-
-    // loop over the neighboring cells for looking up the points
-    const int offsets[9][2] = {{-1, -1}, {0, -1}, {1, -1},
-                               {-1,  0}, {0,  0}, {1,  0},
-                               {-1,  1}, {0,  1}, {1,  1}};
-    for (int i = 0; i < 9; ++i)
-    {
-      int idx = neighborCellsToIndex(cx + offsets[i][0], cy + offsets[i][1]);
-      if (idx < 0 || idx >= int(neighbor_lookup_.size()))
-        continue;
-      const std::vector<int>& pointIndicesToCheck = neighbor_lookup_[idx];
-      for (int point_idx : pointIndicesToCheck) {
-        if (point_idx == curr_index) // point is not a neighbor to itself
-          continue;
-        const KeyPoint& other = occupied_cells_[point_idx];
-        double dx = other.x - kp.x;
-        double dy = other.y - kp.y;
-        double dist_sqr = dx*dx + dy*dy;
-        if (dist_sqr <= dist_sqr_threshold)
-          neighbors.push_back(point_idx);
-      }
-    }
-}
-
-bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2)
-{
-  return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
-}
-
-void CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon)
-{
-    //Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull
-
-    int k = 0;
-    int n = cluster.size();
-
-    // sort points according to x coordinate (TODO. is it already sorted due to the map representation?)
-    std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);
-
-    polygon.points.resize(2*n);
-
-    // lower hull
-    for (int i = 0; i < n; ++i)
-    {
-      while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
-      {
-        --k;
-      }
-      cluster[i].toPointMsg(polygon.points[k]);
-      ++k;
-    }
-
-    // upper hull
-    for (int i = n-2, t = k+1; i >= 0; --i)
-    {
-      while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
-      {
-        --k;
-      }
-      cluster[i].toPointMsg(polygon.points[k]);
-      ++k;
-    }
-
-
-    polygon.points.resize(k); // original
-    // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.
-//     polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost
-
-    simplifyPolygon(polygon);
-}
-
-
-
-void CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon)
-{
-    std::vector<KeyPoint>& P = cluster;
-    std::vector<geometry_msgs::msg::Point32>& points = polygon.points;
-
-    // Sort P by x and y
-    std::sort(P.begin(), P.end(), isXCoordinateSmaller);
-
-    // the output array H[] will be used as the stack
-    int i;                 // array scan index
-
-    // Get the indices of points with min x-coord and min|max y-coord
-    int minmin = 0, minmax;
-    double xmin = P[0].x;
-    for (i = 1; i < (int)P.size(); i++)
-        if (P[i].x != xmin) break;
-    minmax = i - 1;
-    if (minmax == (int)P.size() - 1)
-    {   // degenerate case: all x-coords == xmin
-        points.push_back(geometry_msgs::msg::Point32());
-        P[minmin].toPointMsg(points.back());
-        if (P[minmax].y != P[minmin].y) // a  nontrivial segment
-        {
-            points.push_back(geometry_msgs::msg::Point32());
-            P[minmax].toPointMsg(points.back());
-        }
-        // add polygon endpoint
-        points.push_back(geometry_msgs::msg::Point32());
-        P[minmin].toPointMsg(points.back());
-        return;
-    }
-
-    // Get the indices of points with max x-coord and min|max y-coord
-    int maxmin, maxmax = (int)P.size() - 1;
-    double xmax = P.back().x;
-    for (i = P.size() - 2; i >= 0; i--)
-        if (P[i].x != xmax) break;
-    maxmin = i+1;
-
-    // Compute the lower hull on the stack H
-    // push  minmin point onto stack
-    points.push_back(geometry_msgs::msg::Point32());
-    P[minmin].toPointMsg(points.back());
-    i = minmax;
-    while (++i <= maxmin)
-    {
-        // the lower line joins P[minmin]  with P[maxmin]
-        if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
-            continue;           // ignore P[i] above or on the lower line
-
-        while (points.size() > 1)         // there are at least 2 points on the stack
-        {
-            // test if  P[i] is left of the line at the stack top
-            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
-                break;         // P[i] is a new hull  vertex
-            points.pop_back();         // pop top point off  stack
-        }
-        // push P[i] onto stack
-        points.push_back(geometry_msgs::msg::Point32());
-        P[i].toPointMsg(points.back());
-    }
-
-    // Next, compute the upper hull on the stack H above  the bottom hull
-    if (maxmax != maxmin)      // if  distinct xmax points
-    {
-         // push maxmax point onto stack
-         points.push_back(geometry_msgs::msg::Point32());
-         P[maxmax].toPointMsg(points.back());
-    }
-    int bot = (int)points.size();                  // the bottom point of the upper hull stack
-    i = maxmin;
-    while (--i >= minmax)
-    {
-        // the upper line joins P[maxmax]  with P[minmax]
-        if (cross( P[maxmax], P[minmax], P[i])  >= 0 && i > minmax)
-            continue;           // ignore P[i] below or on the upper line
-
-        while ((int)points.size() > bot)     // at least 2 points on the upper stack
-        {
-            // test if  P[i] is left of the line at the stack top
-            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
-                break;         // P[i] is a new hull  vertex
-            points.pop_back();         // pop top point off stack
-        }
-        // push P[i] onto stack
-        points.push_back(geometry_msgs::msg::Point32());
-        P[i].toPointMsg(points.back());
-    }
-    if (minmax != minmin)
-    {
-        // push  joining endpoint onto stack
-        points.push_back(geometry_msgs::msg::Point32());
-        P[minmin].toPointMsg(points.back());
-    }
-
-    simplifyPolygon(polygon);
-}
-
-void CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::msg::Polygon& polygon)
-{
-  size_t triangleThreshold = 3;
-  // check if first and last point are the same. If yes, a triangle has 4 points
-  if (polygon.points.size() > 1
-      && std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5
-      && std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5)
-  {
-    triangleThreshold = 4;
-  }
-  if (polygon.points.size() <= triangleThreshold) // nothing to do for triangles or lines
-    return;
-  // TODO Reason about better start conditions for splitting lines, e.g., by
-  // https://en.wikipedia.org/wiki/Rotating_calipers
-  polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(), parameter_.min_keypoint_separation_);;
-}
-
-void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  polygons_ = polygons;
-}
-
-
-PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  PolygonContainerConstPtr polygons = polygons_;
-  return polygons;
-}
-
-//void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level)
-//{
-  //boost::mutex::scoped_lock lock(parameter_mutex_);
-  //parameter_buffered_.max_distance_ = config.cluster_max_distance;
-  //parameter_buffered_.min_pts_ = config.cluster_min_pts;
-  //parameter_buffered_.max_pts_ = config.cluster_max_pts;
-  //parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation;
-//}
-
-}//end namespace costmap_converter
-
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons_concave.cpp

@@ -1,231 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016,
- *  TU Dortmund - Institute of Control Theory and Systems Engineering.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the institute nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann, Otniel Rinaldo
- *********************************************************************/
-
-#include <costmap_converter/costmap_to_polygons_concave.h>
-
-#include <pluginlib/class_list_macros.hpp>
-
-PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSConcaveHull, costmap_converter::BaseCostmapToPolygons)
-
-namespace costmap_converter
-{
-    
-CostmapToPolygonsDBSConcaveHull::CostmapToPolygonsDBSConcaveHull() : CostmapToPolygonsDBSMCCH()
-{
-//  dynamic_recfg_ = NULL;
-}
-
-CostmapToPolygonsDBSConcaveHull::~CostmapToPolygonsDBSConcaveHull() 
-{
-//  if (dynamic_recfg_ != NULL)
-//    delete dynamic_recfg_;
-}
-
-void CostmapToPolygonsDBSConcaveHull::initialize(rclcpp::Node::SharedPtr nh)
-{ 
-    BaseCostmapToPolygons::initialize(nh);
-    
-    parameter_.max_distance_ = 0.4; 
-    nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
-    
-    parameter_.min_pts_ = 2;
-    nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
-    
-    parameter_.max_pts_ = 30;
-    nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
-    
-    parameter_.min_keypoint_separation_ = 0.1;
-    nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
-    
-    parameter_buffered_ = parameter_;
-
-    concave_hull_depth_ = 2.0;
-    nh->get_parameter_or<double>("concave_hull_depth", concave_hull_depth_, concave_hull_depth_);
-    
-    // setup dynamic reconfigure
-//    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>(nh);
-//    dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSConcaveHull::reconfigureCB, this, _1, _2);
-//    dynamic_recfg_->setCallback(cb);
-}
-
-
-void CostmapToPolygonsDBSConcaveHull::compute()
-{
-    std::vector< std::vector<KeyPoint> > clusters;
-    dbScan(clusters);
-    
-    // Create new polygon container
-    PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());
-    
-    
-    // add convex hulls to polygon container
-    for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
-    {
-      polygons->push_back( geometry_msgs::msg::Polygon() );
-      concaveHull(clusters[i], concave_hull_depth_, polygons->back() );
-    }
-    
-    // add our non-cluster points to the polygon container (as single points)
-    if (!clusters.empty())
-    {
-      for (size_t i=0; i < clusters.front().size(); ++i)
-      {
-        polygons->push_back( geometry_msgs::msg::Polygon() );
-        convertPointToPolygon(clusters.front()[i], polygons->back());
-      }
-    }
-    
-    // replace shared polygon container
-    updatePolygonContainer(polygons);
-}
-
-
-void CostmapToPolygonsDBSConcaveHull::concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon)
-{
-    // start with convex hull
-    convexHull2(cluster, polygon);
-
-    std::vector<geometry_msgs::msg::Point32>& concave_list = polygon.points;
-
-    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
-    {
-      
-        // find nearest inner point pk from line (vertex1 -> vertex2)
-        const geometry_msgs::msg::Point32& vertex1 = concave_list[i];
-        const geometry_msgs::msg::Point32& vertex2 = concave_list[i+1];
-
-        bool found;
-        size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
-        if (!found) 
-          continue;  
-        
-        double line_length = norm2d(vertex1, vertex2);
-                
-        double dst1 = norm2d(cluster[nearest_idx], vertex1);
-        double dst2 = norm2d(cluster[nearest_idx], vertex2);
-        double dd = std::min(dst1, dst2);
-        if (dd<1e-8)
-          continue;
-
-        if (line_length / dd > depth)
-        {
-            // Check that new candidate edge will not intersect existing edges.
-            bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
-            intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
-            if (!intersects) 
-            {
-              geometry_msgs::msg::Point32 new_point;
-              cluster[nearest_idx].toPointMsg(new_point);
-              concave_list.insert(concave_list.begin() + i + 1, new_point);
-              i--;
-            }
-        }
-    }
-}
-
-
-void CostmapToPolygonsDBSConcaveHull::concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon)
-{
-    // start with convex hull
-    convexHull2(cluster, polygon);
-
-    std::vector<geometry_msgs::msg::Point32>& concave_list = polygon.points;
-    
-    // get line length
-    double mean_length = 0;
-    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
-    {
-      mean_length += norm2d(concave_list[i],concave_list[i+1]);
-    }
-    mean_length /= double(concave_list.size());
-
-    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
-    {
-      
-        // find nearest inner point pk from line (vertex1 -> vertex2)
-        const geometry_msgs::msg::Point32& vertex1 = concave_list[i];
-        const geometry_msgs::msg::Point32& vertex2 = concave_list[i+1];
-
-        double line_length = norm2d(vertex1, vertex2);
-        
-        bool found;
-        size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
-        if (!found) 
-        {
-          continue;  
-        }
-        
-                
-        double dst1 = norm2d(cluster[nearest_idx], vertex1);
-        double dst2 = norm2d(cluster[nearest_idx], vertex2);
-        double dd = std::min(dst1, dst2);
-        if (dd<1e-8)
-          continue;
-
-        if (line_length / dd > depth)
-        {
-            // Check that new candidate edge will not intersect existing edges.
-            bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
-            intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
-            if (!intersects) 
-            {
-              geometry_msgs::msg::Point32 new_point;
-              cluster[nearest_idx].toPointMsg(new_point);
-              concave_list.insert(concave_list.begin() + i + 1, new_point);
-              i--;
-            }
-        }
-    }
-}
-
-
-
-
-//void CostmapToPolygonsDBSConcaveHull::reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level)
-//{
-//    boost::mutex::scoped_lock lock(parameter_mutex_);
-//    parameter_buffered_.max_distance_ = config.cluster_max_distance;
-//    parameter_buffered_.min_pts_ = config.cluster_min_pts;
-//    parameter_buffered_.max_pts_ = config.cluster_max_pts;
-//    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
-//    concave_hull_depth_ = config.concave_hull_depth;
-//}
-
-}//end namespace costmap_converter
-
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.cpp

@@ -1,230 +0,0 @@
-#include <random>
-#include <memory>
-#include <gtest/gtest.h>
-
-#include <costmap_converter/costmap_to_polygons.h>
-
-namespace {
-geometry_msgs::msg::Point32 create_point(double x, double y)
-{
-  geometry_msgs::msg::Point32 result;
-  result.x = x;
-  result.y = y;
-  result.z = 0.;
-  return result;
-}
-} // end namespace
-
-// make things accesible in the test
-class CostmapToPolygons : public costmap_converter::CostmapToPolygonsDBSMCCH
-{
-  public:
-    const std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint>& points() const {return occupied_cells_;}
-    costmap_converter::CostmapToPolygonsDBSMCCH::Parameters& parameters() {return parameter_;}
-    using costmap_converter::CostmapToPolygonsDBSMCCH::addPoint;
-    using costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery;
-    using costmap_converter::CostmapToPolygonsDBSMCCH::dbScan;
-    using costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2;
-    using costmap_converter::CostmapToPolygonsDBSMCCH::simplifyPolygon;
-};
-
-class CostmapToPolygonsDBSMCCHTest : public ::testing::Test
-{
-  protected:
-    void SetUp() override {
-      // parameters
-      costmap_to_polygons.parameters().max_distance_ = 0.5;
-      costmap_to_polygons.parameters().max_pts_ = 100;
-      costmap_to_polygons.parameters().min_pts_ = 2;
-      costmap_to_polygons.parameters().min_keypoint_separation_ = 0.1;
-
-      costmap.reset(new nav2_costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
-      costmap_to_polygons.setCostmap2D(costmap.get());
-
-      std::random_device rand_dev;
-      std::mt19937 generator(rand_dev());
-      std::uniform_real_distribution<> random_angle(-M_PI, M_PI);
-      std::uniform_real_distribution<> random_dist(0.,   costmap_to_polygons.parameters().max_distance_);
-
-      costmap_to_polygons.addPoint(0., 0.);
-      costmap_to_polygons.addPoint(1.3, 1.3);
-
-      // adding random points
-      double center_x = costmap_to_polygons.points()[0].x;
-      double center_y = costmap_to_polygons.points()[0].y;
-      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_ - 1; ++i)
-      {
-        double alpha = random_angle(generator);
-        double dist  = random_dist(generator);
-        double wx = center_x + std::cos(alpha) * dist;
-        double wy = center_y + std::sin(alpha) * dist;
-        costmap_to_polygons.addPoint(wx, wy);
-      }
-
-      // some noisy points not belonging to a cluster
-      costmap_to_polygons.addPoint(-1, -1);
-      costmap_to_polygons.addPoint(-2, -2);
-      
-      // adding random points
-      center_x = costmap_to_polygons.points()[1].x;
-      center_y = costmap_to_polygons.points()[1].y;
-      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_/2; ++i)
-      {
-        double alpha = random_angle(generator);
-        double dist  = random_dist(generator);
-        double wx = center_x + std::cos(alpha) * dist;
-        double wy = center_y + std::sin(alpha) * dist;
-        costmap_to_polygons.addPoint(wx, wy);
-      }
-    }
-
-    void regionQueryTrivial(int curr_index, std::vector<int>& neighbor_indices)
-    {
-      neighbor_indices.clear();
-      const auto& query_point = costmap_to_polygons.points()[curr_index];
-      for (int i = 0; i < int(costmap_to_polygons.points().size()); ++i)
-      {
-        if (i == curr_index)
-          continue;
-        const auto& kp = costmap_to_polygons.points()[i];
-        double dx = query_point.x - kp.x;
-        double dy = query_point.y - kp.y;
-        double dist = sqrt(dx*dx + dy*dy);
-        if (dist < costmap_to_polygons.parameters().max_distance_)
-          neighbor_indices.push_back(i);
-      }
-    }
-
-    CostmapToPolygons costmap_to_polygons;
-    std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap;
-};
-
-TEST_F(CostmapToPolygonsDBSMCCHTest, regionQuery)
-{
-  std::vector<int> neighbors, neighbors_trivial;
-  costmap_to_polygons.regionQuery(0, neighbors);
-  regionQueryTrivial(0, neighbors_trivial);
-  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_ - 1, int(neighbors.size()));
-  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
-  std::sort(neighbors.begin(), neighbors.end());
-  ASSERT_EQ(neighbors_trivial, neighbors);
-
-  costmap_to_polygons.regionQuery(1, neighbors);
-  regionQueryTrivial(1, neighbors_trivial);
-  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2, int(neighbors.size()));
-  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
-  std::sort(neighbors.begin(), neighbors.end());
-  ASSERT_EQ(neighbors_trivial, neighbors);
-}
-
-TEST_F(CostmapToPolygonsDBSMCCHTest, dbScan)
-{
-  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
-  costmap_to_polygons.dbScan(clusters);
-  
-  ASSERT_EQ((unsigned)3, clusters.size());
-  ASSERT_EQ((unsigned)2, clusters[0].size()); // noisy points not belonging to a cluster
-  ASSERT_EQ((unsigned)costmap_to_polygons.parameters().max_pts_, clusters[1].size()); // first cluster at (0,0)
-  ASSERT_EQ((unsigned)costmap_to_polygons.parameters().max_pts_/2 + 1, clusters[2].size()); // second cluster at (1,1)
-}
-
-TEST(CostmapToPolygonsDBSMCCH, EmptyMap)
-{
-  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap =
-    std::make_shared<nav2_costmap_2d::Costmap2D>(nav2_costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
-  CostmapToPolygons costmap_to_polygons;
-  costmap_to_polygons.setCostmap2D(costmap.get());
-
-  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
-  costmap_to_polygons.dbScan(clusters);
-  ASSERT_EQ((unsigned)1, clusters.size());    // noise cluster exists
-  ASSERT_EQ((unsigned)0, clusters[0].size()); // noise clsuter is empty
-}
-
-TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygon)
-{
-  const double simplification_threshold = 0.1;
-  CostmapToPolygons costmap_to_polygons;
-  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
-  
-  geometry_msgs::msg::Polygon polygon;
-  polygon.points.push_back(create_point(0., 0.));
-  polygon.points.push_back(create_point(1., 0.));
-
-  // degenerate case with just two points
-  geometry_msgs::msg::Polygon original_polygon = polygon;
-  costmap_to_polygons.simplifyPolygon(polygon);
-  ASSERT_EQ((unsigned)2, polygon.points.size());
-  for (size_t i = 0; i < polygon.points.size(); ++i)
-  {
-    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
-    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  
-  }
-
-  // degenerate case with three points
-  polygon.points.push_back(create_point(1., simplification_threshold / 2.));
-  original_polygon = polygon;
-  costmap_to_polygons.simplifyPolygon(polygon);
-  ASSERT_EQ((unsigned)3, polygon.points.size());
-  for (size_t i = 0; i < polygon.points.size(); ++i)
-  {
-    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
-    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  
-  }
-
-  polygon.points.push_back(create_point(1., 1.));
-  original_polygon = polygon;
-  // remove the point that has to be removed by the simplification
-  original_polygon.points.erase(original_polygon.points.begin()+2);
-  costmap_to_polygons.simplifyPolygon(polygon);
-  ASSERT_EQ((unsigned)3, polygon.points.size());
-  for (size_t i = 0; i < polygon.points.size(); ++i)
-  {
-    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
-    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);
-  }
-}
-
-TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygonPerfectLines)
-{
-  const double simplification_threshold = 0.1;
-  CostmapToPolygons costmap_to_polygons;
-  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
-  
-  geometry_msgs::msg::Polygon polygon;
-  for (int i = 0; i <= 100; ++i)
-    polygon.points.push_back(create_point(i*1., 0.));
-  geometry_msgs::msg::Point32 lastPoint = polygon.points.back();
-  for (int i = 0; i <= 100; ++i)
-    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
-  lastPoint = polygon.points.back();
-  for (int i = 0; i <= 100; ++i)
-    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
-  lastPoint = polygon.points.back();
-  for (int i = 0; i <= 100; ++i)
-    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
-  lastPoint = polygon.points.back();
-  for (int i = 0; i <= 100; ++i)
-    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
-
-  costmap_to_polygons.simplifyPolygon(polygon);
-  ASSERT_EQ((unsigned)6, polygon.points.size());
-  ASSERT_FLOAT_EQ(  0., polygon.points[0].x);
-  ASSERT_FLOAT_EQ(  0., polygon.points[0].y);
-  ASSERT_FLOAT_EQ(100., polygon.points[1].x);
-  ASSERT_FLOAT_EQ(  0., polygon.points[1].y);
-  ASSERT_FLOAT_EQ(100., polygon.points[2].x);
-  ASSERT_FLOAT_EQ(100., polygon.points[2].y);
-  ASSERT_FLOAT_EQ(200., polygon.points[3].x);
-  ASSERT_FLOAT_EQ(100., polygon.points[3].y);
-  ASSERT_FLOAT_EQ(200., polygon.points[4].x);
-  ASSERT_FLOAT_EQ(200., polygon.points[4].y);
-  ASSERT_FLOAT_EQ(300., polygon.points[5].x);
-  ASSERT_FLOAT_EQ(200., polygon.points[5].y);
-}
-
-int main(int argc, char** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.test

@@ -1,3 +0,0 @@
-<launch>
-  <test test-name="costmap_polygons" pkg="costmap_converter" type="test_costmap_polygons" />
-</launch>

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CHANGELOG.rst

@@ -1,41 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package costmap_converter_msgs
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.1.2 (2020-06-22)
-------------------
-
-0.1.1 (2020-01-25)
-------------------
-
-0.1.0 (2020-01-23)
-------------------
-* Messages moved to a separate package
-* Contributors: Vinnam Kim
-
-0.0.9 (2018-05-28)
-------------------
-
-0.0.8 (2018-05-17)
-------------------
-
-0.0.7 (2017-09-20)
-------------------
-
-0.0.6 (2017-09-18)
-------------------
-
-0.0.5 (2016-02-01)
-------------------
-
-0.0.4 (2016-01-11)
-------------------
-
-0.0.3 (2015-12-23)
-------------------
-
-0.0.2 (2015-12-22)
-------------------
-
-0.0.1 (2015-12-21)
-------------------

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CMakeLists.txt

@@ -1,26 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-
-project(costmap_converter_msgs)
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-find_package(ament_cmake REQUIRED)
-find_package(builtin_interfaces REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-rosidl_generate_interfaces(costmap_converter_msgs
-  "msg/ObstacleMsg.msg"
-  "msg/ObstacleArrayMsg.msg"
-  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
-)
-
-ament_package()

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleArrayMsg.msg

@@ -1,10 +0,0 @@
-# Message that contains a list of polygon shaped obstacles.
-# Special types:
-# Polygon with 1 vertex: Point obstacle
-# Polygon with 2 vertices: Line obstacle
-# Polygon with more than 2 vertices: First and last points are assumed to be connected
-
-std_msgs/Header header
-
-costmap_converter_msgs/ObstacleMsg[] obstacles
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleMsg.msg

@@ -1,24 +0,0 @@
-# Special types:
-# Polygon with 1 vertex: Point obstacle (you might also specify a non-zero value for radius)
-# Polygon with 2 vertices: Line obstacle
-# Polygon with more than 2 vertices: First and last points are assumed to be connected
-
-std_msgs/Header header
-
-# Obstacle footprint (polygon descriptions)
-geometry_msgs/Polygon polygon
-
-# Specify the radius for circular/point obstacles
-float64 radius
-
-# Obstacle ID
-# Specify IDs in order to provide (temporal) relationships
-# between obstacles among multiple messages.
-int64 id
-
-# Individual orientation (centroid)
-geometry_msgs/Quaternion orientation
-
-# Individual velocities (centroid)
-geometry_msgs/TwistWithCovariance velocities
-

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/package.xml

@@ -1,31 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>costmap_converter_msgs</name>
-  <version>0.1.2</version>
-  <description>Package containing message types for costmap conversion</description>
-  <maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
-  
-  <license>BSD</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
-
-  <build_depend>builtin_interfaces</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>std_msgs</build_depend>
-
-  <exec_depend>builtin_interfaces</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-
-  <test_depend>ament_lint_common</test_depend>
-
-  <member_of_group>rosidl_interface_packages</member_of_group>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/CMakeLists.txt

@@ -1,55 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-project(fake_vel_transform)
-
-## Use C++14
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
-## enforcing cleaner code.
-add_definitions(-Wall -Werror)
-
-## Export compile commands for clangd
-set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
-
-#######################
-## Find dependencies ##
-#######################
-
-find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
-
-###########
-## Build ##
-###########
-
-ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/fake_vel_transform.cpp
-)
-
-rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN fake_vel_transform::FakeVelTransform
-  EXECUTABLE ${PROJECT_NAME}_node
-)
-
-#############
-## Testing ##
-#############
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  list(APPEND AMENT_LINT_AUTO_EXCLUDE
-    ament_cmake_copyright
-    ament_cmake_uncrustify
-  )
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-#############
-## Install ##
-#############
-
-ament_auto_package(
-  INSTALL_TO_SHARE
-  launch
-)

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/README.md

@@ -1,29 +0,0 @@
-# fake_vel_transform
-
-该包用于实现雷达和云台共用的 yaw 轴时的兼容性，即使云台处于旋转扫描状态，速度变换后，仍然可以实现较稳定的轨迹跟踪效果。
-
-## 实现方式
-
-创建 XYZ 轴基于 `base_link`, RPY 轴基于局部路径规划朝向的 `base_link_fake` 坐标系，使得 nav2 局部规划器能够将机器人的方向视为与当前路径规划方向一致。
-
-nav2 发布的速度也是基于 `base_link_fake` 坐标系的，通过 tf2 将速度转换到 `base_link` 坐标系，使机器人能够正常运动。在仿真中表现即底盘小陀螺时仍能跟随轨迹。
-
-电控端执行底盘控制命令时，机器人运动正方向为云台枪管朝向。
-
-## fake_vel_transform_node
-
-订阅：
-
-- nav2 发布的基于 base_link_fake 坐标系的速度指令 `/cmd_vel`
-- nav2 controller 发布的局部路径朝向 `/local_path`
-- `odom` 到 `base_link` 的 tf 变换
-
-发布：
-
-- 转换到 base_link 坐标系的速度 `/cmd_vel_chassis`
-
-静态参数：
-
-- 底盘固定旋转速度 `spin_speed`
-
-  搭配电控固定小陀螺速度，将 spin_speed 设为负，可实现移动时小陀螺减慢。

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp

@@ -1,56 +0,0 @@
-#ifndef FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_
-#define FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_
-
-#include <message_filters/subscriber.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/create_timer_ros.h>
-#include <tf2_ros/message_filter.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <geometry_msgs/msg/transform_stamped.hpp>
-#include <geometry_msgs/msg/twist.hpp>
-#include <nav_msgs/msg/path.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp/subscription.hpp>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-
-namespace fake_vel_transform
-{
-class FakeVelTransform : public rclcpp::Node
-{
-public:
-  explicit FakeVelTransform(const rclcpp::NodeOptions & options);
-
-private:
-  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
-
-  void localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg);
-
-  void publishTransform();
-
-  // Subscriber with tf2 message_filter
-  std::string target_frame_;
-  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
-  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
-
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
-  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr local_pose_sub_;
-
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_chassis_pub_;
-
-  // Broadcast tf from base_link to base_link_fake
-  rclcpp::TimerBase::SharedPtr tf_timer_;
-  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
-
-  geometry_msgs::msg::PoseStamped planner_local_pose_;
-  double current_angle_;
-  double base_link_angle_;
-  float spin_speed_;
-};
-
-}  // namespace fake_vel_transform
-
-#endif  // FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/launch/fake_vel_transform.launch.py

@@ -1,20 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-def generate_launch_description():
-    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
-
-    fake_vel_transform_node = Node(
-        package='fake_vel_transform',
-        executable='fake_vel_transform_node',
-        output='screen',
-        parameters=[
-            {'use_sim_time': use_sim_time }
-        ]
-    )
-
-    return LaunchDescription([fake_vel_transform_node])

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/package.xml

@@ -1,31 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model
-  href="http://download.ros.org/schema/package_format3.xsd"
-  schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>fake_vel_transform</name>
-  <version>0.1.0</version>
-  <description>A template for ROS packages.</description>
-  <license>MIT</license>
-  <author email="lihanchen2004@163.com">Lihan Chen</author>
-  <maintainer email="lihanchen2004@163.com">Lihan Chen</maintainer>
-
-  <!-- buildtool_depend: dependencies of the build process -->
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <!-- depend: build, export, and execution dependency -->
-  <depend>rclcpp</depend>
-  <depend>rclcpp_components</depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-  <test_depend>ament_cmake_clang_format</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp

@@ -1,103 +0,0 @@
-#include "fake_vel_transform/fake_vel_transform.hpp"
-
-#include <tf2/utils.h>
-
-#include <rclcpp/logging.hpp>
-#include <rclcpp/qos.hpp>
-#include <rclcpp/utilities.hpp>
-
-namespace fake_vel_transform
-{
-const std::string CMD_VEL_TOPIC = "/cmd_vel";
-const std::string AFTER_TF_CMD_VEL = "/cmd_vel_chassis";
-const std::string TRAJECTORY_TOPIC = "/local_plan";
-const int TF_PUBLISH_FREQUENCY = 20;  // base_link to base_link_fake. Frequency in Hz.
-
-FakeVelTransform::FakeVelTransform(const rclcpp::NodeOptions & options)
-: Node("fake_vel_transform", options)
-{
-  RCLCPP_INFO(get_logger(), "Start FakeVelTransform!");
-
-  // Declare and get the spin speed parameter
-  this->declare_parameter<float>("spin_speed", -6.0);
-  this->get_parameter("spin_speed", spin_speed_);
-
-  // TF broadcaster
-  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
-  tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
-  tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
-
-  // Create Publisher and Subscriber
-  cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
-    CMD_VEL_TOPIC, 1, std::bind(&FakeVelTransform::cmdVelCallback, this, std::placeholders::_1));
-  cmd_vel_chassis_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
-    AFTER_TF_CMD_VEL, rclcpp::QoS(rclcpp::KeepLast(1)));
-  local_pose_sub_ = this->create_subscription<nav_msgs::msg::Path>(
-    TRAJECTORY_TOPIC, 1,
-    std::bind(&FakeVelTransform::localPoseCallback, this, std::placeholders::_1));
-
-  // Create a timer to publish the transform regularly
-  tf_timer_ = this->create_wall_timer(
-    std::chrono::milliseconds(1000 / TF_PUBLISH_FREQUENCY),
-    std::bind(&FakeVelTransform::publishTransform, this));
-}
-
-// Get the local pose from planner
-void FakeVelTransform::localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg)
-{
-  if (!msg || msg->poses.empty()) {
-    RCLCPP_WARN(get_logger(), "Received empty or invalid PoseArray message");
-    return;
-  }
-
-  // Choose the pose based on the size of the poses array
-  size_t index = std::min(msg->poses.size() / 4, msg->poses.size() - 1);
-  const geometry_msgs::msg::Pose & selected_pose = msg->poses[index].pose;
-
-  // Update current angle based on the difference between teb_angle and base_link_angle_
-  double teb_angle = tf2::getYaw(selected_pose.orientation);
-  current_angle_ = teb_angle - base_link_angle_;
-}
-
-// Transform the velocity from base_link to base_link_fake
-void FakeVelTransform::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
-{
-  try {
-    geometry_msgs::msg::TransformStamped transform_stamped;
-    transform_stamped = tf2_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero);
-    base_link_angle_ = tf2::getYaw(transform_stamped.transform.rotation);
-
-    double angle_diff = -current_angle_;
-
-    geometry_msgs::msg::Twist aft_tf_vel;
-    aft_tf_vel.angular.z = (msg->angular.z != 0) ? spin_speed_ : 0;
-    aft_tf_vel.linear.x = msg->linear.x * cos(angle_diff) + msg->linear.y * sin(angle_diff);
-    aft_tf_vel.linear.y = -msg->linear.x * sin(angle_diff) + msg->linear.y * cos(angle_diff);
-
-    cmd_vel_chassis_pub_->publish(aft_tf_vel);
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_INFO(this->get_logger(), "Could not transform odom to base_link: %s", ex.what());
-  }
-}
-
-// Publish transform from base_link to base_link_fake
-void FakeVelTransform::publishTransform()
-{
-  geometry_msgs::msg::TransformStamped t;
-  t.header.stamp = get_clock()->now();
-  t.header.frame_id = "base_link";
-  t.child_frame_id = "base_link_fake";
-  tf2::Quaternion q;
-  q.setRPY(0, 0, current_angle_);
-  t.transform.rotation = tf2::toMsg(q);
-  tf_broadcaster_->sendTransform(t);
-}
-
-}  // namespace fake_vel_transform
-
-#include "rclcpp_components/register_node_macro.hpp"
-
-// Register the component with class_loader.
-// This acts as a sort of entry point, allowing the component to be discoverable when its library
-// is being loaded into a running process.
-RCLCPP_COMPONENTS_REGISTER_NODE(fake_vel_transform::FakeVelTransform)

src/rm_nav_bringup/config/rm_navigation/rm_navigation/CMakeLists.txt

@@ -1,34 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(rm_navigation)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-if(CMAKE_EXPORT_COMPILE_COMMANDS)
-  set(CMAKE_EXPORT_COMPILE_COMMANDS ${CMAKE_EXPORT_COMPILE_COMMANDS})
-endif()
-
-# find dependencies
-find_package(ament_cmake_auto REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_auto_package(INSTALL_TO_SHARE
-    params
-    rviz
-    launch
-  )

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/bringup_rm_navigation.py

@@ -1,156 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import (DeclareLaunchArgument, GroupAction,
-                            IncludeLaunchDescription, SetEnvironmentVariable)
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from launch_ros.actions import PushRosNamespace
-from nav2_common.launch import RewrittenYaml
-
-
-def generate_launch_description():
-    # Get the launch directory
-    bringup_dir = get_package_share_directory('rm_navigation')
-    launch_dir = os.path.join(bringup_dir, 'launch')
-
-    # Create the launch configuration variables
-    namespace = LaunchConfiguration('namespace')
-    use_namespace = LaunchConfiguration('use_namespace')
-    map_yaml_file = LaunchConfiguration('map')
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    params_file = LaunchConfiguration('params_file')
-    autostart = LaunchConfiguration('autostart')
-    use_composition = LaunchConfiguration('use_composition')
-    use_respawn = LaunchConfiguration('use_respawn')
-    log_level = LaunchConfiguration('log_level')
-    use_nav_rviz = LaunchConfiguration('nav_rviz')
-
-    remappings = [('/tf', 'tf'),
-                  ('/tf_static', 'tf_static')]
-
-    # Create our own temporary YAML files that include substitutions
-    param_substitutions = {
-        'use_sim_time': use_sim_time,
-        'yaml_filename': map_yaml_file}
-
-    configured_params = RewrittenYaml(
-        source_file=params_file,
-        root_key=namespace,
-        param_rewrites=param_substitutions,
-        convert_types=True)
-
-    stdout_linebuf_envvar = SetEnvironmentVariable(
-        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
-
-    declare_namespace_cmd = DeclareLaunchArgument(
-        'namespace',
-        default_value='',
-        description='Top-level namespace')
-
-    declare_use_namespace_cmd = DeclareLaunchArgument(
-        'use_namespace',
-        default_value='false',
-        description='Whether to apply a namespace to the navigation stack')
-
-    declare_use_slam_cmd = DeclareLaunchArgument(
-        'use_slam',
-        default_value='True',
-        description='Whether run a SLAM')
-
-    declare_map_yaml_cmd = DeclareLaunchArgument(
-        'map',
-        default_value= os.path.join(bringup_dir,'map', 'RMUL.yaml'),
-        description='Full path to map yaml file to load')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        'use_sim_time',
-        default_value='True',
-        description='Use simulation (Gazebo) clock if true')
-
-    declare_params_file_cmd = DeclareLaunchArgument(
-        'params_file',
-        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
-        description='Full path to the ROS2 parameters file to use for all launched nodes')
-
-
-    declare_autostart_cmd = DeclareLaunchArgument(
-        'autostart', default_value='True',
-        description='Automatically startup the nav2 stack')
-
-    declare_use_composition_cmd = DeclareLaunchArgument(
-        'use_composition', default_value='True',
-        description='Whether to use composed bringup')
-
-    declare_use_respawn_cmd = DeclareLaunchArgument(
-        'use_respawn', default_value='True',
-        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
-
-    declare_log_level_cmd = DeclareLaunchArgument(
-        'log_level', default_value='info',
-        description='log level')
-    
-    declare_nav_rviz_cmd = DeclareLaunchArgument(
-        'nav_rviz',
-        default_value='True',
-        description='Visualize navigation2 if true')
-
-    # Specify the actions
-    bringup_cmd_group = GroupAction([
-        PushRosNamespace(
-            condition=IfCondition(use_namespace),
-            namespace=namespace),
-
-        Node(
-            condition=IfCondition(use_composition),
-            name='nav2_container',
-            package='rclcpp_components',
-            executable='component_container_mt',
-            parameters=[configured_params, {'autostart': autostart}],
-            arguments=['--ros-args', '--log-level', log_level],
-            remappings=remappings,
-            output='screen'),
-
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
-            launch_arguments={'namespace': namespace,
-                              'use_sim_time': use_sim_time,
-                              'autostart': autostart,
-                              'params_file': params_file,
-                              'use_composition': use_composition,
-                              'use_respawn': use_respawn,
-                              'container_name': 'nav2_container'}.items()),
-
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')),
-            condition=IfCondition(use_nav_rviz)
-        ),
-    ])
-
-    # Create the launch description and populate
-    ld = LaunchDescription()
-
-    # Set environment variables
-    ld.add_action(stdout_linebuf_envvar)
-
-    # Declare the launch options
-    ld.add_action(declare_namespace_cmd)
-    ld.add_action(declare_use_namespace_cmd)
-    ld.add_action(declare_use_slam_cmd)
-    ld.add_action(declare_map_yaml_cmd)
-    ld.add_action(declare_use_sim_time_cmd)
-    ld.add_action(declare_params_file_cmd)
-    ld.add_action(declare_autostart_cmd)
-    ld.add_action(declare_use_composition_cmd)
-    ld.add_action(declare_use_respawn_cmd)
-    ld.add_action(declare_log_level_cmd)
-    ld.add_action(declare_nav_rviz_cmd)
-
-    # Add the actions to launch all of the navigation nodes
-    ld.add_action(bringup_cmd_group)
-
-    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/localization_amcl_launch.py

@@ -1,149 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable
-from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import LoadComposableNodes, Node
-from launch_ros.descriptions import ComposableNode, ParameterFile
-from nav2_common.launch import RewrittenYaml
-
-
-def generate_launch_description():
-    # Get the launch directory
-    bringup_dir = get_package_share_directory('nav2_bringup')
-
-    namespace = LaunchConfiguration('namespace')
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    autostart = LaunchConfiguration('autostart')
-    params_file = LaunchConfiguration('params_file')
-    use_composition = LaunchConfiguration('use_composition')
-    container_name = LaunchConfiguration('container_name')
-    container_name_full = (namespace, '/', container_name)
-    use_respawn = LaunchConfiguration('use_respawn')
-    log_level = LaunchConfiguration('log_level')
-
-    lifecycle_nodes = ['amcl']
-
-    remappings = [('/tf', 'tf'),
-                  ('/tf_static', 'tf_static')]
-
-    # Create our own temporary YAML files that include substitutions
-    param_substitutions = {
-        'use_sim_time': use_sim_time}
-
-    configured_params = ParameterFile(
-        RewrittenYaml(
-            source_file=params_file,
-            root_key=namespace,
-            param_rewrites=param_substitutions,
-            convert_types=True),
-        allow_substs=True)
-
-    stdout_linebuf_envvar = SetEnvironmentVariable(
-        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
-
-    declare_namespace_cmd = DeclareLaunchArgument(
-        'namespace',
-        default_value='',
-        description='Top-level namespace')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        'use_sim_time',
-        default_value='false',
-        description='Use simulation (Gazebo) clock if true')
-
-    declare_params_file_cmd = DeclareLaunchArgument(
-        'params_file',
-        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
-        description='Full path to the ROS2 parameters file to use for all launched nodes')
-
-    declare_autostart_cmd = DeclareLaunchArgument(
-        'autostart', default_value='true',
-        description='Automatically startup the nav2 stack')
-
-    declare_use_composition_cmd = DeclareLaunchArgument(
-        'use_composition', default_value='False',
-        description='Use composed bringup if True')
-
-    declare_container_name_cmd = DeclareLaunchArgument(
-        'container_name', default_value='nav2_container',
-        description='the name of conatiner that nodes will load in if use composition')
-
-    declare_use_respawn_cmd = DeclareLaunchArgument(
-        'use_respawn', default_value='False',
-        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
-
-    declare_log_level_cmd = DeclareLaunchArgument(
-        'log_level', default_value='info',
-        description='log level')
-
-    load_nodes = GroupAction(
-        condition=IfCondition(PythonExpression(['not ', use_composition])),
-        actions=[
-            Node(
-                package='nav2_amcl',
-                executable='amcl',
-                name='amcl',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings),
-            Node(
-                package='nav2_lifecycle_manager',
-                executable='lifecycle_manager',
-                name='lifecycle_manager_localization',
-                output='screen',
-                arguments=['--ros-args', '--log-level', log_level],
-                parameters=[{'use_sim_time': use_sim_time},
-                            {'autostart': autostart},
-                            {'node_names': lifecycle_nodes}])
-        ]
-    )
-
-    load_composable_nodes = LoadComposableNodes(
-        condition=IfCondition(use_composition),
-        target_container=container_name_full,
-        composable_node_descriptions=[
-            ComposableNode(
-                package='nav2_amcl',
-                plugin='nav2_amcl::AmclNode',
-                name='amcl',
-                parameters=[configured_params],
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_lifecycle_manager',
-                plugin='nav2_lifecycle_manager::LifecycleManager',
-                name='lifecycle_manager_localization',
-                parameters=[{'use_sim_time': use_sim_time,
-                             'autostart': autostart,
-                             'node_names': lifecycle_nodes}]),
-        ],
-    )
-
-    # Create the launch description and populate
-    ld = LaunchDescription()
-
-    # Set environment variables
-    ld.add_action(stdout_linebuf_envvar)
-
-    # Declare the launch options
-    ld.add_action(declare_namespace_cmd)
-    ld.add_action(declare_use_sim_time_cmd)
-    ld.add_action(declare_params_file_cmd)
-    ld.add_action(declare_autostart_cmd)
-    ld.add_action(declare_use_composition_cmd)
-    ld.add_action(declare_container_name_cmd)
-    ld.add_action(declare_use_respawn_cmd)
-    ld.add_action(declare_log_level_cmd)
-
-    # Add the actions to launch all of the localiztion nodes
-    ld.add_action(load_nodes)
-    ld.add_action(load_composable_nodes)
-
-    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/map_server_launch.py

@@ -1,158 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
-from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import LoadComposableNodes
-from launch_ros.actions import Node
-from launch_ros.descriptions import ComposableNode, ParameterFile
-from nav2_common.launch import RewrittenYaml
-
-
-def generate_launch_description():
-    # Get the launch directory
-    bringup_dir = get_package_share_directory('nav2_bringup')
-
-    namespace = LaunchConfiguration('namespace')
-    map_yaml_file = LaunchConfiguration('map')
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    autostart = LaunchConfiguration('autostart')
-    params_file = LaunchConfiguration('params_file')
-    use_composition = LaunchConfiguration('use_composition')
-    container_name = LaunchConfiguration('container_name')
-    container_name_full = (namespace, '/', container_name)
-    use_respawn = LaunchConfiguration('use_respawn')
-    log_level = LaunchConfiguration('log_level')
-
-    lifecycle_nodes = ['map_server']
-
-    remappings = [('/tf', 'tf'),
-                  ('/tf_static', 'tf_static')]
-
-    # Create our own temporary YAML files that include substitutions
-    param_substitutions = {
-        'use_sim_time': use_sim_time,
-        'yaml_filename': map_yaml_file}
-
-    configured_params = ParameterFile(
-        RewrittenYaml(
-            source_file=params_file,
-            root_key=namespace,
-            param_rewrites=param_substitutions,
-            convert_types=True),
-        allow_substs=True)
-
-    stdout_linebuf_envvar = SetEnvironmentVariable(
-        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
-
-    declare_namespace_cmd = DeclareLaunchArgument(
-        'namespace',
-        default_value='',
-        description='Top-level namespace')
-
-    declare_map_yaml_cmd = DeclareLaunchArgument(
-        'map',
-        description='Full path to map yaml file to load')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        'use_sim_time',
-        default_value='false',
-        description='Use simulation (Gazebo) clock if true')
-
-    declare_params_file_cmd = DeclareLaunchArgument(
-        'params_file',
-        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
-        description='Full path to the ROS2 parameters file to use for all launched nodes')
-
-    declare_autostart_cmd = DeclareLaunchArgument(
-        'autostart', default_value='true',
-        description='Automatically startup the nav2 stack')
-
-    declare_use_composition_cmd = DeclareLaunchArgument(
-        'use_composition', default_value='False',
-        description='Use composed bringup if True')
-
-    declare_container_name_cmd = DeclareLaunchArgument(
-        'container_name', default_value='nav2_container',
-        description='the name of conatiner that nodes will load in if use composition')
-
-    declare_use_respawn_cmd = DeclareLaunchArgument(
-        'use_respawn', default_value='False',
-        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
-
-    declare_log_level_cmd = DeclareLaunchArgument(
-        'log_level', default_value='info',
-        description='log level')
-
-    load_nodes = GroupAction(
-        condition=IfCondition(PythonExpression(['not ', use_composition])),
-        actions=[
-            Node(
-                package='nav2_map_server',
-                executable='map_server',
-                name='map_server',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings),
-
-            Node(
-                package='nav2_lifecycle_manager',
-                executable='lifecycle_manager',
-                name='lifecycle_manager_localization',
-                output='screen',
-                arguments=['--ros-args', '--log-level', log_level],
-                parameters=[{'use_sim_time': use_sim_time},
-                            {'autostart': autostart},
-                            {'node_names': lifecycle_nodes}])
-        ]
-    )
-
-    load_composable_nodes = LoadComposableNodes(
-        condition=IfCondition(use_composition),
-        target_container=container_name_full,
-        composable_node_descriptions=[
-            ComposableNode(
-                package='nav2_map_server',
-                plugin='nav2_map_server::MapServer',
-                name='map_server',
-                parameters=[configured_params],
-                remappings=remappings),
-
-            ComposableNode(
-                package='nav2_lifecycle_manager',
-                plugin='nav2_lifecycle_manager::LifecycleManager',
-                name='lifecycle_manager_localization',
-                parameters=[{'use_sim_time': use_sim_time,
-                             'autostart': autostart,
-                             'node_names': lifecycle_nodes}]),
-        ],
-    )
-
-    # Create the launch description and populate
-    ld = LaunchDescription()
-
-    # Set environment variables
-    ld.add_action(stdout_linebuf_envvar)
-
-    # Declare the launch options
-    ld.add_action(declare_namespace_cmd)
-    ld.add_action(declare_map_yaml_cmd)
-    ld.add_action(declare_use_sim_time_cmd)
-    ld.add_action(declare_params_file_cmd)
-    ld.add_action(declare_autostart_cmd)
-    ld.add_action(declare_use_composition_cmd)
-    ld.add_action(declare_container_name_cmd)
-    ld.add_action(declare_use_respawn_cmd)
-    ld.add_action(declare_log_level_cmd)
-
-    # Add the actions to launch all of the localiztion nodes
-    ld.add_action(load_nodes)
-    ld.add_action(load_composable_nodes)
-
-    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/navigation_launch.py

@@ -1,270 +0,0 @@
-# Copyright (c) 2018 Intel Corporation
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
-from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import LoadComposableNodes
-from launch_ros.actions import Node
-from launch_ros.descriptions import ComposableNode
-from nav2_common.launch import RewrittenYaml
-
-
-def generate_launch_description():
-    # Get the launch directory
-    bringup_dir = get_package_share_directory('rm_navigation')
-
-    namespace = LaunchConfiguration('namespace')
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    autostart = LaunchConfiguration('autostart')
-    params_file = LaunchConfiguration('params_file')
-    use_composition = LaunchConfiguration('use_composition')
-    container_name = LaunchConfiguration('container_name')
-    container_name_full = (namespace, '/', container_name)
-    use_respawn = LaunchConfiguration('use_respawn')
-    log_level = LaunchConfiguration('log_level')
-
-    lifecycle_nodes = ['controller_server',
-                       'smoother_server',
-                       'planner_server',
-                       'behavior_server',
-                       'bt_navigator',
-                       'waypoint_follower',
-                       'velocity_smoother']
-
-    # Map fully qualified names to relative ones so the node's namespace can be prepended.
-    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
-    # https://github.com/ros/geometry2/issues/32
-    # https://github.com/ros/robot_state_publisher/pull/30
-    # TODO(orduno) Substitute with `PushNodeRemapping`
-    #              https://github.com/ros2/launch_ros/issues/56
-    remappings = [('/tf', 'tf'),
-                  ('/tf_static', 'tf_static')]
-
-    # Create our own temporary YAML files that include substitutions
-    param_substitutions = {
-        'use_sim_time': use_sim_time,
-        'autostart': autostart}
-
-    configured_params = RewrittenYaml(
-            source_file=params_file,
-            root_key=namespace,
-            param_rewrites=param_substitutions,
-            convert_types=True)
-
-    stdout_linebuf_envvar = SetEnvironmentVariable(
-        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
-
-    declare_namespace_cmd = DeclareLaunchArgument(
-        'namespace',
-        default_value='',
-        description='Top-level namespace')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        'use_sim_time',
-        default_value='true',
-        description='Use simulation (Gazebo) clock if true')
-
-    declare_params_file_cmd = DeclareLaunchArgument(
-        'params_file',
-        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
-        description='Full path to the ROS2 parameters file to use for all launched nodes')
-
-    declare_autostart_cmd = DeclareLaunchArgument(
-        'autostart', default_value='true',
-        description='Automatically startup the nav2 stack')
-
-    declare_use_composition_cmd = DeclareLaunchArgument(
-        'use_composition', default_value='False',
-        description='Use composed bringup if True')
-
-    declare_container_name_cmd = DeclareLaunchArgument(
-        'container_name', default_value='nav2_container',
-        description='the name of conatiner that nodes will load in if use composition')
-
-    declare_use_respawn_cmd = DeclareLaunchArgument(
-        'use_respawn', default_value='False',
-        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
-
-    declare_log_level_cmd = DeclareLaunchArgument(
-        'log_level', default_value='info',
-        description='log level')
-
-    load_nodes = GroupAction(
-        condition=IfCondition(PythonExpression(['not ', use_composition])),
-        actions=[
-            Node(
-                package='nav2_controller',
-                executable='controller_server',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
-            Node(
-                package='nav2_smoother',
-                executable='smoother_server',
-                name='smoother_server',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings),
-            Node(
-                package='nav2_planner',
-                executable='planner_server',
-                name='planner_server',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings),
-            Node(
-                package='nav2_behaviors',
-                executable='behavior_server',
-                name='behavior_server',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings),
-            Node(
-                package='nav2_bt_navigator',
-                executable='bt_navigator',
-                name='bt_navigator',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings),
-            Node(
-                package='nav2_waypoint_follower',
-                executable='waypoint_follower',
-                name='waypoint_follower',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings),
-            Node(
-                package='nav2_velocity_smoother',
-                executable='velocity_smoother',
-                name='velocity_smoother',
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-                remappings=remappings +
-                        [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
-            Node(
-                package='nav2_lifecycle_manager',
-                executable='lifecycle_manager',
-                name='lifecycle_manager_navigation',
-                output='screen',
-                arguments=['--ros-args', '--log-level', log_level],
-                parameters=[{'use_sim_time': use_sim_time},
-                            {'autostart': autostart},
-                            {'node_names': lifecycle_nodes}]),
-        ]
-    )
-
-    load_composable_nodes = LoadComposableNodes(
-        condition=IfCondition(use_composition),
-        target_container=container_name_full,
-        composable_node_descriptions=[
-            ComposableNode(
-                package='nav2_controller',
-                plugin='nav2_controller::ControllerServer',
-                name='controller_server',
-                parameters=[configured_params],
-                remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
-            ComposableNode(
-                package='nav2_smoother',
-                plugin='nav2_smoother::SmootherServer',
-                name='smoother_server',
-                parameters=[configured_params],
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_planner',
-                plugin='nav2_planner::PlannerServer',
-                name='planner_server',
-                parameters=[configured_params],
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_behaviors',
-                plugin='behavior_server::BehaviorServer',
-                name='behavior_server',
-                parameters=[configured_params],
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_bt_navigator',
-                plugin='nav2_bt_navigator::BtNavigator',
-                name='bt_navigator',
-                parameters=[configured_params],
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_waypoint_follower',
-                plugin='nav2_waypoint_follower::WaypointFollower',
-                name='waypoint_follower',
-                parameters=[configured_params],
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_velocity_smoother',
-                plugin='nav2_velocity_smoother::VelocitySmoother',
-                name='velocity_smoother',
-                parameters=[configured_params],
-                remappings=remappings +
-                           [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
-            ComposableNode(
-                package='nav2_lifecycle_manager',
-                plugin='nav2_lifecycle_manager::LifecycleManager',
-                name='lifecycle_manager_navigation',
-                parameters=[{'use_sim_time': use_sim_time,
-                             'autostart': autostart,
-                             'node_names': lifecycle_nodes}]),
-        ],
-    )
-
-    # Create the launch description and populate
-    ld = LaunchDescription()
-
-    # Set environment variables
-    ld.add_action(stdout_linebuf_envvar)
-
-    # Declare the launch options
-    ld.add_action(declare_namespace_cmd)
-    ld.add_action(declare_use_sim_time_cmd)
-    ld.add_action(declare_params_file_cmd)
-    ld.add_action(declare_autostart_cmd)
-    ld.add_action(declare_use_composition_cmd)
-    ld.add_action(declare_container_name_cmd)
-    ld.add_action(declare_use_respawn_cmd)
-    ld.add_action(declare_log_level_cmd)
-    # Add the actions to launch all of the navigation nodes
-    ld.add_action(load_nodes)
-    ld.add_action(load_composable_nodes)
-
-    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/rviz_launch.py

@@ -1,109 +0,0 @@
-# Copyright (c) 2018 Intel Corporation
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
-from launch.conditions import IfCondition, UnlessCondition
-from launch.event_handlers import OnProcessExit
-from launch.events import Shutdown
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from nav2_common.launch import ReplaceString
-
-
-def generate_launch_description():
-    # Get the launch directory
-    bringup_dir = get_package_share_directory('rm_navigation')
-
-    # Create the launch configuration variables
-    namespace = LaunchConfiguration('namespace')
-    use_namespace = LaunchConfiguration('use_namespace')
-    rviz_config_file = LaunchConfiguration('rviz_config')
-
-    # Declare the launch arguments
-    declare_namespace_cmd = DeclareLaunchArgument(
-        'namespace',
-        default_value='navigation',
-        description=('Top-level namespace. The value will be used to replace the '
-                     '<robot_namespace> keyword on the rviz config file.'))
-
-    declare_use_namespace_cmd = DeclareLaunchArgument(
-        'use_namespace',
-        default_value='false',
-        description='Whether to apply a namespace to the navigation stack')
-
-    declare_rviz_config_file_cmd = DeclareLaunchArgument(
-        'rviz_config',
-        default_value=os.path.join(bringup_dir, 'rviz', 'nav2.rviz'),
-        description='Full path to the RVIZ config file to use')
-
-    # Launch rviz
-    start_rviz_cmd = Node(
-        condition=UnlessCondition(use_namespace),
-        package='rviz2',
-        executable='rviz2',
-        arguments=['-d', rviz_config_file],
-        output='screen')
-
-    namespaced_rviz_config_file = ReplaceString(
-            source_file=rviz_config_file,
-            replacements={'<robot_namespace>': ('/', namespace)})
-
-    start_namespaced_rviz_cmd = Node(
-        condition=IfCondition(use_namespace),
-        package='rviz2',
-        executable='rviz2',
-        namespace=namespace,
-        arguments=['-d', namespaced_rviz_config_file],
-        output='screen',
-        remappings=[('/map', 'map'),
-                    ('/tf', 'tf'),
-                    ('/tf_static', 'tf_static'),
-                    ('/goal_pose', 'goal_pose'),
-                    ('/clicked_point', 'clicked_point'),
-                    ('/initialpose', 'initialpose')])
-
-    exit_event_handler = RegisterEventHandler(
-        condition=UnlessCondition(use_namespace),
-        event_handler=OnProcessExit(
-            target_action=start_rviz_cmd,
-            on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
-
-    exit_event_handler_namespaced = RegisterEventHandler(
-        condition=IfCondition(use_namespace),
-        event_handler=OnProcessExit(
-            target_action=start_namespaced_rviz_cmd,
-            on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
-
-    # Create the launch description and populate
-    ld = LaunchDescription()
-
-    # Declare the launch options
-    ld.add_action(declare_namespace_cmd)
-    ld.add_action(declare_use_namespace_cmd)
-    ld.add_action(declare_rviz_config_file_cmd)
-
-    # Add any conditioned actions
-    ld.add_action(start_rviz_cmd)
-    ld.add_action(start_namespaced_rviz_cmd)
-
-    # Add other nodes and processes we need
-    ld.add_action(exit_event_handler)
-    ld.add_action(exit_event_handler_namespaced)
-
-    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/package.xml

@@ -1,20 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>rm_navigation</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="fyt@todo.todo">fyt</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <exec_depend>spatio_temporal_voxel_layer</exec_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/mapper_params_online_async.yaml

@@ -1,73 +0,0 @@
-slam_toolbox:
-  ros__parameters:
-
-    # Plugin params
-    solver_plugin: solver_plugins::CeresSolver
-    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
-    ceres_preconditioner: SCHUR_JACOBI
-    ceres_trust_strategy: LEVENBERG_MARQUARDT
-    ceres_dogleg_type: TRADITIONAL_DOGLEG
-    ceres_loss_function: None
-
-    # ROS Parameters
-    odom_frame: odom 
-    map_frame: map
-    base_frame: livox_frame 
-    scan_topic: /scan
-    mode: mapping #localization
-
-    # if you'd like to immediately start continuing a map at a given pose
-    # or at the dock, but they are mutually exclusive, if pose is given
-    # will use pose
-    #map_file_name: test_steve
-    # map_start_pose: [0.0, 0.0, 0.0]
-    #map_start_at_dock: true
-
-    debug_logging: false
-    throttle_scans: 1
-    transform_publish_period: 0.02 #if 0 never publishes odometry
-    map_update_interval: 5.0
-    resolution: 0.05
-    max_laser_range: 20.0 #for rastering images
-    minimum_time_interval: 0.5
-    transform_timeout: 0.2
-    tf_buffer_duration: 30.
-    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
-    enable_interactive_mode: true
-
-    # General Parameters
-    use_scan_matching: true
-    use_scan_barycenter: true
-    minimum_travel_distance: 0.5
-    minimum_travel_heading: 0.5
-    scan_buffer_size: 10
-    scan_buffer_maximum_scan_distance: 10.0
-    link_match_minimum_response_fine: 0.1  
-    link_scan_maximum_distance: 1.5
-    loop_search_maximum_distance: 3.0
-    do_loop_closing: true 
-    loop_match_minimum_chain_size: 10           
-    loop_match_maximum_variance_coarse: 3.0  
-    loop_match_minimum_response_coarse: 0.35    
-    loop_match_minimum_response_fine: 0.45
-
-    # Correlation Parameters - Correlation Parameters
-    correlation_search_space_dimension: 0.5
-    correlation_search_space_resolution: 0.01
-    correlation_search_space_smear_deviation: 0.1 
-
-    # Correlation Parameters - Loop Closure Parameters
-    loop_search_space_dimension: 8.0
-    loop_search_space_resolution: 0.05
-    loop_search_space_smear_deviation: 0.03
-
-    # Scan Matcher Parameters
-    distance_variance_penalty: 0.5      
-    angle_variance_penalty: 1.0    
-
-    fine_search_angle_offset: 0.00349     
-    coarse_search_angle_offset: 0.349   
-    coarse_angle_resolution: 0.0349        
-    minimum_angle_penalty: 0.9
-    minimum_distance_penalty: 0.5
-    use_response_expansion: true

src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/nav2_params.yaml

@@ -1,371 +0,0 @@
-amcl:
-  ros__parameters:
-    use_sim_time: False
-    alpha1: 0.2
-    alpha2: 0.2
-    alpha3: 0.2
-    alpha4: 0.2
-    alpha5: 0.2
-    base_frame_id: "base_link"
-    beam_skip_distance: 0.5
-    beam_skip_error_threshold: 0.9
-    beam_skip_threshold: 0.3
-    do_beamskip: false
-    global_frame_id: "map"
-    lambda_short: 0.1
-    laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
-    laser_min_range: -1.0
-    laser_model_type: "likelihood_field"
-    max_beams: 60
-    max_particles: 2000
-    min_particles: 500
-    odom_frame_id: "odom"
-    pf_err: 0.05
-    pf_z: 0.99
-    recovery_alpha_fast: 0.0
-    recovery_alpha_slow: 0.0
-    resample_interval: 1
-    robot_model_type: "nav2_amcl::OmniMotionModel"
-    save_pose_rate: 0.5
-    sigma_hit: 0.2
-    tf_broadcast: true
-    transform_tolerance: 1.0
-    update_min_a: 0.2
-    update_min_d: 0.25
-    z_hit: 0.5
-    z_max: 0.05
-    z_rand: 0.5
-    z_short: 0.05
-    scan_topic: scan
-
-amcl_map_client:
-  ros__parameters:
-    use_sim_time: False
-
-amcl_rclcpp_node:
-  ros__parameters:
-    use_sim_time: False
-
-bt_navigator:
-  ros__parameters:
-    use_sim_time: False
-    global_frame: map
-    robot_base_frame: base_link
-    odom_topic: /Odometry
-    bt_loop_duration: 10
-    default_server_timeout: 20
-    enable_groot_monitoring: True
-    groot_zmq_publisher_port: 1666
-    groot_zmq_server_port: 1667
-    plugin_lib_names:
-    - nav2_compute_path_to_pose_action_bt_node
-    - nav2_compute_path_through_poses_action_bt_node
-    - nav2_follow_path_action_bt_node
-    - nav2_back_up_action_bt_node
-    - nav2_spin_action_bt_node
-    - nav2_wait_action_bt_node
-    - nav2_clear_costmap_service_bt_node
-    - nav2_is_stuck_condition_bt_node
-    - nav2_goal_reached_condition_bt_node
-    - nav2_goal_updated_condition_bt_node
-    - nav2_initial_pose_received_condition_bt_node
-    - nav2_reinitialize_global_localization_service_bt_node
-    - nav2_rate_controller_bt_node
-    - nav2_distance_controller_bt_node
-    - nav2_speed_controller_bt_node
-    - nav2_truncate_path_action_bt_node
-    - nav2_goal_updater_node_bt_node
-    - nav2_recovery_node_bt_node
-    - nav2_pipeline_sequence_bt_node
-    - nav2_round_robin_node_bt_node
-    - nav2_transform_available_condition_bt_node
-    - nav2_time_expired_condition_bt_node
-    - nav2_distance_traveled_condition_bt_node
-    - nav2_single_trigger_bt_node
-    - nav2_is_battery_low_condition_bt_node
-    - nav2_navigate_through_poses_action_bt_node
-    - nav2_navigate_to_pose_action_bt_node
-    - nav2_remove_passed_goals_action_bt_node
-    - nav2_planner_selector_bt_node
-    - nav2_controller_selector_bt_node
-    - nav2_goal_checker_selector_bt_node
-
-bt_navigator_rclcpp_node:
-  ros__parameters:
-    use_sim_time: False
-
-controller_server:
-  ros__parameters:
-    use_sim_time: False
-    controller_frequency: 5.0
-    min_x_velocity_threshold: 0.001
-    min_y_velocity_threshold: 0.5
-    min_theta_velocity_threshold: 0.001
-    failure_tolerance: 0.3
-    progress_checker_plugin: "progress_checker"
-    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
-    controller_plugins: ["FollowPath"]
-
-    # Progress checker parameters
-    progress_checker:
-      plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.5
-      movement_time_allowance: 10.0
-
-    # Goal checker parameters
-    general_goal_checker:
-      stateful: True
-      plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
-    # DWB parameters
-    FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: False
-      min_vel_x: -2.5
-      min_vel_y: -2.5
-      max_vel_x: 2.5
-      max_vel_y: 2.5
-      max_vel_theta: 12.0
-      min_speed_xy: -8.0
-      max_speed_xy: 8.0
-      min_speed_theta: -12.0
-      acc_lim_x: 5.0
-      acc_lim_y: 5.0
-      acc_lim_theta: 15.0
-      decel_lim_x: -5.0
-      decel_lim_y: -5.0
-      decel_lim_theta: -15.0
-      vx_samples: 20
-      vy_samples: 20
-      vtheta_samples: 20
-      sim_time: 0.51
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
-      GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
-      
-controller_server_rclcpp_node:
-  ros__parameters:
-    use_sim_time: False
-
-local_costmap:
-  local_costmap:
-    ros__parameters:
-      update_frequency: 20.0
-      publish_frequency: 10.0
-      # global_frame: odom
-      global_frame: map
-      robot_base_frame: base_link
-      use_sim_time: False
-      rolling_window: true
-      width: 5
-      height: 5
-      resolution: 0.05
-      robot_radius: 0.2
-      plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"]
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 8.0
-        inflation_radius: 0.7
-      voxel2d_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
-        enabled: True
-        publish_voxel_map: False
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        mark_threshold: 0
-        observation_sources: scan 
-        scan:
-          topic: /scan
-          raytrace_min_range: 0.0
-          raytrace_max_range: 50.0
-          min_obstacle_height: 0.0
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
-      voxel3d_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
-        enabled: True
-        publish_voxel_map: False
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        mark_threshold: 1
-        observation_sources: livox
-        min_obstacle_height: 0.00
-        max_obstacle_height: 2.0 
-        livox:
-          topic: /livox/lidar/pointcloud
-          raytrace_min_range: 0.0
-          raytrace_max_range: 50.0
-          min_obstacle_height: 0.0
-          max_obstacle_height: 2.0 
-          clearing: True
-          marking: True
-          data_type: "PointCloud2"
-      static_layer:
-        map_subscribe_transient_local: True
-      always_send_full_costmap: True
-  local_costmap_client:
-    ros__parameters:
-      use_sim_time: False
-  local_costmap_rclcpp_node:
-    ros__parameters:
-      use_sim_time: False
-
-global_costmap:
-  global_costmap:
-    ros__parameters:
-      update_frequency: 20.0
-      publish_frequency: 10.0
-      global_frame: map
-      robot_base_frame: base_link
-      use_sim_time: False
-      robot_radius: 0.2
-      resolution: 0.05
-      track_unknown_space: true
-      plugins: ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"]
-      voxel2d_layer:
-        plugin: "nav2_costmap_2d::ObstacleLayer"
-        enabled: True
-        observation_sources: scan
-        scan:
-          topic: /scan
-          raytrace_min_range: 0.0
-          raytrace_max_range: 50.0
-          min_obstacle_height: 0.0
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
-      voxel3d_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
-        enabled: True
-        publish_voxel_map: False
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        mark_threshold: 1
-        observation_sources: livox
-        min_obstacle_height: 0.00
-        max_obstacle_height: 2.0 
-        livox:
-          topic: /livox/lidar/pointcloud
-          raytrace_min_range: 0.0
-          raytrace_max_range: 50.0
-          min_obstacle_height: 0.0
-          max_obstacle_height: 2.0 
-          clearing: True
-          marking: True
-          data_type: "PointCloud2"
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 8.0
-        inflation_radius: 0.7
-      always_send_full_costmap: True
-  global_costmap_client:
-    ros__parameters:
-      use_sim_time: False
-  global_costmap_rclcpp_node:
-    ros__parameters:
-      use_sim_time: False
-
-map_server:
-  ros__parameters:
-    use_sim_time: False
-    yaml_filename: ""
-
-map_saver:
-  ros__parameters:
-    use_sim_time: False
-    save_map_timeout: 5.0
-    free_thresh_default: 0.25
-    occupied_thresh_default: 0.65
-    map_subscribe_transient_local: True
-
-planner_server:
-  ros__parameters:
-    expected_planner_frequency: 20.0
-    use_sim_time: False
-    planner_plugins: ["GridBased"]
-    GridBased:
-      plugin: "nav2_navfn_planner/NavfnPlanner"
-      tolerance: 0.5
-      use_astar: false
-      allow_unknown: true
-
-# planner_server_rclcpp_node:
-#   ros__parameters:
-#     use_sim_time: False
-
-recoveries_server:
-  ros__parameters:
-    costmap_topic: local_costmap/costmap_raw
-    footprint_topic: local_costmap/published_footprint
-    cycle_frequency: 10.0
-    recovery_plugins: ["spin", "backup", "wait"]
-    spin:
-      plugin: "nav2_recoveries/Spin"
-    backup:
-      plugin: "nav2_recoveries/BackUp"
-    wait:
-      plugin: "nav2_recoveries/Wait"
-    global_frame: odom
-    robot_base_frame: base_link
-    transform_timeout: 0.1
-    use_sim_time: False
-    simulate_ahead_time: 2.0
-    max_rotational_vel: 20.0
-    min_rotational_vel: 0.4
-    rotational_acc_lim: 10.0
-
-robot_state_publisher:
-  ros__parameters:
-    use_sim_time: False
-
-waypoint_follower:
-  ros__parameters:
-    loop_rate: 20
-    stop_on_failure: false
-    waypoint_task_executor_plugin: "wait_at_waypoint"   
-    wait_at_waypoint:
-      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
-      enabled: True
-      waypoint_pause_duration: 200
-
-velocity_smoother:
-  ros__parameters:
-    use_sim_time: True
-    smoothing_frequency: 5.0
-    scale_velocities: False
-    feedback: "CLOSED_LOOP"
-    max_velocity: [2.5, 2.5, 12.0]
-    min_velocity: [-2.5, -2.5, -12.0]
-    max_accel: [5.0, 5.0, 15.0]
-    max_decel: [-5.0, -5.0, -15.0]
-    odom_topic: "Odometry"
-    odom_duration: 0.1
-    deadband_velocity: [0.0, 0.0, 0.0]
-    velocity_timeout: 1.0

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitignore

@@ -1,3 +0,0 @@
-.svn
-.kdev4
-*.kdev4

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitlab-ci.yml

@@ -1,23 +0,0 @@
-build-kinetic:
-  variables:
-    ROSDISTRO: "kinetic"
-    CATKIN_WS: "/root/catkin_ws"
-  stage: build
-  image: osrf/ros:$ROSDISTRO-desktop-full
-  before_script: 
-    - apt-get -qq update
-    - apt-get -qq install git-core python-catkin-tools curl
-    - mkdir -p $CATKIN_WS/src
-    - cp -a . $CATKIN_WS/src/
-    - cd $CATKIN_WS
-    - rosdep update
-    - rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false
-  script: 
-    - source /ros_entrypoint.sh
-    - cd $CATKIN_WS
-    - catkin build -i -s --no-status
-  only:
-    - kinetic-devel
-  tags:
-    - ubuntu
-    - docker