feat: update translators to include new trajectory msgs (#194)

CHANGELOG.md

@@ -33,6 +33,7 @@ Release Versions:
 - feat(controllers)!: add TF broadcaster in BaseControllerInterface (#170)
 - test(controllers): add TF listener and broadcaster tests (#172)
 - fix(controllers)!: remove input validity setter (#209)
+- feat(core): update translators to include new trajectory msgs (#194)
 
 ## 5.4.1
 
@@ -103,6 +104,11 @@ base class.
 
 - feat(components): verify return value of callbacks (#206)
 - fix(controllers): move input validity period to base (#207)
+- feat(controllers)!: remove robot description parameter (#186)
+- feat(controllers): add TF listener interface in BaseControllerInterface (#169)
+- feat(controllers): add TF broadcaster in BaseControllerInterface (#170)
+- test(controllers): add TF listener and broadcaster tests (#172)
+- feat(controllers): use parent node for tf listener (#190)
 
 ## 5.2.0
 

aica-package.toml

@@ -12,7 +12,7 @@ type = "ros"
 image = "v2.0.6-jazzy"
 
 [build.dependencies]
-"@aica/foss/control-libraries" = "v9.0.0"
+"@aica/foss/control-libraries" = "v10.0.0-rc0001"
 
 [build.packages.modulo_components]
 source = "./source/modulo_components"

source/modulo_core/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(std_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(tf2_msgs REQUIRED)
+find_package(trajectory_msgs REQUIRED)
 find_package(modulo_interfaces REQUIRED)
 
 find_package(control_libraries 9.0.0 REQUIRED COMPONENTS state_representation)
@@ -56,6 +57,7 @@ ament_target_dependencies(
   sensor_msgs
   std_msgs
   tf2_msgs
+  trajectory_msgs
   modulo_interfaces
 )
 
@@ -83,7 +85,7 @@ if(BUILD_TESTING)
   ament_add_gtest(test_modulo_core ${TEST_CPP_SOURCES})
   target_include_directories(test_modulo_core PRIVATE include)
   target_link_libraries(test_modulo_core ${PROJECT_NAME} clproto state_representation)
-  ament_target_dependencies(test_modulo_core geometry_msgs sensor_msgs std_msgs rclcpp rclcpp_lifecycle tf2_msgs)
+  ament_target_dependencies(test_modulo_core geometry_msgs sensor_msgs std_msgs rclcpp rclcpp_lifecycle tf2_msgs trajectory_msgs)
 
   # prevent pluginlib from using boost
   target_compile_definitions(test_modulo_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
@@ -105,6 +107,7 @@ ament_export_dependencies(
   rclcpp
   rclcpp_lifecycle
   tf2_msgs
+  trajectory_msgs
   modulo_interfaces
 )
 

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

@@ -13,6 +13,8 @@
 #include <std_msgs/msg/float64_multi_array.hpp>
 #include <std_msgs/msg/int32.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
 
 #include <clproto.hpp>
 #include <state_representation/AnalogIOState.hpp>
@@ -21,6 +23,8 @@
 #include <state_representation/space/Jacobian.hpp>
 #include <state_representation/space/cartesian/CartesianPose.hpp>
 #include <state_representation/space/joint/JointPositions.hpp>
+#include <state_representation/trajectory/CartesianTrajectory.hpp>
+#include <state_representation/trajectory/JointTrajectory.hpp>
 
 #include "modulo_core/EncodedState.hpp"
 #include "modulo_core/exceptions.hpp"
@@ -104,6 +108,13 @@ void read_message(state_representation::CartesianState& state, const geometry_ms
  */
 void read_message(state_representation::JointState& state, const sensor_msgs::msg::JointState& message);
 
+/**
+ * @brief Convert a ROS trajectory_msgs::msg::JointTrajectory to a JointTrajectory
+ * @param state The JointTrajectory to populate
+ * @param message The ROS message to read from
+ */
+void read_message(state_representation::JointTrajectory& state, const trajectory_msgs::msg::JointTrajectory& message);
+
 /**
  * @brief Template function to convert a ROS std_msgs::msg::T to a Parameter<T>
  * @tparam T All types of parameters supported in ROS std messages
@@ -436,6 +447,12 @@ inline void read_message(std::shared_ptr<state_representation::State>& state, co
       case StateType::JACOBIAN:
         safe_dynamic_cast<Jacobian>(state, new_state);
         break;
+      case StateType::CARTESIAN_TRAJECTORY:
+        safe_dynamic_cast<CartesianTrajectory>(state, new_state);
+        break;
+      case StateType::JOINT_TRAJECTORY:
+        safe_dynamic_cast<JointTrajectory>(state, new_state);
+        break;
       case StateType::PARAMETER: {
         auto param_ptr = std::dynamic_pointer_cast<ParameterInterface>(state);
         switch (param_ptr->get_parameter_type()) {

source/modulo_core/include/modulo_core/translators/message_writers.hpp

@@ -13,11 +13,15 @@
 #include <std_msgs/msg/int32.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
 
 #include <clproto.hpp>
 #include <state_representation/parameters/Parameter.hpp>
 #include <state_representation/space/cartesian/CartesianState.hpp>
 #include <state_representation/space/joint/JointState.hpp>
+#include <state_representation/trajectory/CartesianTrajectory.hpp>
+#include <state_representation/trajectory/JointTrajectory.hpp>
 
 #include "modulo_core/EncodedState.hpp"
 #include "modulo_core/exceptions.hpp"
@@ -150,6 +154,17 @@ void write_message(
 void write_message(
     tf2_msgs::msg::TFMessage& message, const state_representation::CartesianState& state, const rclcpp::Time& time);
 
+/**
+ * @brief Convert a CartesianTrajectory to a ROS trajectory_msgs::msg::JointTrajectory
+ * @param message The ROS message to populate
+ * @param state The state to read from
+ * @param time The time of the message
+ * @throws modulo_core::exceptions::MessageTranslationException if the provided state is empty.
+ */
+void write_message(
+    trajectory_msgs::msg::JointTrajectory& message, const state_representation::JointTrajectory& state,
+    const rclcpp::Time& time);
+
 /**
  * @brief Convert a Parameter<T> to a ROS equivalent representation
  * @tparam T All types of parameters supported in ROS std messages

source/modulo_core/modulo_core/translators/message_readers.py

@@ -1,11 +1,13 @@
 from typing import List, TypeVar, Union
 
 import clproto
+import datetime
 import geometry_msgs.msg as geometry
 import state_representation as sr
 from modulo_core import EncodedState
 from modulo_core.exceptions import MessageTranslationError
 from sensor_msgs.msg import JointState
+import trajectory_msgs.msg as trajectory
 
 DataT = TypeVar('DataT')
 MsgT = TypeVar('MsgT')
@@ -74,6 +76,26 @@ def read_message(state: StateT, message: MsgT) -> StateT:
                     state.set_torques(message.effort)
             except Exception as e:
                 raise MessageTranslationError(f"{e}")
+        elif isinstance(message, trajectory.JointTrajectory) and isinstance(state, sr.JointTrajectory):
+            try:
+                state.set_joint_names(list(message.joint_names))
+                time_from_start = 0
+                for i, point in enumerate(message.points):
+                    time_head = point.time_from_start.sec + point.time_from_start.nanosec * 1e-9
+                    duration = time_head - time_from_start
+                    time_from_start = time_head
+                    joint_state = sr.JointState(f"point_{i}", state.get_joint_names())
+                    joint_state.set_positions(point.positions)
+                    joint_state.set_velocities(point.velocities)
+                    joint_state.set_accelerations(point.accelerations)
+                    joint_state.set_torques(point.effort)
+                    state.add_point(
+                        joint_state,
+                        datetime.timedelta(seconds=duration)
+                    )
+                    state.set_name(message.header.frame_id)
+            except Exception as e:
+                raise MessageTranslationError(f"{e}")
         else:
             raise MessageTranslationError("The provided combination of state type and message type is not supported")
     except MessageTranslationError:

source/modulo_core/modulo_core/translators/message_writers.py

@@ -8,6 +8,7 @@
 from modulo_core import EncodedState
 from modulo_core.exceptions import MessageTranslationError
 from sensor_msgs.msg import JointState
+import trajectory_msgs.msg as trajectory
 
 DataT = TypeVar('DataT')
 MsgT = TypeVar('MsgT')
@@ -55,6 +56,10 @@ def get_clproto_msg_type(state: StateT) -> clproto.MessageType:
         return clproto.MessageType.DIGITAL_IO_STATE_MESSAGE
     elif state_type == sr.StateType.ANALOG_IO_STATE:
         return clproto.MessageType.ANALOG_IO_STATE_MESSAGE
+    elif state_type == sr.StateType.CARTESIAN_TRAJECTORY:
+        return clproto.MessageType.CARTESIAN_TRAJECTORY_MESSAGE
+    elif state_type == sr.StateType.JOINT_TRAJECTORY:
+        return clproto.MessageType.JOINT_TRAJECTORY_MESSAGE
     return clproto.MessageType.UNKNOWN_MESSAGE
 
 
@@ -155,10 +160,24 @@ def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time):
             write_message(message.twist, state)
         elif isinstance(message, geometry.WrenchStamped):
             write_message(message.wrench, state)
+        elif isinstance(message, trajectory.JointTrajectory) and isinstance(state, sr.JointTrajectory):
+            message.joint_names = state.get_joint_names()
+            point = trajectory.JointTrajectoryPoint()
+            for i, point in enumerate(state.get_points()):
+                ros_point = trajectory.JointTrajectoryPoint()
+                ros_point.positions = point.get_positions().tolist()
+                ros_point.velocities = point.get_velocities().tolist()
+                ros_point.accelerations = point.get_accelerations().tolist()
+                ros_point.effort = point.get_torques().tolist()
+                ros_point.time_from_start = rclpy.time.Duration(
+                    seconds=state.get_time_from_start(i).total_seconds()).to_msg()
+                message.points.append(ros_point)
+                message.header.frame_id = state.get_name()
         else:
             raise MessageTranslationError("The provided combination of state type and message type is not supported")
         message.header.stamp = time.to_msg()
-        message.header.frame_id = state.get_reference_frame()
+        if not isinstance(message, trajectory.JointTrajectory):
+            message.header.frame_id = state.get_reference_frame()
     except MessageTranslationError:
         raise
     except Exception as e:

source/modulo_core/package.xml

@@ -20,6 +20,7 @@
   <depend>std_msgs</depend>
   <depend>tf2_msgs</depend>
   <depend>modulo_interfaces</depend>
+  <depend>trajectory_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

source/modulo_core/src/translators/message_readers.cpp

@@ -1,4 +1,6 @@
 #include "modulo_core/translators/message_readers.hpp"
+#include <rclcpp/time.hpp>
+#include <stdexcept>
 
 namespace modulo_core::translators {
 
@@ -82,6 +84,41 @@ void read_message(state_representation::JointState& state, const sensor_msgs::ms
   }
 }
 
+void read_message(state_representation::JointTrajectory& state, const trajectory_msgs::msg::JointTrajectory& message) {
+  try {
+    state.set_name(message.header.frame_id);
+    if (!message.joint_names.empty()) {
+      state.set_joint_names(message.joint_names);
+    } else {
+      throw std::runtime_error("JointTrajectory message has no joint names");
+    }
+    std::chrono::nanoseconds time_from_start(0);
+    for (unsigned int i = 0; i < message.points.size(); ++i) {
+      state_representation::JointState point("point_" + std::to_string(i), state.get_joint_names());
+      if (!message.points[i].positions.empty()) {
+        point.set_positions(message.points[i].positions);
+      }
+      if (!message.points[i].velocities.empty()) {
+        point.set_velocities(message.points[i].velocities);
+      }
+      if (!message.points[i].accelerations.empty()) {
+        point.set_accelerations(message.points[i].accelerations);
+      }
+      if (!message.points[i].effort.empty()) {
+        point.set_torques(message.points[i].effort);
+      }
+      auto ros_time_from_start = std::chrono::nanoseconds(message.points[i].time_from_start.nanosec);
+      auto duration = ros_time_from_start - time_from_start;
+      time_from_start = ros_time_from_start;
+      state.add_point(point, duration);
+    }
+  } catch (const std::exception& ex) {
+    throw exceptions::MessageTranslationException(ex.what());
+  } catch (...) {
+    throw exceptions::MessageTranslationException("Unknown error while reading JointTrajectory message");
+  }
+}
+
 void read_message(bool& state, const std_msgs::msg::Bool& message) {
   state = message.data;
 }

source/modulo_core/src/translators/message_writers.cpp

@@ -1,6 +1,8 @@
 #include "modulo_core/translators/message_writers.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
 
 #include <state_representation/space/cartesian/CartesianPose.hpp>
+#include <state_representation/trajectory/JointTrajectory.hpp>
 
 using namespace state_representation;
 
@@ -127,6 +129,33 @@ void write_message(tf2_msgs::msg::TFMessage& message, const CartesianState& stat
   message.transforms.push_back(transform);
 }
 
+void write_message(
+    trajectory_msgs::msg::JointTrajectory& message, const JointTrajectory& state, const rclcpp::Time& time) {
+  if (!state) {
+    throw exceptions::MessageTranslationException(
+        state.get_name() + " state is empty while attempting to write it to message");
+  }
+  message.set__joint_names(state.get_joint_names());
+  for (unsigned int i = 0; i < state.get_size(); ++i) {
+    auto [joint_states, duration] = state[i];
+    trajectory_msgs::msg::JointTrajectoryPoint point;
+    point.positions.assign(
+        joint_states.get_positions().data(), joint_states.get_positions().data() + joint_states.get_positions().size());
+    point.velocities.assign(
+        joint_states.get_velocities().data(),
+        joint_states.get_velocities().data() + joint_states.get_velocities().size());
+    point.accelerations.assign(
+        joint_states.get_accelerations().data(),
+        joint_states.get_accelerations().data() + joint_states.get_accelerations().size());
+    point.effort.assign(
+        joint_states.get_torques().data(), joint_states.get_torques().data() + joint_states.get_torques().size());
+    point.time_from_start = rclcpp::Duration(state.get_time_from_start(i));
+    message.points.push_back(point);
+  }
+  message.header.stamp = time;
+  message.header.frame_id = state.get_name();
+}
+
 template<typename U, typename T>
 void write_message(U& message, const Parameter<T>& state, const rclcpp::Time&) {
   if (!state) {