Humble

.clang-format

@@ -0,0 +1,16 @@
+---
+BasedOnStyle: Google
+
+ColumnLimit: 120
+SortIncludes: Never
+ReflowComments: false
+ReferenceAlignment: Left
+PointerAlignment: Left
+FixNamespaceComments: false
+BreakBeforeBraces: Custom
+BraceWrapping:
+    AfterStruct: true
+BreakConstructorInitializers: AfterColon
+PackConstructorInitializers: BinPack
+---
+

.github/workflows/main.yml

@@ -8,11 +8,13 @@ on: [push, pull_request] # on all pushes and PRs
 
 jobs:
   industrial_ci:
+    continue-on-error: true
+
     strategy:
       matrix:
         env:
-          - {ROS_DISTRO: melodic, ROS_REPO: testing}
-          - {ROS_DISTRO: noetic, ROS_REPO: testing}
+          - {ROS_DISTRO: humble, ROS_REPO: testing}
+          - {ROS_DISTRO: rolling, ROS_REPO: testing}
     env:
       CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)
     runs-on: ubuntu-latest

CMakeLists.txt

@@ -11,35 +11,24 @@ else()
   add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0)
 endif()
 
-set(THIS_PACKAGE_DEPS dynamic_reconfigure filters geometric_shapes laser_geometry moveit_core moveit_ros_perception roscpp sensor_msgs tf2 tf2_ros urdf visualization_msgs)
+set(THIS_PACKAGE_DEPS backward_ros urdf urdf_parser_plugin filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros tf2_eigen tf2_sensor_msgs urdf visualization_msgs GLUT OpenCV)
 set(MESSAGE_DEPS geometry_msgs std_msgs)
 
-find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_DEPS} ${MESSAGE_DEPS} message_generation pcl_conversions tf2_eigen tf2_sensor_msgs)
-find_package(PCL REQUIRED COMPONENTS common filters)
-find_package(Threads REQUIRED)
+find_package(ament_cmake REQUIRED)
 
-find_package(PkgConfig REQUIRED)
-pkg_check_modules(LIBFCL_PC REQUIRED fcl)
-# find *absolute* paths to LIBFCL_* paths
-find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS})
-set(LIBFCL_LIBRARIES)
-foreach(_lib ${LIBFCL_PC_LIBRARIES})
-  find_library(_lib_${_lib} ${_lib} HINTS ${LIBFCL_PC_LIBRARY_DIRS})
-  list(APPEND LIBFCL_LIBRARIES ${_lib_${_lib}})
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_DEPS})
+        find_package(${Dependency} REQUIRED)
 endforeach()
 
+find_package(PCL REQUIRED COMPONENTS common filters)
+find_package(Threads REQUIRED)
 
-add_message_files(FILES OrientedBoundingBox.msg OrientedBoundingBoxStamped.msg Sphere.msg SphereStamped.msg)
-generate_messages(DEPENDENCIES ${MESSAGE_DEPS})
+find_package(PkgConfig REQUIRED)
 
-catkin_package(
-    CATKIN_DEPENDS ${THIS_PACKAGE_DEPS} ${MESSAGE_DEPS} message_runtime
-    INCLUDE_DIRS include
-    LIBRARIES ${PROJECT_NAME} RayCastingShapeMask TFFramesWatchdog ${PROJECT_NAME}_utils
-)
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}_msgs "msg/OrientedBoundingBox.msg" "msg/OrientedBoundingBoxStamped.msg" DEPENDENCIES ${MESSAGE_DEPS} LIBRARY_NAME ${PROJECT_NAME})
 
-include_directories(SYSTEM ${LIBFCL_INCLUDE_DIRS})
-include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
+include_directories(include include/${PROJECT_NAME} include/${PROJECT_NAME}/utils)
 
 set(UTILS_SRCS
   src/utils/bodies.cpp
@@ -48,100 +37,111 @@ set(UTILS_SRCS
   src/utils/string_utils.cpp
   src/utils/tf2_eigen.cpp
   src/utils/tf2_sensor_msgs.cpp
-  src/utils/time_utils.cpp)
-
-if (PCL_VERSION VERSION_LESS "1.10")
-  set(UTILS_SRCS ${UTILS_SRCS} src/utils/crop_box.cpp)
-endif()
-
+  src/utils/time_utils.cpp
+)
 add_library(${PROJECT_NAME}_utils ${UTILS_SRCS})
-target_link_libraries(${PROJECT_NAME}_utils ${catkin_LIBRARIES} ${LIBFCL_LIBRARIES} ${PCL_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES})
+ament_target_dependencies(${PROJECT_NAME}_utils ${THIS_PACKAGE_DEPS})
+ament_export_libraries(${PROJECT_NAME}_utils)
 
 add_library(tf2_sensor_msgs_rbf src/utils/tf2_sensor_msgs.cpp)
-target_link_libraries(tf2_sensor_msgs_rbf PRIVATE ${PROJECT_NAME}_utils PUBLIC ${catkin_LIBRARIES})
-add_dependencies(tf2_sensor_msgs_rbf ${catkin_EXPORTED_TARGETS})
+target_link_libraries(tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils)
+ament_export_libraries(tf2_sensor_msgs_rbf)
 
 add_library(RayCastingShapeMask src/RayCastingShapeMask.cpp)
-target_link_libraries(RayCastingShapeMask ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
-add_dependencies(RayCastingShapeMask ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(RayCastingShapeMask ${PROJECT_NAME}_utils)
 
 add_library(TFFramesWatchdog src/TFFramesWatchdog.cpp)
-target_link_libraries(TFFramesWatchdog ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
-add_dependencies(TFFramesWatchdog ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(TFFramesWatchdog ${PROJECT_NAME}_utils)
 
 add_library(${PROJECT_NAME} src/RobotBodyFilter.cpp)
+target_include_directories(${PROJECT_NAME} PUBLIC
+$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include>)
 target_link_libraries(${PROJECT_NAME}
   ${PROJECT_NAME}_utils
   tf2_sensor_msgs_rbf
   TFFramesWatchdog
   RayCastingShapeMask
-  ${catkin_LIBRARIES}
-  ${PCL_LIBRARIES}
 )
-add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS})
 
-install(DIRECTORY include/${PROJECT_NAME}/
-   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(DIRECTORY include/
+   DESTINATION include)
 
-install(TARGETS ${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask ${PROJECT_NAME}_utils tf2_sensor_msgs_rbf
-   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
-
-install(FILES laser_filters.xml
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/)
+install(TARGETS ${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils
+  EXPORT export_${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
 
 install(FILES rviz/debug.rviz
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz)
+  DESTINATION share/rviz)
 
-if (CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
+if (BUILD_TESTING) 
+  find_package(ament_cmake_gtest REQUIRED)
 
-  catkin_add_gtest(test_set_utils test/test_set_utils.cpp)
-  target_link_libraries(test_set_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+  ament_add_gtest(test_set_utils test/test_set_utils.cpp)
+  target_link_libraries(test_set_utils ${PROJECT_NAME}_utils)
+  ament_target_dependencies(test_set_utils ${THIS_PACKAGE_DEPS})
 
-  catkin_add_gtest(test_shapes_rbf test/test_shapes.cpp)
-  target_link_libraries(test_shapes_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+  ament_add_gtest(test_shapes_rbf test/test_shapes.cpp)
+  target_link_libraries(test_shapes_rbf ${PROJECT_NAME}_utils)
+  ament_target_dependencies(test_shapes_rbf ${THIS_PACKAGE_DEPS})
 
-  catkin_add_gtest(test_urdf_eigen test/test_urdf_eigen.cpp)
-  target_link_libraries(test_urdf_eigen ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+  # ament_add_gtest(test_urdf_eigen test/test_urdf_eigen.cpp)
+  # target_link_libraries(test_urdf_eigen ${PROJECT_NAME}_utils)
 
-  catkin_add_gtest(test_time_utils test/test_time_utils.cpp)
-  target_link_libraries(test_time_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+#   ament_add_gtest(test_time_utils test/test_time_utils.cpp)
+#   target_link_libraries(test_time_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
 
-  catkin_add_gtest(test_tf2_eigen test/test_tf2_eigen.cpp)
-  target_link_libraries(test_tf2_eigen ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+#   ament_add_gtest(test_tf2_eigen test/test_tf2_eigen.cpp)
+#   target_link_libraries(test_tf2_eigen ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
 
-  catkin_add_gtest(test_string_utils test/test_string_utils.cpp)
-  target_link_libraries(test_string_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+#   ament_add_gtest(test_string_utils test/test_string_utils.cpp)
+#   target_link_libraries(test_string_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
 
-  catkin_add_gtest(test_bodies test/test_bodies.cpp)
-  target_link_libraries(test_bodies ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+  # ament_add_gtest(test_bodies test/test_bodies.cpp)
+  # ament_target_dependencies(test_bodies ${THIS_PACKAGE_DEPS})
+  # target_link_libraries(test_bodies ${PROJECT_NAME} ${PROJECT_NAME}_utils)
 
-  catkin_add_gtest(test_cloud test/test_cloud.cpp)
-  target_link_libraries(test_cloud ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+#   ament_add_gtest(test_cloud test/test_cloud.cpp)
+#   target_link_libraries(test_cloud ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
 
-  catkin_add_gtest(test_tf2_sensor_msgs test/test_tf2_sensor_msgs.cpp)
-  target_link_libraries(test_tf2_sensor_msgs tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+#   ament_add_gtest(test_tf2_sensor_msgs test/test_tf2_sensor_msgs.cpp)
+#   target_link_libraries(test_tf2_sensor_msgs tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
 
-  catkin_add_gtest(test_xmlrpc_traits test/test_xmlrpc_traits.cpp)
-  target_link_libraries(test_xmlrpc_traits ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
+#   ament_add_gtest(test_xmlrpc_traits test/test_xmlrpc_traits.cpp)
+#   target_link_libraries(test_xmlrpc_traits ${PROJECT_NAME}_utils ${catkin_LIBRARIES})
 
-  add_rostest_gtest(test_filter_utils test/test_filter_utils.test test/test_filter_utils.cpp)
-  target_link_libraries(test_filter_utils ${PROJECT_NAME} ${catkin_LIBRARIES})
+#   ament_add_gtest(test_filter_utils test/test_filter_utils.test test/test_filter_utils.cpp)
+#   ament_target_dependencies(test_filter_utils ${THIS_PACKAGE_DEPS})
+#   target_link_libraries(test_filter_utils lib${PROJECT_NAME})
 
-  catkin_add_gtest(test_tf_frames_watchdog test/test_tf_frames_watchdog.cpp)
-  target_link_libraries(test_tf_frames_watchdog TFFramesWatchdog ${catkin_LIBRARIES})
+#   ament_add_gtest(test_tf_frames_watchdog test/test_tf_frames_watchdog.cpp)
+#   target_link_libraries(test_tf_frames_watchdog TFFramesWatchdog ${catkin_LIBRARIES})
 
-  catkin_add_gtest(test_ray_casting_shape_mask test/test_ray_casting_shape_mask.cpp)
-  target_link_libraries(test_ray_casting_shape_mask RayCastingShapeMask ${catkin_LIBRARIES})
-  if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
-    target_compile_definitions(test_ray_casting_shape_mask PRIVATE RELEASE_BUILD=1)
-  else()
-    target_compile_definitions(test_ray_casting_shape_mask PRIVATE RELEASE_BUILD=0)
-  endif()
+#   ament_add_gtest(test_ray_casting_shape_mask test/test_ray_casting_shape_mask.cpp)
+#   target_link_libraries(test_ray_casting_shape_mask RayCastingShapeMask ${catkin_LIBRARIES})
+#   if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
+#     target_compile_definitions(test_ray_casting_shape_mask PRIVATE RELEASE_BUILD=1)
+#   else()
+#     target_compile_definitions(test_ray_casting_shape_mask PRIVATE RELEASE_BUILD=0)
+#   endif()
 
-  add_rostest_gtest(test_robot_body_filter test/test_robot_body_filter.test test/test_robot_body_filter.cpp)
-  target_link_libraries(test_robot_body_filter ${PROJECT_NAME} ${catkin_LIBRARIES})
-  target_compile_options(test_robot_body_filter PRIVATE -fno-var-tracking-assignments)  # speed up build
-endif()
+#   ament_add_gtest(test_robot_body_filter test/test_robot_body_filter.test test/test_robot_body_filter.cpp)
+#   target_link_libraries(test_robot_body_filter ${PROJECT_NAME} ${catkin_LIBRARIES})
+#   target_compile_options(test_robot_body_filter PRIVATE -fno-var-tracking-assignments)  # speed up build
+
+  install(FILES test/box.dae test/triangle.dae
+    DESTINATION share/robot_body_filter/test)
+endif()
+
+ament_export_include_directories(include include/${PROJECT_NAME} include/${PROJECT_NAME}/utils)
+ament_export_dependencies(${THIS_PACKAGE_DEPS} rosidl_default_runtime)
+ament_export_libraries(${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+ament_package()

examples/body_filter_node.launch.py

@@ -0,0 +1,57 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    ld = LaunchDescription()
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("body_filter_node"),
+                    "examples",
+                    "full_example.urdf",
+                ]
+            ),
+            " sim_mode:=",
+            LaunchConfiguration("sim_mode"),
+        ]
+    )
+    config = os.path.join(
+        get_package_share_directory('ros2_tutorials'),
+        'config',
+        'params.yaml'
+        )
+
+    node = Node(
+        package="ros2_tutorials",
+        name="your_amazing_node",
+        executable="test_yaml_params",
+        parameters=[config],
+    )
+    robot_description = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[
+            {"robot_description": robot_description_content},
+        ],
+    )
+    ld.add_action(robot_description)
+    ld.add_action(node)
+    return ld

include/robot_body_filter/RayCastingShapeMask.h

@@ -10,7 +10,7 @@
 #include <robot_body_filter/utils/bodies.h>
 #include <robot_body_filter/utils/cloud.h>
 #include <geometric_shapes/body_operations.h>
-#include <ros/console.h>
+// #include <ros/console.h>
 
 namespace robot_body_filter
 {