Add support for ROS 2 (Humble)

.github/workflows/build_test.yaml

@@ -0,0 +1,69 @@
+name: build test
+
+on:
+  workflow_dispatch:
+  pull_request:
+
+jobs:
+  build:
+    name: build
+    runs-on: ubuntu-22.04
+    timeout-minutes: 120
+    strategy:
+      fail-fast: false
+    env:
+      ROS_DISTRO: humble
+    container:
+      image: osrf/ros:humble-desktop-jammy
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+
+      - name: Install dependencies of choreonoid
+        run: |
+          apt-get update && \
+          apt-get -y upgrade && \
+          apt-get -y install \
+          build-essential \
+          cmake-curses-gui \
+          libboost-dev \
+          libboost-system-dev \
+          libboost-program-options-dev \
+          libboost-iostreams-dev \
+          libeigen3-dev \
+          uuid-dev \
+          libxfixes-dev \
+          libyaml-dev \
+          libfmt-dev \
+          gettext \
+          zlib1g-dev \
+          libjpeg-dev \
+          libpng-dev \
+          libfreetype-dev \
+          qtbase5-dev \
+          libqt5x11extras5-dev \
+          libqt5svg5-dev \
+          qttranslations5-l10n \
+          python3-dev \
+          python3-numpy \
+          libassimp-dev \
+          libode-dev \
+          libfcl-dev \
+          libpulse-dev \
+          libsndfile1-dev \
+          libgstreamer1.0-dev \
+          libgstreamer-plugins-base1.0-dev \
+          libzip-dev \
+          python3-colcon-coveragepy-result \
+          python3-colcon-lcov-result
+
+      - name: Run build test
+        uses: ros-tooling/[email protected]
+        with:
+          target-ros2-distro: humble
+          vcs-repo-file-url: dependency.repos
+          extra-cmake-args: -DWITH_INTEGRATION_TEST=ON
+          # If possible, pin the repository in the workflow to a specific commit to avoid
+          # changes in colcon-mixin-repository from breaking your tests.

CMakeLists.txt

@@ -7,25 +7,39 @@ if(NOT CMAKE_BUILD_TYPE)
     FORCE)
 endif()
 
-find_package(
-  catkin REQUIRED COMPONENTS
-  choreonoid
-  roscpp
-  std_msgs
-  sensor_msgs
-  image_transport
-
-  # For ros_control
-  pluginlib
-  angles
-  controller_manager
-  hardware_interface
-  joint_limits_interface
-  transmission_interface
-  urdf
-  )
-
-catkin_package(SKIP_CMAKE_CONFIG_GENERATION SKIP_PKG_CONFIG_GENERATION)
+if(DEFINED ENV{ROS_VERSION})
+  if($ENV{ROS_VERSION} EQUAL 1)
+    find_package(
+      catkin REQUIRED COMPONENTS
+      roscpp
+      pluginlib
+      angles
+      controller_manager
+      hardware_interface
+      joint_limits_interface
+      transmission_interface
+      urdf
+    )
+  elseif ($ENV{ROS_VERSION} EQUAL 2)
+    find_package(
+      ament_cmake REQUIRED COMPONENTS
+      rclcpp
+    )
+  endif()
+endif()
+
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(choreonoid REQUIRED)
+
+if(DEFINED ENV{ROS_VERSION})
+  if($ENV{ROS_VERSION} EQUAL 1)
+    catkin_package(SKIP_CMAKE_CONFIG_GENERATION SKIP_PKG_CONFIG_GENERATION)
+    include_directories(${catkin_INCLUDE_DIRS})
+  endif()
+endif()
 
 if(CHOREONOID_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD ${CHOREONOID_CXX_STANDARD})
@@ -34,14 +48,19 @@ else()
   add_compile_options(-std=c++14)
 endif()
 
-include_directories(${catkin_INCLUDE_DIRS})
 
 add_subdirectory(src)
 
-# For ros_control
-install(FILES plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+if(DEFINED ENV{ROS_VERSION})
+  if($ENV{ROS_VERSION} EQUAL 1)
+    # For ros_control
+    install(FILES plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  USE_SOURCE_PERMISSIONS
-  )
+    install(DIRECTORY launch
+      DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+      USE_SOURCE_PERMISSIONS
+    )
+  elseif($ENV{ROS_VERSION} EQUAL 2)
+    ament_package()
+  endif()
+endif()

dependency.repos

@@ -0,0 +1,5 @@
+repositories:
+  choreonoid:
+    type: git
+    url: https://github.com/choreonoid/choreonoid.git
+    version: master

package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>choreonoid_ros</name>
   <version>1.7.0</version>
   <description>The package for using Choreonoid as a ROS node</description>
@@ -10,22 +10,29 @@
   <url type="website">http://choreonoid.org</url>
   <url type="repository">https://github.com/choreonoid/choreonoid_ros.git</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
   <depend>choreonoid</depend>
-  <depend>roscpp</depend>
+
+  <depend condition="$ROS_VERSION == 1">roscpp</depend>
+  <depend condition="$ROS_VERSION == 2">rclcpp</depend>
+
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>sensor_msgs</depend>
   <depend>image_transport</depend>
-  <depend>pluginlib</depend>
-  <depend>angles</depend>
-  <depend>controller_manager</depend>
-  <depend>hardware_interface</depend>
-  <depend>joint_limits_interface</depend>
-  <depend>transmission_interface</depend>
-  <depend>urdf</depend>
+
+  <depend condition="$ROS_VERSION == 1">pluginlib</depend>
+  <depend condition="$ROS_VERSION == 1">angles</depend>
+  <depend condition="$ROS_VERSION == 1">controller_manager</depend>
+  <depend condition="$ROS_VERSION == 1">hardware_interface</depend>
+  <depend condition="$ROS_VERSION == 1">joint_limits_interface</depend>
+  <depend condition="$ROS_VERSION == 1">transmission_interface</depend>
+  <depend condition="$ROS_VERSION == 1">urdf</depend>
 
   <export>
-    <build_type>cmake</build_type>
-    <choreonoid_ros plugin="${prefix}/plugins.xml" />
+    <build_type condition="$ROS_VERSION == 1">cmake</build_type>
+    <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+    <choreonoid_ros plugin="${prefix}/plugins.xml"/>
   </export>
 </package>

src/node/CMakeLists.txt

@@ -1,5 +1,17 @@
-set(target choreonoid_ros)
-add_executable(${target} choreonoid_ros.cpp)
-set_target_properties(${target} PROPERTIES OUTPUT_NAME choreonoid)
-target_link_libraries(${target} Choreonoid::CnoidBase ${roscpp_LIBRARIES})
-install(TARGETS ${target} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+set(target choreonoid)
+
+if(DEFINED ENV{ROS_VERSION})
+  if($ENV{ROS_VERSION} EQUAL 1)
+    add_executable(${target} choreonoid_ros.cpp)
+    set_target_properties(${target} PROPERTIES OUTPUT_NAME choreonoid)
+    target_link_libraries(${target} Choreonoid::CnoidBase ${roscpp_LIBRARIES})
+    install(TARGETS ${target} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+  elseif($ENV{ROS_VERSION} EQUAL 2)
+    add_executable(${target} choreonoid_ros2.cpp)
+    ament_target_dependencies(${target} rclcpp std_msgs sensor_msgs image_transport)
+    set_target_properties(${target} PROPERTIES OUTPUT_NAME choreonoid)
+    target_link_libraries(${target} Choreonoid::CnoidBase)
+    install(TARGETS ${target}
+      RUNTIME DESTINATION lib/${PROJECT_NAME})
+  endif()
+endif()

src/node/choreonoid_ros2.cpp

@@ -0,0 +1,37 @@
+#include "../plugin/ROS2Plugin.hpp"
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <cnoid/App>
+#include <cnoid/Config>
+#include <cnoid/ExecutablePath>
+#include <cnoid/FileUtil>
+#include <cnoid/Plugin>
+#include <cnoid/PluginManager>
+#include <cnoid/ProjectManager>
+#include <cnoid/UTF8>
+#include <cstdlib>
+#include <rclcpp/rclcpp.hpp>
+#include <regex>
+
+using namespace std;
+using namespace cnoid;
+namespace filesystem = stdx::filesystem;
+
+int main(int argc, char** argv)
+{
+    cnoid::App app(argc, argv, "Choreonoid-ROS2", "Choreonoid");
+
+    auto plugin_manager = cnoid::PluginManager::instance();
+
+    if(auto pluginPath = getenv("CNOID_PLUGIN_PATH")){
+      plugin_manager->addPluginPath(toUTF8(pluginPath));
+    }
+
+    auto plugin_path = ament_index_cpp::get_package_share_directory("choreonoid_ros2") + "/../../lib/choreonoid_ros2";
+    plugin_manager->addPluginPath(plugin_path);
+
+    app.requirePluginToCustomizeApplication("ROS2");
+
+    int ret = app.exec();
+
+    return ret;
+}