Merge pull request #16 from MaximilienNaveau/topic/mnaveau/humble-dev…

…el/packaging

migrate to ROS2

.github/workflows/ci_ros2.yml

@@ -0,0 +1,35 @@
+name: Ubuntu20.04, ROS2 Continuous Integration
+
+on: [push, pull_request]
+
+jobs:
+  build:
+    strategy:
+      matrix:
+        include:
+          - ros_version: "humble"
+            ubuntu_version: "ubuntu-22.04"
+          - ros_version: "iron"
+            ubuntu_version: "ubuntu-22.04"
+    runs-on: ${{ matrix.ubuntu_version }}
+    steps:
+      #
+      # Setup the machines and build environment
+      #
+      - name: Install ROS.
+        uses: ros-tooling/setup-ros@0.7.9
+        with:
+          required-ros-distributions: ${{ matrix.ros_version }}
+
+      #
+      # Checkout the current branch
+      #
+      - uses: actions/checkout@v2
+
+      #
+      # Build and test the repo
+      #
+      - uses: ros-tooling/action-ros-ci@0.3.14
+        with:
+          package-name: linear_feedback_controller_msgs
+          target-ros2-distro: ${{ matrix.ros_version }}

CMakeLists.txt

@@ -1,79 +1,65 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.22.1)
 
-#
-# Choosing to build the unittest (Catkin vs Modern CMake vs Ament).
-#
-if(DEFINED CATKIN_ENABLE_TESTING)
-  set(BUILD_TESTING ${CATKIN_ENABLE_TESTING})
-endif()
+project(linear_feedback_controller_msgs)
 
-#
-# Project definition
-#
-project(linear_feedback_controller_msgs LANGUAGES CXX)
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-#
-# Dependencies
-#
+find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs geometry_msgs
-                                        eigen_conversions message_generation)
+find_package(tf2_eigen REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
-#
-# Generate the messages
-#
-add_message_files(DIRECTORY msg FILES Contact.msg Control.msg Sensor.msg)
-generate_messages(DEPENDENCIES std_msgs sensor_msgs geometry_msgs)
-
-#
-# Export as catkin package (For ROS1 compatibility...).
-#
-# cmake-format: off
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS std_msgs sensor_msgs geometry_msgs message_runtime eigen_conversions
-  DEPENDS EIGEN3)
-# cmake-format: on
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Contact.msg"
+  "msg/Control.msg"
+  "msg/Sensor.msg"
+  DEPENDENCIES builtin_interfaces std_msgs sensor_msgs geometry_msgs
+  ADD_LINTER_TESTS
+)
+rosidl_get_typesupport_target(cpp_typesupport_target
+  ${PROJECT_NAME} rosidl_typesupport_cpp)
 
 #
 # Main lib.
 #
 set(${PROJECT_NAME}_HEADERS include/${PROJECT_NAME}/eigen_conversions.hpp)
 set(${PROJECT_NAME}_SOURCES "")
-add_library(${PROJECT_NAME} INTERFACE)
-target_link_libraries(
-  ${PROJECT_NAME}
-  INTERFACE Eigen3::Eigen ${std_msgs_LIBRARIES} ${sensor_msgs_LIBRARIES}
-            ${eigen_conversions_LIBRARIES} ${geometry_msgs_LIBRARIES}
-            ${message_generation_LIBRARIES})
+add_library(${PROJECT_NAME}_conversion INTERFACE)
+target_link_libraries(${PROJECT_NAME}_conversion
+  INTERFACE Eigen3::Eigen tf2_eigen::tf2_eigen "${cpp_typesupport_target}")
 target_include_directories(
-  ${PROJECT_NAME} SYSTEM
-  INTERFACE ${std_msgs_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS}
-            ${eigen_conversions_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS}
-            ${message_generation_INCLUDE_DIRS})
-target_include_directories(
-  ${PROJECT_NAME}
+  ${PROJECT_NAME}_conversion
   INTERFACE $<INSTALL_INTERFACE:include/>
             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
 
 #
 # Unit tests
 #
 if(BUILD_TESTING)
-  catkin_add_gtest(test_eigen_conversions tests/test_eigen_conversions.cpp
-                   tests/gtest_main.cpp)
-  target_link_libraries(test_eigen_conversions ${PROJECT_NAME})
+  ament_add_gtest(test_eigen_conversions tests/test_eigen_conversions.cpp
+                  tests/gtest_main.cpp)
+  target_link_libraries(test_eigen_conversions ${PROJECT_NAME}_conversion)
 endif()
 
 #
 # Install
 #
 install(
-  TARGETS ${PROJECT_NAME}
+  TARGETS ${PROJECT_NAME}_conversion
   EXPORT ${TARGETS_EXPORT_NAME}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 install(FILES package.xml DESTINATION share/${PROJECT_NAME})
 install(
   DIRECTORY include/${PROJECT_NAME}
   DESTINATION include
   PATTERN "third_party" EXCLUDE)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_export_dependencies(eigen3_cmake_module)
+ament_export_dependencies(Eigen3)
+
+ament_package()

include/linear_feedback_controller_msgs/eigen_conversions.hpp

@@ -1,12 +1,11 @@
 #ifndef ROS_WBMPC_MSGS_ROS_EIGEN_CONVERSION_HPP
 #define ROS_WBMPC_MSGS_ROS_EIGEN_CONVERSION_HPP
 
-#include <eigen_conversions/eigen_msg.h>
-#include <linear_feedback_controller_msgs/Control.h>
-#include <linear_feedback_controller_msgs/Sensor.h>
-
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <linear_feedback_controller_msgs/msg/control.hpp>
+#include <linear_feedback_controller_msgs/msg/sensor.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 namespace linear_feedback_controller_msgs {
 
@@ -40,16 +39,48 @@ struct Control {
 };
 }  // namespace Eigen
 
+// Create an alias to use only one namespace here.
+inline void wrenchEigenToMsg(const ::Eigen::Matrix<double, 6, 1>& e,
+                             geometry_msgs::msg::Wrench& m) {
+  tf2::toMsg(e.template head<3>(), m.force);
+  tf2::toMsg(e.template tail<3>(), m.torque);
+}
+
+inline void wrenchMsgToEigen(const geometry_msgs::msg::Wrench& m,
+                             ::Eigen::Matrix<double, 6, 1>& e) {
+  e(0) = m.force.x;
+  e(1) = m.force.y;
+  e(2) = m.force.z;
+  e(3) = m.torque.x;
+  e(4) = m.torque.y;
+  e(5) = m.torque.z;
+}
+
 // Create an alias to use only one namespace here.
 template <class Derived>
-void matrixEigenToMsg(const ::Eigen::MatrixBase<Derived>& e,
-                      std_msgs::Float64MultiArray& m) {
-  tf::matrixEigenToMsg(e, m);
+inline void matrixEigenToMsg(const ::Eigen::MatrixBase<Derived>& e,
+                             std_msgs::msg::Float64MultiArray& m) {
+  // verify the input object.
+  m.layout.data_offset = 0;
+  m.layout.dim.resize(2);
+  m.layout.dim[0].label = "rows";
+  m.layout.dim[0].size = e.rows();
+  m.layout.dim[0].stride = e.size();
+  m.layout.dim[1].label = "cols";
+  m.layout.dim[1].stride = e.cols();
+  m.layout.dim[1].size = e.cols();
+  m.data.resize(m.layout.dim[0].stride, 0.0);
+
+  for (int i = 0; i < e.rows(); ++i) {
+    for (int j = 0; j < e.cols(); ++j) {
+      m.data[m.layout.data_offset + m.layout.dim[1].stride * i + j] = e(i, j);
+    }
+  }
 }
 
 template <class Derived>
-void matrixMsgToEigen(const std_msgs::Float64MultiArray& m,
-                      ::Eigen::MatrixBase<Derived>& e) {
+inline void matrixMsgToEigen(const std_msgs::msg::Float64MultiArray& m,
+                             ::Eigen::MatrixBase<Derived>& e) {
   assert(m.layout.dim.size() == 2 && "The ROS message must be a 2D matrix.");
 
   // verify the input object.
@@ -62,10 +93,9 @@ void matrixMsgToEigen(const std_msgs::Float64MultiArray& m,
   assert(m.layout.dim[1].size == e.cols() &&
          "Input and output size do not match.");
 
-  int ii = 0;
   for (int i = 0; i < e.rows(); ++i) {
     for (int j = 0; j < e.cols(); ++j) {
-      e(i, j) = m.data[ii++];
+      e(i, j) = m.data[m.layout.data_offset + m.layout.dim[1].stride * i + j];
     }
   }
 }
@@ -75,7 +105,7 @@ void matrixMsgToEigen(const std_msgs::Float64MultiArray& m,
  */
 
 inline void jointStateMsgToEigen(
-    const sensor_msgs::JointState& m,
+    const sensor_msgs::msg::JointState& m,
     linear_feedback_controller_msgs::Eigen::JointState& e) {
   e.name = m.name;
   e.position = ::Eigen::Map<const ::Eigen::VectorXd>(m.position.data(),
@@ -87,11 +117,11 @@ inline void jointStateMsgToEigen(
 }
 
 inline void contactMsgToEigen(
-    const linear_feedback_controller_msgs::Contact& m,
+    const linear_feedback_controller_msgs::msg::Contact& m,
     linear_feedback_controller_msgs::Eigen::Contact& e) {
   e.active = m.active;
   e.name = m.name;
-  tf::wrenchMsgToEigen(m.wrench, e.wrench);
+  wrenchMsgToEigen(m.wrench, e.wrench);
   e.pose(0) = m.pose.position.x;
   e.pose(1) = m.pose.position.y;
   e.pose(2) = m.pose.position.z;
@@ -102,15 +132,16 @@ inline void contactMsgToEigen(
 }
 
 inline void sensorMsgToEigen(
-    const Sensor& m, linear_feedback_controller_msgs::Eigen::Sensor& e) {
+    const linear_feedback_controller_msgs::msg::Sensor& m,
+    linear_feedback_controller_msgs::Eigen::Sensor& e) {
   e.base_pose(0) = m.base_pose.position.x;
   e.base_pose(1) = m.base_pose.position.y;
   e.base_pose(2) = m.base_pose.position.z;
   e.base_pose(3) = m.base_pose.orientation.x;
   e.base_pose(4) = m.base_pose.orientation.y;
   e.base_pose(5) = m.base_pose.orientation.z;
   e.base_pose(6) = m.base_pose.orientation.w;
-  tf::twistMsgToEigen(m.base_twist, e.base_twist);
+  tf2::fromMsg(m.base_twist, e.base_twist);
   jointStateMsgToEigen(m.joint_state, e.joint_state);
   e.contacts.resize(m.contacts.size());
   for (std::size_t i = 0; i < m.contacts.size(); ++i) {
@@ -119,7 +150,8 @@ inline void sensorMsgToEigen(
 }
 
 inline void controlMsgToEigen(
-    const Control& m, linear_feedback_controller_msgs::Eigen::Control& e) {
+    const linear_feedback_controller_msgs::msg::Control& m,
+    linear_feedback_controller_msgs::Eigen::Control& e) {
   matrixMsgToEigen(m.feedback_gain, e.feedback_gain);
   matrixMsgToEigen(m.feedforward, e.feedforward);
   sensorMsgToEigen(m.initial_state, e.initial_state);
@@ -131,7 +163,7 @@ inline void controlMsgToEigen(
 
 inline void jointStateEigenToMsg(
     const linear_feedback_controller_msgs::Eigen::JointState& e,
-    sensor_msgs::JointState& m) {
+    sensor_msgs::msg::JointState& m) {
   m.name = e.name;
   m.position = std::vector<double>(e.position.data(),
                                    e.position.data() + e.position.size());
@@ -143,10 +175,10 @@ inline void jointStateEigenToMsg(
 
 inline void contactEigenToMsg(
     const linear_feedback_controller_msgs::Eigen::Contact& e,
-    linear_feedback_controller_msgs::Contact& m) {
+    linear_feedback_controller_msgs::msg::Contact& m) {
   m.active = e.active;
   m.name = e.name;
-  tf::wrenchEigenToMsg(e.wrench, m.wrench);
+  wrenchEigenToMsg(e.wrench, m.wrench);
   m.pose.position.x = e.pose(0);
   m.pose.position.y = e.pose(1);
   m.pose.position.z = e.pose(2);
@@ -157,15 +189,16 @@ inline void contactEigenToMsg(
 }
 
 inline void sensorEigenToMsg(
-    const linear_feedback_controller_msgs::Eigen::Sensor e, Sensor& m) {
+    const linear_feedback_controller_msgs::Eigen::Sensor e,
+    linear_feedback_controller_msgs::msg::Sensor& m) {
   m.base_pose.position.x = e.base_pose(0);
   m.base_pose.position.y = e.base_pose(1);
   m.base_pose.position.z = e.base_pose(2);
   m.base_pose.orientation.x = e.base_pose(3);
   m.base_pose.orientation.y = e.base_pose(4);
   m.base_pose.orientation.z = e.base_pose(5);
   m.base_pose.orientation.w = e.base_pose(6);
-  tf::twistEigenToMsg(e.base_twist, m.base_twist);
+  m.base_twist = tf2::toMsg(e.base_twist);
   jointStateEigenToMsg(e.joint_state, m.joint_state);
   m.contacts.resize(e.contacts.size());
   for (std::size_t i = 0; i < e.contacts.size(); ++i) {
@@ -174,7 +207,8 @@ inline void sensorEigenToMsg(
 }
 
 inline void controlEigenToMsg(
-    const linear_feedback_controller_msgs::Eigen::Control& e, Control& m) {
+    const linear_feedback_controller_msgs::Eigen::Control& e,
+    linear_feedback_controller_msgs::msg::Control& m) {
   matrixEigenToMsg(e.feedback_gain, m.feedback_gain);
   matrixEigenToMsg(e.feedforward, m.feedforward);
   sensorEigenToMsg(e.initial_state, m.initial_state);

msg/Control.msg

@@ -20,7 +20,7 @@
 # Metadata
 ##########
 
-Header header
+std_msgs/Header header
 
 # Control data
 ##############

msg/Sensor.msg

@@ -20,7 +20,7 @@
 # Metadata
 ##########
 
-Header header
+std_msgs/Header header
 
 # Sensory data:
 ###############