From be145af84ebbb1a8646027b01f9213c7fb4f7d0d Mon Sep 17 00:00:00 2001 From: Michael Lehning Date: Tue, 18 Jun 2024 20:09:15 +0200 Subject: [PATCH] PR #347 merged, SLAM finalization ROS2 RTAB-MAP --- CMakeLists.txt | 12 ++ FAQ.md | 2 +- launch/sick_multiscan_rtabmap.launch.py | 147 ++++++++++++++++++ roswrap/src/launchparser/launchparser.cpp | 28 +++- ...ux_ros2_simu_multiscan_slam3d_rtabmap.bash | 133 ++++++++++++++++ 5 files changed, 320 insertions(+), 2 deletions(-) create mode 100644 launch/sick_multiscan_rtabmap.launch.py create mode 100644 test/scripts/run_linux_ros2_simu_multiscan_slam3d_rtabmap.bash diff --git a/CMakeLists.txt b/CMakeLists.txt index c28a5820..bc23b27c 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/FAQ.md b/FAQ.md index 00f2e84b..d8e55d07 100644 --- a/FAQ.md +++ b/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. diff --git a/launch/sick_multiscan_rtabmap.launch.py b/launch/sick_multiscan_rtabmap.launch.py new file mode 100644 index 00000000..4833f34c --- /dev/null +++ b/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 + diff --git a/roswrap/src/launchparser/launchparser.cpp b/roswrap/src/launchparser/launchparser.cpp index a3ceff5b..1126d5c4 100644 --- a/roswrap/src/launchparser/launchparser.cpp +++ b/roswrap/src/launchparser/launchparser.cpp @@ -240,8 +240,34 @@ bool LaunchParser::parseFile(std::string launchFileFullName, std::vectorNextSibling(); // 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 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 paramOrgList = getParamList(node); for (size_t j = 0; j < paramOrgList.size(); j++) diff --git a/test/scripts/run_linux_ros2_simu_multiscan_slam3d_rtabmap.bash b/test/scripts/run_linux_ros2_simu_multiscan_slam3d_rtabmap.bash new file mode 100644 index 00000000..68bedb7b --- /dev/null +++ b/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: +# +# +# Default settings in sick_multiscan.launch for all other pcang-files: +# +# + +# +# 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