EKF/SLAM Object Tracking & Map Publishing

all_seaing_autonomy/all_seaing_autonomy/roboboat/waypoint_finder.py

@@ -12,7 +12,6 @@
 import os
 from ament_index_python.packages import get_package_share_directory
 
-
 class WaypointFinder(Node):
     def __init__(self):
         super().__init__("waypoint_finder")

all_seaing_bringup/config/perception/contour_matching_color_ranges.yaml

@@ -0,0 +1,6 @@
+orange: [10, 30, 40, 255, 40, 255]
+red: [0, 15, 40, 255, 40, 255]
+red2: [150, 179, 40, 255, 40, 255]
+green: [35, 80, 40, 255, 40, 255]
+black: [0, 0, 0, 0, 0, 50]
+white: [0, 0, 0, 10, 200, 255]

all_seaing_bringup/config/perception/matching_weights.yaml

@@ -0,0 +1,7 @@
+clustering_distance_weight: 1.0
+clustering_color_weights: [1.0, 0.0, 0.0]
+clustering_color_thres: 30
+cluster_contour_distance_weight: 1.0
+cluster_contour_color_weights: [1.0, 0.0, 0.0]
+contour_detection_color_weights: [1.0, 0.0, 0.0]
+cluster_contour_size_weight: 1.0

all_seaing_bringup/launch/sim.launch.py

@@ -33,7 +33,12 @@ def launch_setup(context, *args, **kwargs):
     color_ranges = os.path.join(
         bringup_prefix, "config", "perception", "color_ranges.yaml"
     )
-
+    matching_weights = os.path.join(
+        bringup_prefix, "config", "perception", "matching_weights.yaml"
+    )
+    contour_matching_color_ranges = os.path.join(
+        bringup_prefix, "config", "perception", "contour_matching_color_ranges.yaml"
+    )
     subprocess.run(["cp", "-r", os.path.join(bringup_prefix, "tile"), "/tmp"])
 
     launch_rviz = LaunchConfiguration("launch_rviz")
@@ -165,6 +170,46 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
+    bbox_project_pcloud_node = launch_ros.actions.Node(
+        package="all_seaing_perception",
+        executable="bbox_project_pcloud",
+        output="screen",
+        remappings=[
+            ("camera_info_topic", "/wamv/sensors/cameras/front_left_camera_sensor/camera_info"),
+            ("camera_topic", "/wamv/sensors/cameras/front_left_camera_sensor/image_raw"),
+            ("lidar_topic", "point_cloud/filtered")
+        ],
+        parameters=[
+            {"bbox_object_margin": 0.0},
+            {"color_label_mappings_file": color_label_mappings},
+            {"color_ranges_file": color_ranges},
+            {"obstacle_size_min": 2},
+            {"obstacle_size_max": 60},
+            {"clustering_distance": 1.0},
+            {"matching_weights_file": matching_weights},
+            {"contour_matching_color_ranges_file": contour_matching_color_ranges}
+        ]
+    )
+
+    object_tracking_map_node = launch_ros.actions.Node(
+        package="all_seaing_perception",
+        executable="object_tracking_map",
+        output="screen",
+        # arguments=['--ros-args', '--log-level', 'debug'],
+        remappings=[
+            ("camera_info_topic", "/wamv/sensors/cameras/front_left_camera_sensor/camera_info"),
+        ],
+        parameters=[
+            {"obstacle_seg_thresh": 10.0},
+            {"obstacle_drop_thresh": 2.0},
+            {"range_uncertainty": 1.0},
+            {"bearing_uncertainty": 0.1},
+            {"new_object_slam_threshold": 2.0},
+            {"init_new_cov": 10.0},
+            {"track_robot": False},
+        ]
+    )
+
     rviz_node = launch_ros.actions.Node(
         package="rviz2",
         executable="rviz2",
@@ -260,6 +305,8 @@ def launch_setup(context, *args, **kwargs):
         controller_server,
         obstacle_bbox_overlay_node,
         obstacle_bbox_visualizer_node,
+        bbox_project_pcloud_node,
+        object_tracking_map_node,
         color_segmentation_node,
         point_cloud_filter_node,
         obstacle_detector_node,

all_seaing_bringup/rviz/dashboard.rviz

@@ -5,9 +5,11 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /ObstacleMap1
+        - /PointCloud21
         - /PointCloud21/Topic1
-      Splitter Ratio: 0.48148149251937866
-    Tree Height: 922
+      Splitter Ratio: 0.5
+    Tree Height: 280
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -68,16 +70,16 @@ Visualization Manager:
       Enabled: true
       Name: ObstacleMap
       Namespaces:
-        labeled: true
-        labeled_edges: true
-        labeled_text: true
-        labeled_vertices: true
+        tracked: true
+        tracked_edges: true
+        tracked_text: true
+        tracked_vertices: true
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /obstacle_map/labeled
+        Value: /obstacle_map/refined_tracked
       Value: true
     - Alpha: 1
       Class: rviz_default_plugins/RobotModel
@@ -290,27 +292,6 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /clicked_point
       Value: true
-    - 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: /map
-      Update Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /map_updates
-      Use Timestamp: false
-      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -323,7 +304,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 0
@@ -333,18 +314,72 @@ Visualization Manager:
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Points
+      Size (m): 0.30000001192092896
+      Style: Flat Squares
       Topic:
         Depth: 5
         Durability Policy: Volatile
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Best Effort
-        Value: /point_cloud/filtered
+        Value: /refined_object_point_clouds_viz
       Use Fixed Frame: true
       Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /refined_object_contours_viz
       Value: false
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /wamv/sensors/cameras/front_left_camera_sensor/image_raw
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /refined_object_segments_viz
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        "": true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /obstacle_map/map_cov_viz
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -391,33 +426,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 72.52093505859375
+      Distance: 69.62364196777344
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.7055070400238037
-        Y: 29.966957092285156
-        Z: 8.167037010192871
+        X: 44.580535888671875
+        Y: 23.84090805053711
+        Z: 14.400632858276367
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7347967028617859
+      Pitch: 0.5253973603248596
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.648568630218506
+      Yaw: 4.643567085266113
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1137
+  Height: 934
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001a7000003d5fc0200000008fb000000100044006900730070006c006100790073010000003b000003d5000000c700fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000003d7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003d7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000c7fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075e00000040fc0100000002fb0000000800540069006d006501000000000000075e0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005b1000003d500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000161000002f7fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000470000015f000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000178000000690000002f00fffffffb0000000a0049006d00610067006501000001ad000000a60000002f00fffffffb0000000a0049006d006100670065010000025a000000e40000002f00ffffff0000000100000100000003d7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003d7000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000040fc0100000002fb0000000800540069006d00650100000000000007800000038100fffffffb0000000800540069006d0065010000000000000450000000000000000000000618000002f700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -426,6 +463,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1886
-  X: 15
-  Y: 44
+  Width: 1920
+  X: 0
+  Y: 34

all_seaing_interfaces/CMakeLists.txt

@@ -24,8 +24,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/WaypointArray.msg"
     "msg/ObstacleCounter.msg"
     "msg/ObstacleCounterArray.msg"
+    "msg/LabeledObjectPointCloud.msg"
+    "msg/LabeledObjectPointCloudArray.msg"
     DEPENDENCIES
     geometry_msgs
+    sensor_msgs
 )
 
 ament_auto_package()

all_seaing_interfaces/msg/LabeledObjectPointCloud.msg

@@ -0,0 +1,4 @@
+builtin_interfaces/Time time
+int8 label
+sensor_msgs/PointCloud2 cloud
+sensor_msgs/Image segment

all_seaing_interfaces/msg/LabeledObjectPointCloudArray.msg

@@ -0,0 +1 @@
+all_seaing_interfaces/LabeledObjectPointCloud[] objects

all_seaing_interfaces/msg/Obstacle.msg

@@ -13,4 +13,4 @@ geometry_msgs/Point bbox_min
 geometry_msgs/Point bbox_max
 
 geometry_msgs/Point global_bbox_min
-geometry_msgs/Point global_bbox_max
+geometry_msgs/Point global_bbox_max

all_seaing_perception/CMakeLists.txt

@@ -12,6 +12,10 @@ ament_auto_find_build_dependencies()
 
 find_package(yaml-cpp REQUIRED)
 
+find_package(PCL 1.12 REQUIRED COMPONENTS common visualization io)
+
+find_package(Eigen3 3.4 REQUIRED)
+
 ament_auto_add_executable(
     obstacle_detector
     ${PROJECT_NAME}/obstacle_detector.cpp
@@ -33,15 +37,28 @@ ament_auto_add_executable(
     obstacle_bbox_visualizer
     ${PROJECT_NAME}/obstacle_bbox_visualizer.cpp
 )
+ament_auto_add_executable(
+    bbox_project_pcloud
+    ${PROJECT_NAME}/bbox_project_pcloud.cpp
+)
+ament_auto_add_executable(
+    object_tracking_map
+    ${PROJECT_NAME}/object_tracking_map.cpp
+    ${PROJECT_NAME}/obstacle.cpp
+)
 
 target_link_libraries(obstacle_bbox_visualizer yaml-cpp)
+target_link_libraries(bbox_project_pcloud yaml-cpp ${PCL_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_VISUALIZATION_LIBRARIES})
+target_link_libraries(object_tracking_map yaml-cpp Eigen3::Eigen ${PCL_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_VISUALIZATION_LIBRARIES})
 
 install(TARGETS
     obstacle_detector
     obstacle_bbox_overlay
     point_cloud_filter
     point_cloud_image_overlay
-    obstacle_bbox_visualizer  
+    obstacle_bbox_visualizer
+    bbox_project_pcloud
+    object_tracking_map
     DESTINATION lib/${PROJECT_NAME}
 )