PR #347 merged, SLAM finalization ROS2 RTAB-MAP

CMakeLists.txt

@@ -978,3 +978,15 @@ if(${ENABLE_EMULATOR} OR ENABLE_EMULATOR EQUAL ON)
         tools/test_server/launch
         DESTINATION share/${PROJECT_NAME})
 endif()
+
+#
+# rtabmap support utilities
+#
+
+if(${ENABLE_EMULATOR} OR ENABLE_EMULATOR EQUAL ON)
+    if(NOT WIN32 AND ROS_VERSION EQUAL 1)
+        add_executable(pose2d_to_odom_converter test/src/pose2d_to_odom_converter.cpp)
+        target_link_libraries(pose2d_to_odom_converter ${PROJECT_NAME}_lib ${SICK_LDMRS_LIBRARIES} ${catkin_LIBRARIES})
+        install(TARGETS pose2d_to_odom_converter DESTINATION lib/${PROJECT_NAME})
+    endif()    
+endif()

FAQ.md

@@ -44,7 +44,7 @@ To run two picoScan devices simultanously, each sick_scan_xd node must be config
 * "imu_topic": e.g. "imu0" and "imu1"
 * "udp_port": e.g. "56661" and "56662"
 * "imu_udp_port": e.g. "7503" and "7504"
-* individual topics and frame id for each customized point cloud, e.g.
+* individual topics and frame ids for each customized point cloud, e.g.
     * replace all "topic=/cloud_" by "topic=/cloud0_" resp. "topic=/cloud1_"
     * replace all "frameid=world" by "frameid=world0" resp. "frameid=world1"
 It is recommend to first verify the launchfile configurations separately for each picoScan before running them simultanously.

launch/sick_multiscan_rtabmap.launch.py

@@ -0,0 +1,147 @@
+import os
+import sys
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+def generate_launch_description():
+
+    ld = LaunchDescription()
+    sick_scan_pkg_prefix = get_package_share_directory('sick_scan_xd')
+    sick_launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/sick_multiscan_rtabmap.launch')
+    sick_node_arguments=[sick_launch_file_path]
+
+    # Append optional commandline arguments in name:=value syntax
+    for arg in sys.argv:
+        if len(arg.split(":=")) == 2:
+            sick_node_arguments.append(arg)
+    # Create sick_scan_xd node
+    sick_node = Node(
+        package='sick_scan_xd',
+        executable='sick_generic_caller',
+        output='screen',
+        arguments=sick_node_arguments
+    )
+
+    # Common parameter
+    wait_for_transform = 0.01
+    point_cloud_topic = "/cloud_all_fields_fullframe" # Point cloud topic published by sick_scan_xd
+    point_cloud_frame_id = "cloud"                    # Point cloud frame id published by sick_scan_xd
+    imu_topic = "/multiScan/imu"                      # IMU topic published by sick_scan_xd
+    deskewing = True                                  # Optional lidar deskewing
+
+    if deskewing: # Create deskewing node
+        point_cloud_topic_desk = f"{point_cloud_topic}/deskewed"
+        deskewing_node = Node(
+            package='rtabmap_util', executable='lidar_deskewing', name="lidar_deskewing", output="screen",
+            parameters=[{
+                "wait_for_transform": wait_for_transform,
+                "fixed_frame_id": point_cloud_frame_id,
+                "slerp": False,
+             }],
+            remappings=[("input_cloud", point_cloud_topic)],
+            arguments=[],
+            namespace="deskewing")
+    else:
+        point_cloud_topic_desk = point_cloud_topic
+        deskewing_node = None
+
+    # Create rtabmap_odom node
+    rtabmap_odom_node = Node(
+            package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
+            parameters=[{
+                "frame_id": point_cloud_frame_id,
+                "odom_frame_id": "odom",
+                "guess_frame_id": point_cloud_frame_id,
+                "wait_imu_to_init": True,
+                "wait_for_transform_duration": str(wait_for_transform),
+                "Icp/PointToPlane": "True",
+                "Icp/Iterations": "10",
+                "Icp/VoxelSize": "0.2",
+                "Icp/DownsamplingStep": "1",
+                "Icp/Epsilon": "0.001",
+                "Icp/PointToPlaneK": "20",
+                "Icp/PointToPlaneRadius": "0",
+                "Icp/MaxTranslation": "2",
+                "Icp/MaxCorrespondenceDistance": "1",
+                "Icp/PM": "True",
+                "Icp/PMOutlierRatio": "0.1",
+                "Icp/CorrespondenceRatio": "0.01",
+                "Icp/ReciprocalCorrespondences": "False",
+                "Odom/ScanKeyFrameThr": "0.8",
+                "Odom/Strategy": "0",
+                "OdomF2M/ScanSubtractRadius": "0.2",
+                "OdomF2M/ScanMaxSize": "15000",
+            }],
+            remappings=[("scan_cloud", point_cloud_topic_desk), ("imu", imu_topic)],
+            arguments=[],
+            namespace="rtabmap")
+
+
+    # Create rtabmap_slam node, see https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_launch/launch/rtabmap.launch.py for an example configuration
+    rtabmap_slam_node = Node(
+            package='rtabmap_slam', executable='rtabmap', name="rtabmap", output="screen",
+            parameters=[{
+                "frame_id": point_cloud_frame_id,
+                "subscribe_depth": False,
+                "subscribe_rgb": False,
+                "subscribe_rgbd": False,
+                "subscribe_scan": False,
+                "subscribe_scan_cloud": True,
+                "approx_sync": True,
+                "Rtabmap/DetectionRate": "1",
+                "RGBD/NeighborLinkRefining": "False",
+                "RGBD/ProximityBySpace": "True",
+                "RGBD/ProximityMaxGraphDepth": "0",
+                "RGBD/ProximityPathMaxNeighbors": "1",
+                "RGBD/AngularUpdate": "0.05",
+                "RGBD/LinearUpdate": "0.05",
+                "Mem/NotLinkedNodesKept": "False",
+                "Mem/STMSize": "30",
+                "Reg/Strategy": "1",
+                "Grid/CellSize": "0.1",
+                "Grid/RangeMax": "20",
+                "Grid/ClusterRadius": "1",
+                "Grid/GroundIsObstacle": "True",
+                "Optimizer/GravitySigma": "0.3",
+                "Icp/VoxelSize": "0.3",
+                "Icp/PointToPlaneK": "20",
+                "Icp/PointToPlaneRadius": "0",
+                "Icp/PointToPlane": "False",
+                "Icp/Iterations": "10",
+                "Icp/Epsilon": "0.001",
+                "Icp/MaxTranslation": "3",
+                "Icp/MaxCorrespondenceDistance": "1",
+                "Icp/PM": "True",
+                "Icp/PMOutlierRatio": "0.7",
+                "Icp/CorrespondenceRatio": "0.4",
+            }],
+            remappings=[("scan_cloud", point_cloud_topic_desk), ("imu", imu_topic)],
+            arguments=["-d"], # parameter "-d": delete_db_on_start
+            namespace="rtabmap")
+
+    # Create rtabmap_viz node
+    rtabmap_viz_node = Node(
+            package='rtabmap_viz', executable='rtabmap_viz', name="rtabmap_viz", output="screen",
+            parameters=[{
+                "frame_id": point_cloud_frame_id,
+                "odom_frame_id": "odom",
+                "subscribe_odom_info": False,
+                "subscribe_scan_cloud": True,
+                "approx_sync": False,
+             }],
+            remappings=[("scan_cloud", point_cloud_topic_desk)],
+            arguments=[],
+            namespace="rtabmap")
+
+    ld.add_action(sick_node)
+    if deskewing:
+        ld.add_action(deskewing_node)
+    ld.add_action(rtabmap_odom_node)
+    ld.add_action(rtabmap_slam_node)
+    ld.add_action(rtabmap_viz_node)
+    return ld
+

roswrap/src/launchparser/launchparser.cpp

@@ -240,8 +240,34 @@ bool LaunchParser::parseFile(std::string launchFileFullName, std::vector<std::st
       }
       arg_node = (TiXmlElement *)arg_node->NextSibling();  // go to next sibling
     }
+
+    // Parse optional group for param "scanner_type"
+    bool node_with_scanner_type_found = false;
+    TiXmlNode * group_node = node->FirstChild("group");
+    if (group_node)
+    {
+      group_node = group_node->FirstChild("node");
+      if (group_node)
+      {
+        std::vector<paramEntryAscii> paramOrgList = getParamList(group_node);
+        for (size_t j = 0; j < paramOrgList.size(); j++)
+        {
+          if (paramOrgList[j].getName() == "scanner_type")
+          {
+            node_with_scanner_type_found = true;
+            ROS_INFO_STREAM("LaunchParser::parseFile(" << launchFileFullName << "): group node found with param scanner_type");
+            node = group_node;
+            break;
+          }
+        }
+      }
+    }
+    if (!node_with_scanner_type_found)
+    {
+      node = node->FirstChild("node");
+    }
+
     // parse all node specific parameters
-    node = node->FirstChild("node");
     std::vector<paramEntryAscii> paramOrgList = getParamList(node);
 
     for (size_t j = 0; j < paramOrgList.size(); j++)

test/scripts/run_linux_ros2_simu_multiscan_slam3d_rtabmap.bash

@@ -0,0 +1,133 @@
+#!/bin/bash
+
+# killall and cleanup after exit
+function killall_cleanup()
+{
+  killall sick_generic_caller
+  killall rviz2 rtabmap icp_odometry rtabmap_viz
+  sleep 3
+  pkill -f multiscan_sopas_test_server.py
+  pkill -f multiscan_pcap_player.py
+  sleep 3
+  killall -9 rviz2 rtabmap icp_odometry rtabmap_viz
+  killall -9 sick_generic_caller
+}
+
+# Wait for a given amount of time in seconds, or until 'q' or 'Q' was pressed, or until rviz is closed
+function waitUntilRvizClosed()
+{
+    duration_sec=$1
+    sleep 1
+    for ((cnt=1;cnt<=$duration_sec;cnt++)) ; do  
+        echo -e "sick_scan_xd running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key
+        if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi
+        rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)`
+        if [ $rviz_running -lt 1 ] ; then break ; fi
+    done
+}
+
+# 
+# Build rtabmap and sick_scan_xd
+#
+# rtabmap can be installed using a binary distribution:
+# sudo apt install ros-$ROS_DISTRO-rtabmap-ros
+# 
+
+pushd ../../../..
+source /opt/ros/humble/setup.bash
+killall_cleanup
+printf "\033c"
+
+REBUILD_ALL=0
+BUILD_UPDATE=0
+if [ ! $REBUILD_ALL -eq 0 ] ; then
+  sudo apt-get install libboost-all-dev
+  sudo apt-get install libeigen3-dev
+  sudo apt-get install libsdl-image1.2-dev
+  sudo apt-get install libsdl1.2-dev
+  sudo apt-get install ros-humble-nav-msgs
+  sudo apt-get install ros-humble-tf2-sensor-msgs
+  sudo apt-get install ros-humble-imu-filter-madgwick
+  sudo apt-get install python3-wstool
+  sudo apt-get install ros-humble-scan-tools
+  sudo apt install ros-humble-pcl-ros
+  pushd /tmp
+  git clone https://github.com/introlab/rtabmap.git rtabmap
+  git clone https://github.com/ethz-asl/libnabo.git libnabo
+  git clone https://github.com/ethz-asl/libpointmatcher.git libpointmatcher  
+  mkdir -p libnabo/build && pushd libnabo/build
+  cmake ..
+  cmake --build .
+  sudo cmake --build . --target install
+  popd  
+  mkdir -p libpointmatcher/build && pushd libpointmatcher/build
+  cmake ..
+  make -j4
+  sudo make install
+  popd
+  sudo ldconfig    
+  mkdir -p rtabmap/build && pushd rtabmap/build
+  cmake ..
+  make -j4
+  sudo make install
+  popd  
+  sudo ldconfig
+  popd
+  pushd src
+  git clone --branch ros2 https://github.com/introlab/rtabmap_ros.git rtabmap_ros
+  if [ ! -d ./sick_scan_xd ] ; then git clone https://github.com/SICKAG/sick_scan_xd.git ; fi
+  popd
+  rosdep update && rosdep install --from-paths src --ignore-src -r -y
+  rm -rf ./build ./devel ./install ./build_isolated ./devel_isolated ./install_isolated ./log
+  colcon build --symlink-install --cmake-args " -DROS_VERSION=2" " -DCMAKE_ENABLE_EMULATOR=1" "-DCMAKE_BUILD_TYPE=Release" --event-handlers console_direct+
+  sudo ldconfig
+fi
+if [ ! $BUILD_UPDATE -eq 0 ] ; then
+  colcon build --symlink-install --cmake-args " -DROS_VERSION=2" " -DCMAKE_ENABLE_EMULATOR=1" "-DCMAKE_BUILD_TYPE=Release" --event-handlers console_direct+
+  sudo ldconfig
+fi
+
+# 
+# Run sick_scan_xd multiScan simulation with rtabmap
+# Note: run "ros2 run rtabmap_slam rtabmap --params" to see all rtabmap parameters and their meaning
+# 
+
+source ./install/setup.bash
+killall_cleanup
+cp -f ./src/sick_scan_xd/launch/sick_multiscan*.launch* ./install/sick_scan_xd/share/sick_scan_xd/launch/ # optional update in case of launchfile changes after the last build
+rm -f ~/.ros/rtabmap.db
+sleep 1 ; python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 &
+
+# rviz: Add topics "/cloud_unstructured_fullframe", "/rtabmap/grid_map", "/rtabmap/localization_pose" and "/rtabmap/odom" to view the navigational map and localization pose
+sleep 1 ; ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_rtabmap.rviz &
+
+sleep 1 ; ros2 launch sick_scan_xd sick_multiscan_rtabmap.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" &
+
+# sleep 3 ; python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20240604_multiscan_01_motionless.pcapng --udp_port=-1 --repeat=10 --verbose=0 --send_rate=100 --filter=pcap_filter_multiscan_hildesheim &
+sleep 3 ; python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20240604_multiscan_02_moving_planar_defaultsettings.pcapng --udp_port=-1 --repeat=10 --verbose=0 --send_rate=0 --filter=pcap_filter_multiscan_hildesheim &
+# sleep 3 ; python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20240604_multiscan_03_moving_planar_anglefilter_-45_45deg.pcapng --udp_port=-1 --repeat=10 --verbose=0 --send_rate=100 --filter=pcap_filter_multiscan_hildesheim &
+# Settings in sick_multiscan.launch for 20240604_multiscan_03_moving_planar_anglefilter_-45_45deg.pcapng with +/-45 deg angle filter:
+# <arg name="host_LFPangleRangeFilter" default="1 -45.0 +45.0 -90.0 +90.0 1" />
+# <arg name="host_set_LFPangleRangeFilter" default="True" />
+# Default settings in sick_multiscan.launch for all other pcang-files:
+# <arg name="host_LFPangleRangeFilter" default="0 -180.0 +179.0 -90.0 +90.0 1" />
+# <arg name="host_set_LFPangleRangeFilter" default="False" />
+
+#
+# Switch to mapping and/or localization mode
+#
+
+sleep  3 ; ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN IsSystemReady'}" # query sensor state
+sleep  1 ; ros2 service call /rtabmap/resume std_srvs/srv/Empty                # resume if previously paused
+sleep  1 ; ros2 service call /rtabmap/trigger_new_map std_srvs/srv/Empty       # start a new map
+sleep  1 ; ros2 service call /rtabmap/set_mode_mapping std_srvs/srv/Empty      # set mapping mode
+sleep 60 ; ros2 service call /rtabmap/set_mode_localization std_srvs/srv/Empty # set localization mode
+
+#
+# Shutdown after 240 seconds
+#
+
+waitUntilRvizClosed 240
+echo -e "rtabmap simulation finished, killing all processes ..."
+killall_cleanup
+popd