feat: support io states in modulo core

.devcontainer/devcontainer.json

@@ -5,7 +5,7 @@
     "dockerfile": "../Dockerfile",
     "context": "..",
     "target": "development",
-    "args": { "CL_VERSION": "v7.3.0" }
+    "args": { "CL_VERSION": "v7.4.0" }
   },
   "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros2/.devcontainer,type=bind,consistency=cached",
   "workspaceFolder": "/home/ros2/.devcontainer",

CHANGELOG.md

@@ -17,6 +17,11 @@ Release Versions:
 - feat: add mutex around step callback (#67)
 - feat: add warn log when topics are sanitized (#66)
 - feat: add modulo interface package (#73)
+- feat: support io states in modulo core (#70)
+
+### Dependencies
+
+- control-libraries 7.1.0 -> 7.4.0: #76
 
 ## 4.0.0
 

Dockerfile

@@ -1,5 +1,5 @@
 #syntax=docker/dockerfile:1.4.0
-ARG CL_VERSION=v7.3.0
+ARG CL_VERSION=v7.4.0
 ARG ROS2_VERSION=iron
 FROM ghcr.io/aica-technology/control-libraries:${CL_VERSION} as cl
 FROM ghcr.io/aica-technology/ros2-ws:${ROS2_VERSION} as base

build.sh

@@ -4,7 +4,7 @@ IMAGE_NAME=ghcr.io/aica-technology/modulo
 IMAGE_TAG=latest
 
 ROS2_VERSION=iron
-CL_VERSION=v7.3.0
+CL_VERSION=v7.4.0
 
 SSH_PORT=4440
 

source/modulo_core/CMakeLists.txt

@@ -26,8 +26,8 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(tf2_msgs REQUIRED)
 
-find_package(control_libraries 7.1.0 REQUIRED COMPONENTS state_representation)
-find_package(clproto 7.1.0 REQUIRED)
+find_package(control_libraries 7.4.0 REQUIRED COMPONENTS state_representation)
+find_package(clproto 7.4.0 REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
     ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp

source/modulo_core/include/modulo_core/translators/message_readers.hpp

@@ -15,6 +15,8 @@
 #include <sensor_msgs/msg/joint_state.hpp>
 
 #include <clproto.hpp>
+#include <state_representation/DigitalIOState.hpp>
+#include <state_representation/AnalogIOState.hpp>
 #include <state_representation/parameters/Parameter.hpp>
 #include <state_representation/space/cartesian/CartesianPose.hpp>
 #include <state_representation/space/joint/JointPositions.hpp>
@@ -255,6 +257,7 @@ inline void safe_spatial_state_conversion(
  * @param conversion_callback A callback function taking parameters of type StateT and NewStateT, where the
  * referenced StateT instance can be modified as needed from the NewStateT value
  */
+// FIXME(#77): rename this upon modulo 5.0
 template<typename StateT, typename NewStateT>
 inline void safe_joint_state_conversion(
     std::shared_ptr<state_representation::State>& state, std::shared_ptr<state_representation::State>& new_state,
@@ -277,7 +280,7 @@ inline void read_message(std::shared_ptr<state_representation::State>& state, co
   std::string tmp(message.data.begin(), message.data.end());
   std::shared_ptr<State> new_state;
   try {
-    new_state = clproto::decode<std::shared_ptr<State >>(tmp);
+    new_state = clproto::decode<std::shared_ptr<State>>(tmp);
   } catch (const std::exception& ex) {
     throw exceptions::MessageTranslationException(ex.what());
   }
@@ -291,6 +294,28 @@ inline void read_message(std::shared_ptr<state_representation::State>& state, co
         }
         break;
       }
+      case StateType::DIGITAL_IO_STATE: {
+        if (new_state->get_type() == StateType::DIGITAL_IO_STATE) {
+          safe_joint_state_conversion<DigitalIOState, DigitalIOState>(
+              state, new_state, [](DigitalIOState& a, const DigitalIOState& b) {
+                a.set_data(b.data());
+              });
+        } else {
+          safe_dynamic_cast<DigitalIOState>(state, new_state);
+        }
+        break;
+      }
+      case StateType::ANALOG_IO_STATE: {
+        if (new_state->get_type() == StateType::ANALOG_IO_STATE) {
+          safe_joint_state_conversion<AnalogIOState, AnalogIOState>(
+              state, new_state, [](AnalogIOState& a, const AnalogIOState& b) {
+                a.set_data(b.data());
+              });
+        } else {
+          safe_dynamic_cast<AnalogIOState>(state, new_state);
+        }
+        break;
+      }
       case StateType::SPATIAL_STATE: {
         std::set<StateType> spatial_state_types = {
             StateType::SPATIAL_STATE, StateType::CARTESIAN_STATE, StateType::CARTESIAN_POSE, StateType::CARTESIAN_TWIST,

source/modulo_core/test/cpp/communication/test_encoded_communication.cpp

@@ -141,6 +141,18 @@ TEST_F(EncodedCommunicationTest, SameType) {
         EXPECT_EQ(pub_state->get_reference_frame(), recv_state->get_reference_frame());
         EXPECT_TRUE(pub_state->data().isApprox(recv_state->data()));
       });
+  this->communicate<DigitalIOState, DigitalIOState>(
+      DigitalIOState::Random("this", 3), DigitalIOState("that", 3), true,
+      [](const std::shared_ptr<DigitalIOState>& pub_state, const std::shared_ptr<DigitalIOState>& recv_state) {
+        EXPECT_EQ(pub_state->get_name(), recv_state->get_name());
+        EXPECT_TRUE(pub_state->data().isApprox(recv_state->data()));
+      });
+  this->communicate<AnalogIOState, AnalogIOState>(
+      AnalogIOState::Random("this", 3), AnalogIOState("that", 3), true,
+      [](const std::shared_ptr<AnalogIOState>& pub_state, const std::shared_ptr<AnalogIOState>& recv_state) {
+        EXPECT_EQ(pub_state->get_name(), recv_state->get_name());
+        EXPECT_TRUE(pub_state->data().isApprox(recv_state->data()));
+      });
 }
 
 TEST_F(EncodedCommunicationTest, CompatibleType) {