ROS 2 Migration

.github/workflows/build_code.yml

@@ -6,33 +6,39 @@ jobs:
   build-code:
     runs-on: ubuntu-latest
     container:
-      image: osrf/ros:noetic-desktop-full
+      image: osrf/ros:humble-desktop-full
 
     steps:
-    - name: Create catkin workspace
-      shell: bash
-      run: |
-        mkdir -p ~/catkin_ws/src
-    - uses: actions/checkout@v3
-      with:
-        path: 'catkin_ws/src/Cartesian-Impedance-Controller'
-    - name: Build in catkin_ws
-      shell: bash
-      run: |
-        apt-get update
-        apt-get install -y python3-catkin-tools  python3-rosdep git
-        cd catkin_ws
-        bash src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh
-        rosdep update
-        source /opt/ros/$ROS_DISTRO/setup.bash
-        rosdep install --from-paths src --ignore-src --rosdistro=$ROS_DISTRO -y
-        catkin init
-        catkin config --extend /opt/ros/$ROS_DISTRO
-        catkin build
-        echo "Compile complete."
-    - name: Run tests
-      shell: bash
-      run: |
-        source /opt/ros/$ROS_DISTRO/setup.bash
-        cd catkin_ws
-        catkin test
+      - name: Install system dependencies
+        shell: bash
+        run: |
+          apt-get update && apt-get install -y \
+            python3-colcon-common-extensions \
+            python3-rosdep \
+            git \
+            nodejs npm  # for actions/checkout@v3
+
+      - name: Create ROS2 workspace directories
+        shell: bash
+        run: |
+          mkdir -p $GITHUB_WORKSPACE/ros2_ws/src
+
+      - name: Checkout repository into workspace
+        uses: actions/checkout@v3
+        with:
+          ref: ros2
+          path: ros2_ws/src/Cartesian-Impedance-Controller
+
+      - name: Install package dependencies & Build workspace
+        shell: bash
+        run: |
+          cd $GITHUB_WORKSPACE/ros2_ws
+
+          bash src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh
+
+          rosdep update
+          rosdep install --from-paths src --ignore-src --rosdistro=humble -y
+
+          source /opt/ros/humble/setup.bash
+          colcon build --base-paths $GITHUB_WORKSPACE/ros2_ws
+

CMakeLists.txt

@@ -1,119 +1,103 @@
-cmake_minimum_required(VERSION 2.8.3)
-cmake_policy(SET CMP0048 NEW) # Version not in project() command
+cmake_minimum_required(VERSION 3.16)
 project(cartesian_impedance_controller)
 
-find_package(Boost 1.49 REQUIRED)
-find_package(PkgConfig)
+# Compiler warnings (for GNU and Clang)
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(
+    -Wall -Wextra -Wpedantic
+    -Werror=conversion -Werror=unused-but-set-variable
+    -Werror=return-type -Werror=shadow -Werror=format
+    -Werror=range-loop-construct -Werror=missing-braces
+  )
+endif()
 
-# Find RBDyn library and dependencies
-pkg_search_module(Eigen3 REQUIRED eigen3)
-pkg_check_modules(mc_rbdyn_urdf REQUIRED mc_rbdyn_urdf)
-pkg_check_modules(RBDyn REQUIRED RBDyn)
-pkg_check_modules(SpaceVecAlg REQUIRED SpaceVecAlg)
-pkg_check_modules(tinyxml2 REQUIRED tinyxml2)
+# Export all symbols on Windows
+set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
 
-find_package(catkin REQUIRED COMPONENTS
-  actionlib
-  actionlib_msgs
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  rclcpp
   control_msgs
+  sensor_msgs
   controller_interface
-  controller_manager
-  dynamic_reconfigure
-  eigen_conversions
   geometry_msgs
   hardware_interface
-  message_generation
-  pluginlib
+  rclcpp_lifecycle
+  rclcpp_action
   realtime_tools
-  roscpp
-  sensor_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
   std_msgs
-  tf
-  tf_conversions
   trajectory_msgs
+  generate_parameter_library
+  urdf
+  RBDyn
+  mc_rbdyn_urdf
+  Eigen3
+  pluginlib
 )
 
-add_message_files(FILES
-  ControllerConfig.msg
-  ControllerState.msg
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ControllerConfig.msg"
+  "msg/ControllerState.msg"
+  DEPENDENCIES std_msgs geometry_msgs sensor_msgs
 )
 
-generate_messages(
-DEPENDENCIES
-  std_msgs
-  geometry_msgs
-  sensor_msgs
-)
+set(library_name cartesian_impedance_controller_lib)
 
-generate_dynamic_reconfigure_options(
-  cfg/damping.cfg
-  cfg/stiffness.cfg
-  cfg/wrench.cfg
+generate_parameter_library(cartesian_impedance_controller_parameters
+  src/cartesian_impedance_controller_parameters.yaml
 )
 
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS actionlib actionlib_msgs control_msgs controller_interface controller_manager dynamic_reconfigure eigen_conversions geometry_msgs hardware_interface message_runtime pluginlib realtime_tools roscpp sensor_msgs std_msgs tf tf_conversions trajectory_msgs
-  DEPENDS Boost Eigen3 mc_rbdyn_urdf RBDyn SpaceVecAlg tinyxml2
-  LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_ros
+add_library(${library_name} SHARED
+  src/cartesian_impedance_controller_ros.cpp
+  src/cartesian_impedance_controller.cpp
 )
 
-###########
-## Build ##
-###########
-
-# Sets build type to "Release" in case no build type has not been set before. This is necessary to run this controller at 1 kHz.
-# On our RT system with an i5 11th Gen processor this reduces update evaluations from about 800 to 300 microseconds
-if(NOT CMAKE_BUILD_TYPE) 
-    set(CMAKE_BUILD_TYPE Release)
-endif(NOT CMAKE_BUILD_TYPE)
-
-# Core library
-add_library(${PROJECT_NAME} src/cartesian_impedance_controller.cpp)
-add_dependencies(
-  ${PROJECT_NAME}
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-)
+target_compile_features(${library_name} PUBLIC cxx_std_17)
 
-target_compile_options(${PROJECT_NAME} PUBLIC -std=c++14)
-target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${SpaceVecAlg_INCLUDE_DIRS} ${RBDyn_INCLUDE_DIRS} ${mc_rbdyn_urdf_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} PUBLIC ${Eigen3_LIBRARIES})
-
-# ROS Integration
-add_library(${PROJECT_NAME}_ros src/cartesian_impedance_controller_ros.cpp)
-add_dependencies(
-  ${PROJECT_NAME}_ros
-  ${${PROJECT_NAME}_ros_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  ${PROJECT_NAME}_gencfg
+target_include_directories(${library_name} PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp> 
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+target_link_libraries(${library_name} PUBLIC
+  "${cpp_typesupport_target}"
+  cartesian_impedance_controller_parameters
+  Eigen3::Eigen
+  RBDyn
+  mc_rbdyn_urdf::mc_rbdyn_urdf
 )
 
-target_compile_options(${PROJECT_NAME}_ros PUBLIC -std=c++14)
-target_include_directories(${PROJECT_NAME}_ros PUBLIC include ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${tinyxml2_INCLUDE_DIRS} ${SpaceVecAlg_INCLUDE_DIRS} ${RBDyn_INCLUDE_DIRS} ${mc_rbdyn_urdf_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME}_ros PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${tinyxml2_LIBRARIES} RBDyn mc_rbdyn_urdf ${Eigen3_LIBRARIES})
-
-
-## Install project namespaced headers
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  FILES_MATCHING PATTERN "*.h"
-  PATTERN ".svn" EXCLUDE)
+pluginlib_export_plugin_description_file(controller_interface cartesian_impedance_controller_plugin.xml)
 
-## Testing
-if (CATKIN_ENABLE_TESTING)
-  # Base library tests
-	find_package(rostest REQUIRED)
-  add_rostest_gtest(base_tests test/base.test test/base_tests.cpp)
-  target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES})
+ament_target_dependencies(${library_name} PUBLIC 
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}  
+)
 
-  # ROS basic tests
-  add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp)
-  target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES})
+install(
+  DIRECTORY include/
+  DESTINATION include/cartesian_impedance_controller
+)
 
-  # ROS functionality tests
-  add_rostest_gtest(ros_func_tests test/ros_func.test test/ros_func_tests.cpp)
-  target_link_libraries(ros_func_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES})
-endif()
+install(
+  TARGETS ${library_name} cartesian_impedance_controller_parameters
+  EXPORT export_${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
 
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+ament_package()

Dockerfile

@@ -0,0 +1,97 @@
+# If you would like to use lightweight image, you can use the following base image:
+# FROM ros:humble-ros-base
+
+FROM osrf/ros:humble-desktop-full 
+
+ENV DEBIAN_FRONTEND=noninteractive \
+    LANG=C.UTF-8 \
+    LC_ALL=C.UTF-8 \
+    ROS_DISTRO=humble
+
+ARG BUILD_FRANKA_ROS=false
+
+ARG USER_UID=1001
+ARG USER_GID=1001
+ARG USERNAME=user
+
+RUN apt-get update && \
+    apt-get install -y --no-install-recommends \
+        bash-completion \
+        curl \
+        gdb \
+        git \
+        nano \
+        openssh-client \
+        python3-colcon-argcomplete \
+        python3-colcon-common-extensions \
+        sudo \
+        libeigen3-dev \
+        libboost-all-dev \
+        vim && \
+    apt-get clean && \
+    rm -rf /var/lib/apt/lists/*
+
+RUN groupadd --gid $USER_GID $USERNAME && \
+    useradd --uid $USER_UID --gid $USER_GID -m $USERNAME && \
+    echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers && \
+    echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> /home/$USERNAME/.bashrc && \
+    echo "source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" >> /home/$USERNAME/.bashrc
+
+USER $USERNAME
+
+WORKDIR /ros2_ws
+COPY . /ros2_ws/src/Cartesian-Impedance-Controller
+
+RUN sudo chown -R $USERNAME:$USERNAME /ros2_ws
+
+RUN sudo apt update
+
+RUN rosdep update && \
+    rosdep install --from-paths src/Cartesian-Impedance-Controller --ignore-src --rosdistro=$ROS_DISTRO -y
+
+RUN if [ "$BUILD_FRANKA_ROS" = "true" ]; then \
+      echo "Installing franka_ros2 dependencies and cloning repository..."; \
+      sudo apt-get install -y --no-install-recommends \
+      	   ros-humble-franka-description \
+           ros-humble-ros-gz \
+           ros-humble-sdformat-urdf \
+           ros-humble-joint-state-publisher-gui \
+           ros-humble-ros2controlcli \
+           ros-humble-controller-interface \
+           ros-humble-hardware-interface-testing \
+           ros-humble-ament-cmake-clang-format \
+           ros-humble-ament-cmake-clang-tidy \
+           ros-humble-controller-manager \
+           ros-humble-ros2-control-test-assets \
+           libignition-gazebo6-dev \
+           libignition-plugin-dev \
+           ros-humble-hardware-interface \
+           ros-humble-control-msgs \
+           ros-humble-backward-ros \
+           ros-humble-generate-parameter-library \
+           ros-humble-realtime-tools \
+           ros-humble-joint-state-publisher \
+           ros-humble-joint-state-broadcaster \
+           ros-humble-moveit-ros-move-group \
+           ros-humble-moveit-kinematics \
+           ros-humble-moveit-planners-ompl \
+           ros-humble-moveit-ros-visualization \
+           ros-humble-joint-trajectory-controller \
+           ros-humble-moveit-simple-controller-manager \
+           ros-humble-rviz2 \	
+           ros-humble-xacro && \
+      sudo apt-get clean && \
+      git clone https://github.com/frankaemika/franka_ros2.git /ros2_ws/src/franka_ros2 && \
+      vcs import /ros2_ws/src < /ros2_ws/src/franka_ros2/franka.repos --recursive --skip-existing && \
+      rosdep install --from-paths src/franka_ros2 --ignore-src --rosdistro=$ROS_DISTRO -y; \
+    else \
+      echo "Skipping franka_ros2 integration - building only the Cartesian Impedance Controller."; \
+    fi
+
+RUN /bin/bash src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh
+
+RUN echo 'export CMAKE_PREFIX_PATH=/usr/local:$CMAKE_PREFIX_PATH' >> /home/$USERNAME/.bashrc
+
+RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release"
+
+CMD ["/bin/bash"]