Rtabmap updates

Dockerfile

@@ -18,7 +18,7 @@ COPY ./ .$WS/src/rae-ros
 
 RUN rm -rf .$WS/src/rae-ros/rae_gazebo
 
-RUN cd  .$WS/ && apt update && rosdep update && rosdep install --from-paths src --ignore-src  -y --skip-keys depthai --skip-keys depthai_bridge
+RUN cd  .$WS/ && apt update && rosdep update && rosdep install --from-paths src --ignore-src  -y --skip-keys depthai --skip-keys depthai_bridge --skip-keys depthai_ros_driver
 
 RUN cd .$WS/ && . /opt/ros/${ROS_DISTRO}/setup.sh && . $DEPTHAI_WS/install/setup.sh && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=${BUILD_TYPE}
 

laserscan_kinect/CMakeLists.txt

@@ -1,54 +1,78 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.22)
 project(laserscan_kinect)
 
-add_compile_options(-Wall -Wextra -Wpedantic)
+# Default to C99
+set(CMAKE_C_STANDARD 99)
 
+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()
 ### Dependencies
-find_package(ament_cmake_auto REQUIRED)
+find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(image_geometry REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(cv_bridge REQUIRED)
+find_package(rclcpp_components REQUIRED)
 
-ament_auto_find_build_dependencies()
+set(DEPENDENCIES
+rclcpp
+sensor_msgs
+image_geometry
+image_transport
+cv_bridge
+rclcpp_components)
+
+include_directories(include)
 
 ### Build
-ament_auto_add_library(${PROJECT_NAME} SHARED
+add_library(${PROJECT_NAME} SHARED
   src/laserscan_kinect.cpp
   src/laserscan_kinect_node.cpp
 )
 
-ament_auto_add_executable(${PROJECT_NAME}_exe
-  src/main.cpp
+ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES})
+rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::LaserScanKinectNode")
+
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-target_link_libraries(${PROJECT_NAME}_exe ${PROJECT_NAME})
+ament_export_include_directories(include)
 
-# Unit tests
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-  find_package(ament_cmake_gtest REQUIRED)
+ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies})
 
-  ament_add_gtest(${PROJECT_NAME}_test
-    test/laserscan_kinect_test.cpp
-  )
-  target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME})
-endif()
 
-# Export package
-target_include_directories(${PROJECT_NAME}
-  PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
+install(TARGETS ${PROJECT_NAME}
+EXPORT ${PROJECT_NAME}Targets
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+        INCLUDES DESTINATION include
 )
 
-ament_export_dependencies(${PROJECT_NAME} PUBLIC image_geometry image_transport sensor_msgs cv_bridge)
-rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::LaserScanKinectNode")
+install(EXPORT ${PROJECT_NAME}Targets
+      DESTINATION share/${PROJECT_NAME}/cmake)
+
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+ament_export_libraries(${PROJECT_NAME})
 
-### Install
-ament_auto_package(INSTALL_TO_SHARE
-  launch
-  config
+install(
+    DIRECTORY include/
+    DESTINATION include
 )
+ament_package()

laserscan_kinect/config/params.yaml

@@ -3,17 +3,17 @@ laserscan_kinect_front:
     # Frame id for the output laserscan.
     output_frame_id: base_link
     # Minimum sensor range (m).
-    range_min: 0.4
+    range_min: 0.3
     # Maximum sensor range (m).
     range_max: 10.0
     # Height of used part of depth img (px).
-    scan_height: 60
+    scan_height: 180
     # Row step in depth image processing (px).
-    depth_img_row_step: 10
+    depth_img_row_step: 20
     # If continously camera data update.
     cam_model_update: false
     # Offset in Y axis
-    vertical_offset: 310
+    vertical_offset: 180
 
     # Height of sensor optical center mount (m).
     sensor_mount_height: 0.8
@@ -33,17 +33,17 @@ laserscan_kinect_back:
     # Frame id for the output laserscan.
     output_frame_id: base_link_back
     # Minimum sensor range (m).
-    range_min: 0.4
+    range_min: 0.3
     # Maximum sensor range (m).
     range_max: 10.0
     # Height of used part of depth img (px).
-    scan_height: 60
+    scan_height: 180
     # Row step in depth image processing (px).
     depth_img_row_step: 10
     # If continously camera data update.
     cam_model_update: false
     # Offset in Y axis
-    vertical_offset: 310
+    vertical_offset: 180
 
     # Height of sensor optical center mount (m).
     sensor_mount_height: 0.8
@@ -57,4 +57,4 @@ laserscan_kinect_back:
     tilt_compensation_en: true
 
     publish_dbg_info: true
-    threads_num: 1
+    threads_num: 1

rae_bringup/config/slam_param.yaml

@@ -421,22 +421,4 @@ slam_toolbox:
     minimum_distance_penalty: 0.5
     use_response_expansion: true
 
-depth_sensor_pose:
-  ros__parameters:
-    range_min: 0.5
-    range_max: 8.0
-    mount_height_min: 0.7
-    mount_height_max: 1.0
-    tilt_angle_min: 5.0
-    tilt_angle_max: 35.0
-
-    cam_model_update: false
-    used_depth_height: 240
-    depth_img_step_row: 8
-    depth_img_step_col: 8
-
-    ground_max_points: 2500
-    ransac_max_iter: 2000
-    ransac_dist_thresh: 0.001
 
-    publish_dbg_info: true

rae_bringup/launch/bringup.launch.py

@@ -1,7 +1,7 @@
 import os
 from ament_index_python.packages import get_package_share_path
 import launch
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch.substitutions import LaunchConfiguration
@@ -36,12 +36,16 @@ def generate_launch_description():
             os.path.join(bridge_prefix, 'launch', 'rosbridge_websocket_launch.xml'),
             condition=IfCondition(enable_rosbridge)
             ),
+        TimerAction(
+            period=50.0,
+            actions=[
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(nav_prefix, 'navigation_launch.py')),
             launch_arguments={
                               'use_sim_time': 'false',
                               'params_file': params,
+                              'use_composition': 'True',
                               'container_name': 'rae_container'}.items(),
             condition=IfCondition(enable_nav)
-            ),
+            )]),
             ])

rae_bringup/launch/rtabmap.launch.py

@@ -16,21 +16,45 @@ def launch_setup(context, *args, **kwargs):
     params_file = LaunchConfiguration("params_file")
     depthai_prefix = get_package_share_directory("depthai_ros_driver")
     name = LaunchConfiguration('name').perform(context)
-
+    laserscan_config = os.path.join(
+        get_package_share_directory('laserscan_kinect'),
+        'config',
+        'params.yaml'
+    )
     parameters = [
         {
             "frame_id": "base_footprint",
             "subscribe_rgb": True,
             "subscribe_depth": True,
+            "subscribe_scan": True,
             "subscribe_odom_info": False,
             "approx_sync": True,
-            "Rtabmap/DetectionRate": "3.5",
+            "Grid/MaxGroundHeight": "0.1",
+            "Grid/FromDepth": True,
+            "Grid/RangeMin": "0.4",
+            "Grid/RangeMax": "8.0",
+            "Kp/RoiRatio": "0 0 0 0.3",
+            # "Grid/3D": "false",
+            "Grid/MaxGroundAngle": "60.0",
+            "Grid/FootprintHeight": "0.1",
+            "Reg/Force3DoF": "true",
+            "Optimizer/Slam2D": True,
+            "Rtabmap/DetectionRate": "1.0",
+            "Grid/RayTracing": "true",
+            'RGBD/NeighborLinkRefining':'True',
+            "RGBD/LocalLoopDetectionTime": "false",
+            "RGBD/OptimizeFromGraphEnd": "false",
+            "RGBD/AngularUpdate": "0.01",
+            "RGBD/LinearUpdate": "0.01",
+#            "Reg/Strategy": "1",
+            "qos_scan": 2
         }
     ]
 
     remappings = [
         #   ('imu', '/imu/data'),
-        ("rgb/image", name+"/right_front/image_rect"),
+        ('odom', '/diff_controller/odom'),
+        ("rgb/image", name+"/right_front/image_raw"),
         ("rgb/camera_info", name+"/right_front/camera_info"),
         ("depth/image", name+"/stereo_front/image_raw"),
     ]
@@ -48,20 +72,56 @@ def launch_setup(context, *args, **kwargs):
                     remappings=[('image', name+'/right_front/image_raw'),
                                 ('camera_info', name+'/right_front/camera_info'),
                                 ('image_rect', name+'/right_front/image_rect'),]
-                )
+                ),
+                ComposableNode(
+                    package="rtabmap_slam",
+                    plugin="rtabmap_slam::CoreWrapper",
+                    parameters=parameters,
+                    remappings=remappings,
+                    )
             ]),
-        Node(
-            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
-            parameters=parameters,
-            remappings=remappings,
-            arguments=['-d']),
-        Node(
-            package='rtabmap_slam', executable='rtabmap', output='screen',
-            parameters=parameters,
-            remappings=remappings,
-            arguments=['-d']),
 
-    ]
+        LoadComposableNodes(
+            target_container=name+"_container",
+            composable_node_descriptions=[
+                    ComposableNode(
+                        package='laserscan_kinect',
+                        plugin='laserscan_kinect::LaserScanKinectNode',
+                        name='laserscan_kinect_front',
+                        parameters=[laserscan_config],
+                        remappings=[
+                            ('/image', name+'/stereo_front/image_raw'),
+                            ('/camera_info', name+'/stereo_front/camera_info'),
+                            ('/scan', name+'/scan_front'),
+                            ('/debug_image', name+'/debug_image_front'),
+                            ('/debug_image/compressed', name+'/debug_image_front/compressed')
+                        ]
+                    ),
+                    ComposableNode(
+                        package='laserscan_kinect',
+                        plugin='laserscan_kinect::LaserScanKinectNode',
+                        name='laserscan_kinect_back',
+                        parameters=[laserscan_config],
+                        remappings=[
+                            ('/image', name+'/stereo_back/image_raw'),
+                            ('/camera_info', name+'/stereo_back/camera_info'),
+                            ('/scan', name+'/scan_back'),
+                            ('/debug_image', name+'/debug_image_back'),
+                            ('/debug_image/compressed', name+'/debug_image_back/compressed')
+                        ]
+                    ),
+                    ComposableNode(
+                        package="ira_laser_tools",
+                        name="laser_scan_multi_merger",
+                        plugin="ira_laser_tools::LaserscanMerger",
+                        parameters=[{'laserscan_topics': '/rae/scan_back /rae/scan_front',
+                                    'destination_frame': 'base_link',
+                                    'scan_destination_topic': '/scan'}
+                                    ]
+        ),
+            ]
+        )  
+        ]
 
 
 def generate_launch_description():