ROS-2 Humble support

CMakeLists.txt

@@ -5,13 +5,25 @@ project(sick_lidar_localization)
 ## Compile options ##
 #####################
 
+if(ROS_VERSION EQUAL 2)
+    set(ROS2_HUMBLE 1)
+    if(WIN32 OR EXISTS "/opt/ros/eloquent" OR EXISTS "/opt/ros/foxy" OR EXISTS "/opt/ros/galactic") # rosidl_typesupport for ROS2 eloquent, foxy, galaxy # use $ENV{ROS_DISTRO} instead
+        set(ROS2_HUMBLE 0)
+    endif()
+endif()
+
 ## Compile as C++14
 set(CMAKE_CXX_STANDARD 14)
 if(WIN32)
     add_compile_options(-D_CRT_SECURE_NO_WARNINGS)
     set(WIN_LIBRARIES Ws2_32)
 else()
-    add_compile_options(-std=c++14 -g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs -Wno-unused-parameter -Wno-unused-function -Wno-unused-result)
+    if(ROS2_HUMBLE) # ROS2 humble or later require C++17
+        add_compile_options(-std=c++17)
+    else()
+        add_compile_options(-std=c++14)
+    endif()
+    add_compile_options(-g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs -Wno-unused-parameter -Wno-unused-function -Wno-unused-result)
     # add_compile_options(-Wshadow) # Note: compiler option -Wshadow generates a lot of warnings in ros header files. Therefore it's deactivated by default, but can be usefull for development and testing.
     set(LINUX_LIBRARIES pthread)    # gcc maps std::thread to pthread, using std::thread requires linking with pthread
 endif()
@@ -41,7 +53,8 @@ elseif(ROS_VERSION EQUAL 2)
     find_package(sensor_msgs REQUIRED)
     find_package(std_msgs REQUIRED)
     find_package(tf2 REQUIRED)  
-    find_package(tf2_ros REQUIRED)  
+    find_package(tf2_ros REQUIRED)
+    find_package(visualization_msgs REQUIRED)
 endif() 
 
 ## packages required by sick_lidar_localization
@@ -112,10 +125,16 @@ if(ROS_VERSION EQUAL 1)
     add_dependencies(sick_lidar_localization_main sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
     add_dependencies(pointcloud_converter sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 elseif(ROS_VERSION EQUAL 2)
-    ament_target_dependencies(sick_localization_lib rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros)
-    rosidl_target_interfaces(sick_localization_lib ${PROJECT_NAME} "rosidl_typesupport_cpp")
-    ament_target_dependencies(pointcloud_converter rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros)
-    rosidl_target_interfaces(pointcloud_converter ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    ament_target_dependencies(sick_localization_lib rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros visualization_msgs)
+    ament_target_dependencies(pointcloud_converter rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros visualization_msgs)
+    if(ROS2_HUMBLE) # rosidl_typesupport for ROS2 humble or later
+        rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+        target_link_libraries(sick_localization_lib "${cpp_typesupport_target}")
+        target_link_libraries(pointcloud_converter "${cpp_typesupport_target}")
+    else()
+        rosidl_target_interfaces(sick_localization_lib ${PROJECT_NAME} "rosidl_typesupport_cpp")
+        rosidl_target_interfaces(pointcloud_converter ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    endif()
 else()
     add_dependencies(sick_lidar_localization_main sick_localization_lib)
 endif()     

README.md

@@ -343,24 +343,36 @@ pip install scapy
 pip install pypcapfile
 pip install python-pcapng
 ```
+If you're still getting `ModuleNotFoundError` messages, try `sudo pip install` resp. `sudo pip3 install` instead of `pip install`.
 
 :question: UDP messages have CRC checksum error in wireshark or tcpdump
 
 :white_check_mark: Excerpt from https://docs.gz.ro/tuning-network-cards-on-linux.html : "If you have offload features enabled and you see chksum incorrect in tcpdump output, without any packet errors and your network is working properly: it is nothing to worry about because the checksum is actually calculated on the network adapter and the tcpdump is showing the checksum calculated on kernel level."
 
 :question: Error MSB3491 on Windows: "Could not write lines to file. The fully qualified file name must be less than 260 characters"
 
-:white_check_mark: Possible solutions are f.e.
+:white_check_mark: Possible solutions are e.g.:
 * Just clone repository https://github.com/SICKAG/sick_lidar_localization into a short path, f.e. directly to `C:\`: 
     ```
     cd c:\
     git clone https://github.com/SICKAG/sick_lidar_localization.git
     ```
-* Use subst to shorten long paths, f.e.:
+    This is the recommended solution. Working on shared folder might cause errors using ROS-2 on Windows.
+* Alternatively, use subst to shorten long paths, f.e.:
     ```
     subst <long_path_to_sick_lidar_localization> s:
     cd /d s:\
     ```
-* See https://docs.ros.org/en/foxy/Guides/Installation-Troubleshooting.html:
+* Alternatively, See https://docs.ros.org/en/foxy/Guides/Installation-Troubleshooting.html:
     * Run regedit.exe, navigate to Computer\HKEY_LOCAL_MACHINE\SYSTEM\CurrentControlSet\Control\FileSystem, and set LongPathsEnabled to 0x00000001 (1).
-    * Hit the windows key and type Edit Group Policy. Navigate to Local Computer Policy > Computer Configuration > Administrative Templates > System > Filesystem. Right click Enable Win32 long paths, click Edit. In the dialog, select Enabled and click OK.
+    * Hit the windows key and type Edit Group Policy (gpedit.msc). Navigate to Local Computer Policy > Computer Configuration > Administrative Templates > System > Filesystem. Right click Enable Win32 long paths, click Edit. In the dialog, select Enabled and click OK.
+
+:question: `rivz2` does not work on Windows
+
+:white_check_mark: Start rviz2 with 
+    ```
+    call C:\dev\ros2_foxy\local_setup.bat
+    call c:\opt\ros\foxy\x64\setup.bat
+    call .\install\setup.bat
+    ros2 run rviz2 rviz2
+    ```

package_ros2.xml

@@ -31,6 +31,7 @@
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>visualization_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
 

src/udp_message_parser.cpp

@@ -739,10 +739,10 @@ static std::string printHex(const std::vector<uint8_t>& data)
 template<class T> static void decodeAndPrint(std::vector<uint8_t>& reference_bin)
 {
     sick_lidar_localization::UDPMessage::HeaderData msg_header = { 0 };
-    sick_lidar_localization::UDPMessage::decodeHeader(&reference_bin[0], reference_bin.size(), msg_header);
+    sick_lidar_localization::UDPMessage::decodeHeader(&reference_bin[0], (int)reference_bin.size(), msg_header);
     bool payload_is_big_endian = (msg_header.payloadtype == 0x0642);
     uint8_t* udp_payload_buffer = &reference_bin[0] + sizeof(sick_lidar_localization::UDPMessage::HeaderData);
-    int udp_payload_len = reference_bin.size() - sizeof(sick_lidar_localization::UDPMessage::HeaderData);
+    int udp_payload_len = (int)reference_bin.size() - (int)sizeof(sick_lidar_localization::UDPMessage::HeaderData);
     sick_lidar_localization::UDPMessage::Payload<T> reference_payload;
     reference_payload.decodePayload(udp_payload_buffer, udp_payload_len, payload_is_big_endian);
     ROS_INFO_STREAM("UDPMessage::unittest: payload_reference = " << reference_payload.toString(false) << ", payload_big_endian = " << payload_is_big_endian << ", source_id = " << msg_header.sourceid);

test/scripts/make_ros1.bash

@@ -1,6 +1,6 @@
 #!/bin/bash
 pushd ../../../..
-source /opt/ros/melodic/setup.bash
+source /opt/ros/noetic/setup.bash
 rm -f ./build/catkin_make_install.log
 
 #

test/scripts/make_ros2.bash

@@ -10,7 +10,11 @@ pushd ../../../..
 # BUILDTYPE=Debug
 BUILDTYPE=Release
 
-source /opt/ros/eloquent/setup.bash
+if [ -f /opt/ros/humble/setup.bash ] ; then 
+    source /opt/ros/humble/setup.bash
+elif [ -f /opt/ros/foxy/setup.bash ] ; then 
+    source /opt/ros/foxy/setup.bash
+fi
 colcon build --cmake-args " -DROS_VERSION=2" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
 source ./install/setup.bash
 

test/scripts/make_ros2.cmd

@@ -3,6 +3,11 @@ REM Build sick_lidar_localization on Windows ROS-2
 REM
 
 pushd ..\..\..\..
+rem Workaround for "The fully qualified file name must be less than 260 characters" errors:
+rem Copy folder sick_lidar_localization2_pretest to c:\<short_folder_name> and run in that directory...
+rem if not exist r:\sick_lidar_localization2_pretest subst r: ..
+rem pushd r:\sick_lidar_localization2_pretest
+rem Cleanup
 rmdir /s/q .\log
 for %%i in ( .\install\sick_lidar_localization\lib .\install\sick_lidar_localization\lib\sick_lidar_localization .\build\sick_lidar_localization\Debug .\build\sick_lidar_localization\Release ) do (
   if exist %%i\gen_service_call.exe        del /f/q %%i\gen_service_call.exe
@@ -31,6 +36,7 @@ if not exist .\build\sick_lidar_localization\Release\gen_service_call.exe
 if not exist .\build\sick_lidar_localization\Release\sick_lidar_localization.exe ( @echo colcon build sick_lidar_localization.exe failed & @pause ) else ( @echo Successfully build sick_lidar_localization.exe for ROS-2 Windows )
 if not exist .\build\sick_lidar_localization\Release\pointcloud_converter.exe    ( @echo colcon build pointcloud_converter.exe failed    & @pause ) else ( @echo Successfully build pointcloud_converter.exe for ROS-2 Windows )
 
+popd
 popd
 @pause
 rem @timeout /t 10

test/scripts/makeall_ros1.bash

@@ -1,6 +1,6 @@
 #!/bin/bash
 pushd ../../../..
-source /opt/ros/melodic/setup.bash
+source /opt/ros/noetic/setup.bash
 
 #
 # cleanup

test/scripts/makeall_ros2.bash

@@ -4,6 +4,7 @@
 # cleanup
 #
 
+printf "\033c"
 pushd ../../../..
 if [ -d ./src/sick_lidar_localization2_pretest ] ; then 
   cp -f ./src/sick_lidar_localization2_pretest/package_ros2.xml ./src/sick_lidar_localization2_pretest/package.xml

test/scripts/run_linux_ros1_simu.bash

@@ -68,7 +68,7 @@ sleep 3
 # 
 
 pushd ../../../..
-source /opt/ros/melodic/setup.bash
+source /opt/ros/noetic/setup.bash
 source ./install/setup.bash 
 SICK_LIDAR_LOCALIZATION_ROOT=./src/sick_lidar_localization
 if [ -d ./src/sick_lidar_localization2_pretest ] ; then SICK_LIDAR_LOCALIZATION_ROOT=./src/sick_lidar_localization2_pretest ; fi

test/scripts/run_linux_ros2_simu.bash

@@ -60,6 +60,8 @@ printf "\033c"
 # 
 
 echo -e "(Re-)starting sudo sick_rest_server.py ...\n" 
+pkill -f roslaunch > /dev/null 2>&1
+pkill -f rosmaster > /dev/null 2>&1
 sudo pkill -f sick_rest_server.py > /dev/null 2>&1
 sudo pkill -9 -f sick_rest_server.py > /dev/null 2>&1
 sudo python3 ../rest_server/python/sick_rest_server.py --time_sync=1 &
@@ -70,11 +72,15 @@ sleep 3
 # 
 
 pushd ../../../..
-source /opt/ros/eloquent/setup.bash
+if [ -f /opt/ros/humble/setup.bash ] ; then 
+    source /opt/ros/humble/setup.bash
+elif [ -f /opt/ros/foxy/setup.bash ] ; then 
+    source /opt/ros/foxy/setup.bash
+fi
 source ./install/setup.bash 
 SICK_LIDAR_LOCALIZATION_ROOT=./src/sick_lidar_localization
 if [ -d ./src/sick_lidar_localization2_pretest ] ; then SICK_LIDAR_LOCALIZATION_ROOT=./src/sick_lidar_localization2_pretest ; fi
-ros2 run sick_lidar_localization sick_lidar_localization $SICK_LIDAR_LOCALIZATION_ROOT/launch/sick_lidar_localization.launch --ros-args hostname:=localhost udp_ip_sim_input:=127.0.0.1 udp_ip_sim_output:=localhost verbose:=1 &
+ros2 run sick_lidar_localization sick_lidar_localization $SICK_LIDAR_LOCALIZATION_ROOT/launch/sick_lidar_localization.launch --ros-args -p hostname:=localhost -p udp_ip_sim_input:=127.0.0.1 -p udp_ip_sim_output:=localhost -p verbose:=1 &
 sleep 3
 
 # 

test/scripts/run_win64_ros2_simu.cmd

@@ -22,6 +22,12 @@ REM Start rest server
 REM 
 
 python --version
+rem Required for python rest server and pcapng player:
+rem pip install flask
+rem pip install pcapng
+rem pip install scapy
+rem pip install pypcapfile
+rem pip install python-pcapng
 start "rest server" python %SICK_LIDAR_LOCALIZATION_ROOT%/test/rest_server/python/sick_rest_server.py
 @timeout /t 3
 
@@ -71,7 +77,7 @@ REM rviz -> Add by display type TF
 REM 
 
 start "pointcloud_converter" /min ros2 run sick_lidar_localization pointcloud_converter %SICK_LIDAR_LOCALIZATION_ROOT%/launch/pointcloud_converter.launch
-start "rviz2" rviz2 -d %SICK_LIDAR_LOCALIZATION_ROOT%/test/config/rviz2_win64_sick_lidar_localization_pointcloud.rviz
+start "rviz2" ros2 run rviz2 rviz2 -d %SICK_LIDAR_LOCALIZATION_ROOT%/test/config/rviz2_win64_sick_lidar_localization_pointcloud.rviz
 start "ros2 topic echo /localizationcontroller/out" cmd /k .\src\sick_lidar_localization2_pretest\test\scripts\simu_echo_localizationcontroller_out_topics.cmd
 
 REM 

test/scripts/vs_code.bash

@@ -1,9 +1,14 @@
 #!/bin/bash
 
-
-if [ -f /opt/ros/melodic/setup.bash  ] ; then source /opt/ros/melodic/setup.bash  ; fi
-if [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash ; fi
+if [ -f /opt/ros/humble/setup.bash ] ; then 
+    source /opt/ros/humble/setup.bash
+elif [ -f /opt/ros/foxy/setup.bash ] ; then 
+    source /opt/ros/foxy/setup.bash
+elif [ -f /opt/ros/noetic/setup.bash ] ; then 
+    source /opt/ros/noetic/setup.bash
+fi
 pushd ../../../..
 if [ -f ./install/setup.bash ] ; then source ./install/setup.bash ; fi
 code ./sick_lidar_localization_vscode.code-workspace
-popd 
+popd
+