[pull] devel from choreonoid:master

.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 ament_index_cpp
+    )
+  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,28 @@ 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 `launch` dir. only if `--install` option is set in catkin config
+    if(${CMAKE_INSTALL_PREFIX} MATCHES .*/install)
+      install(DIRECTORY launch
+          DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+          USE_SOURCE_PERMISSIONS
+          PATTERN "ros2" EXCLUDE
+        )
+    endif()
+  elseif($ENV{ROS_VERSION} EQUAL 2)
+    ament_package()
 
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  USE_SOURCE_PERMISSIONS
-  )
+    install(DIRECTORY
+      launch/ros2
+      DESTINATION share/${PROJECT_NAME}/launch
+    )
+  endif()
+endif()

dependency.repos

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

launch/ros2/choreonoid.launch.xml

@@ -0,0 +1,5 @@
+<launch>
+  <arg name="choreonoid_arg" default=""/>
+  <node name="choreonoid" pkg="choreonoid_ros" exec="choreonoid"
+        args="$(var choreonoid_arg)" output="screen"/>
+</launch>

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,32 @@
   <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>geometry_msgs</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>image_transport_plugins</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>
+  <depend condition="$ROS_VERSION == 2">ament_index_cpp</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 ament_index_cpp 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,73 @@
+#include <ament_index_cpp/get_package_prefix.hpp>
+#include <rclcpp/utilities.hpp>
+
+#include <cnoid/App>
+#include <cnoid/PluginManager>
+#include <cnoid/UTF8>
+
+#include <string>
+#include <vector>
+#include <iostream>
+
+int main(int argc, char** argv)
+{
+    // removes ros-dependent arguments
+    // rclcpp::remove_ros_arguments throws an error if it fails parsing
+    std::vector<std::string> nonRosArgvString = rclcpp::remove_ros_arguments(argc, argv);
+    int nonRosArgc = nonRosArgvString.size();
+    char* nonRosArgv[nonRosArgc];
+    for (int i = 0; i < nonRosArgc; ++i) {
+        nonRosArgv[i] = nonRosArgvString.at(i).data();
+    }
+    if (nonRosArgvString.at(nonRosArgc - 1).empty()) {
+        // ignores the final nonRosArgv
+        // because remove_ros_arguments potentially returns an empty string,
+        // which causes [Warning:  Input file "" was not processed.] on Choreonoid
+        --nonRosArgc;
+    }
+
+    cnoid::App app(nonRosArgc, nonRosArgv, "Choreonoid-ROS2", "Choreonoid");
+
+    auto pluginManager = cnoid::PluginManager::instance();
+
+    const char* prefixVar = getenv("AMENT_PREFIX_PATH");
+    if(prefixVar){
+        do {
+            const char* begin = prefixVar;
+            while(*prefixVar != ':' && *prefixVar) prefixVar++;
+            pluginManager->addPluginDirectoryAsPrefix(cnoid::toUTF8(std::string(begin, prefixVar)));
+        } while (0 != *prefixVar++);
+    } else {
+        try {
+            pluginManager->addPluginDirectoryAsPrefix(
+                ament_index_cpp::get_package_prefix("choreonoid_ros"));
+        }
+        catch(const ament_index_cpp::PackageNotFoundError& ex){
+            std::cerr << "Error: The choreonoid_ros package is not found." << std::endl;
+            return 1;
+        }
+    }
+
+    if(!app.requirePluginToCustomizeApplication("ROS2")){
+        if(app.error() == cnoid::App::PluginNotFound){
+            auto message = app.errorMessage();
+            if(message.empty()){
+                std::cerr << "ROS2 plugin is not found." << std::endl;
+            } else {
+                std::cerr << "ROS2 plugin cannot be loaded.\n";
+                std::cerr << message << std::endl;
+            }
+        } else {
+            std::cerr << "ROS2 plugin does not work corerctly." << std::endl;
+        }
+        return 1;
+    }
+
+    rclcpp::init(argc, argv);
+
+    int ret = app.exec();
+
+    rclcpp::shutdown();
+
+    return ret;
+}