Untracked detection & tracked obstacle map publishing, integrating updated sensor fusion algorithm with the ObstacleMap interface

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")
@@ -162,6 +167,42 @@ 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=[
+            {"global_frame_id": "odom"},
+            {"obstacle_seg_thresh": 10.0},
+            {"obstacle_drop_thresh": 1.0},
+        ]
+    )
+
     rviz_node = launch_ros.actions.Node(
         package="rviz2",
         executable="rviz2",
@@ -274,6 +315,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,
         obstacle_detector_node,
         color_segmentation_node,
         point_cloud_filter_node,

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

@@ -12,4 +12,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,8 @@ ament_auto_find_build_dependencies()
 
 find_package(yaml-cpp REQUIRED)
 
+find_package(PCL 1.12 REQUIRED COMPONENTS common visualization io)
+
 ament_auto_add_executable(
     obstacle_detector
     ${PROJECT_NAME}/obstacle_detector.cpp
@@ -33,15 +35,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 ${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}
 )