message_readers.hpp

#pragma once
 
#include <set>
 
#include <geometry_msgs/msg/accel_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/float64.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/string.hpp>
 
#include <clproto.hpp>
#include <state_representation/AnalogIOState.hpp>
#include <state_representation/DigitalIOState.hpp>
#include <state_representation/parameters/Parameter.hpp>
#include <state_representation/space/Jacobian.hpp>
#include <state_representation/space/cartesian/CartesianPose.hpp>
#include <state_representation/space/joint/JointPositions.hpp>
 
#include "modulo_core/EncodedState.hpp"
#include "modulo_core/exceptions.hpp"
 
namespace modulo_core::translators {
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Pose& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::PoseStamped& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Transform& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::TransformStamped& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Twist& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Wrench& message);
 
void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& message);
 
void read_message(state_representation::JointState& state, const sensor_msgs::msg::JointState& message);
 
template<typename T, typename U>
void read_message(state_representation::Parameter<T>& state, const U& message) {
  state.set_value(message.data);
}
 
void read_message(bool& state, const std_msgs::msg::Bool& message);
 
void read_message(double& state, const std_msgs::msg::Float64& message);
 
void read_message(std::vector<double>& state, const std_msgs::msg::Float64MultiArray& message);
 
void read_message(int& state, const std_msgs::msg::Int32& message);
 
void read_message(std::string& state, const std_msgs::msg::String& message);
 
template<typename T>
inline void read_message(T& state, const EncodedState& message) {
  try {
    std::string tmp(message.data.begin(), message.data.end());
    state = clproto::decode<T>(tmp);
  } catch (const std::exception& ex) {
    throw exceptions::MessageTranslationException(ex.what());
  }
}
 
template<typename T>
inline std::shared_ptr<T> safe_dynamic_pointer_cast(std::shared_ptr<state_representation::State>& state) {
  auto derived_state_ptr = std::dynamic_pointer_cast<T>(state);
  if (derived_state_ptr == nullptr) {
    throw modulo_core::exceptions::MessageTranslationException(
        "Dynamic casting of state " + state->get_name() + " failed.");
  }
  return derived_state_ptr;
}
 
template<typename T>
inline void safe_dynamic_cast(
    std::shared_ptr<state_representation::State>& state, std::shared_ptr<state_representation::State>& new_state) {
  auto derived_state_ptr = safe_dynamic_pointer_cast<T>(state);
  auto received_state_ptr = safe_dynamic_pointer_cast<T>(new_state);
  *derived_state_ptr = *received_state_ptr;
}
 
template<typename StateT, typename NewStateT>
inline void safe_spatial_state_conversion(
    std::shared_ptr<state_representation::State>& state, std::shared_ptr<state_representation::State>& new_state,
    std::function<void(StateT&, const NewStateT&)> conversion_callback = {}) {
  auto derived_state_ptr = safe_dynamic_pointer_cast<StateT>(state);
  auto derived_new_state_ptr = safe_dynamic_pointer_cast<NewStateT>(new_state);
  StateT tmp_new_state;
  tmp_new_state.set_name(derived_new_state_ptr->get_name());
  tmp_new_state.set_reference_frame(derived_new_state_ptr->get_reference_frame());
  if (!derived_new_state_ptr->is_empty()) {
    if (conversion_callback) {
      conversion_callback(tmp_new_state, *derived_new_state_ptr);
    }
  }
  *derived_state_ptr = tmp_new_state;
}
 
// FIXME(#77): rename this upon modulo 5.0
template<typename StateT, typename NewStateT>
inline void safe_state_with_names_conversion(
    std::shared_ptr<state_representation::State>& state, std::shared_ptr<state_representation::State>& new_state,
    std::function<void(StateT&, const NewStateT&)> conversion_callback = {}) {
  auto derived_state_ptr = safe_dynamic_pointer_cast<StateT>(state);
  auto derived_new_state_ptr = safe_dynamic_pointer_cast<NewStateT>(new_state);
  StateT tmp_new_state(derived_new_state_ptr->get_name(), derived_new_state_ptr->get_names());
  if (!derived_new_state_ptr->is_empty()) {
    if (conversion_callback) {
      conversion_callback(tmp_new_state, *derived_new_state_ptr);
    }
  }
  *derived_state_ptr = tmp_new_state;
}
 
template<>
inline void read_message(std::shared_ptr<state_representation::State>& state, const EncodedState& message) {
  using namespace state_representation;
  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);
  } catch (const std::exception& ex) {
    throw exceptions::MessageTranslationException(ex.what());
  }
  try {
    switch (state->get_type()) {
      case StateType::STATE: {
        if (new_state->get_type() == StateType::STATE) {
          *state = *new_state;
        } else {
          *state = State(new_state->get_name());
        }
        break;
      }
      case StateType::DIGITAL_IO_STATE: {
        if (new_state->get_type() == StateType::DIGITAL_IO_STATE) {
          safe_state_with_names_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_state_with_names_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,
                                                   StateType::CARTESIAN_ACCELERATION, StateType::CARTESIAN_WRENCH};
        if (spatial_state_types.find(new_state->get_type()) != spatial_state_types.cend()) {
          safe_spatial_state_conversion<SpatialState, SpatialState>(state, new_state);
        } else {
          safe_dynamic_cast<SpatialState>(state, new_state);
        }
        break;
      }
      case StateType::CARTESIAN_STATE: {
        if (new_state->get_type() == StateType::CARTESIAN_POSE) {
          safe_spatial_state_conversion<CartesianState, CartesianPose>(
              state, new_state, [](CartesianState& a, const CartesianPose& b) { a.set_pose(b.get_pose()); });
        } else if (new_state->get_type() == StateType::CARTESIAN_TWIST) {
          safe_spatial_state_conversion<CartesianState, CartesianTwist>(
              state, new_state, [](CartesianState& a, const CartesianTwist& b) { a.set_twist(b.get_twist()); });
        } else if (new_state->get_type() == StateType::CARTESIAN_ACCELERATION) {
          safe_spatial_state_conversion<CartesianState, CartesianAcceleration>(
              state, new_state,
              [](CartesianState& a, const CartesianAcceleration& b) { a.set_acceleration(b.get_acceleration()); });
        } else if (new_state->get_type() == StateType::CARTESIAN_WRENCH) {
          safe_spatial_state_conversion<CartesianState, CartesianWrench>(
              state, new_state, [](CartesianState& a, const CartesianWrench& b) { a.set_wrench(b.get_wrench()); });
        } else {
          safe_dynamic_cast<CartesianState>(state, new_state);
        }
        break;
      }
      case StateType::CARTESIAN_POSE: {
        if (new_state->get_type() == StateType::CARTESIAN_STATE) {
          safe_spatial_state_conversion<CartesianPose, CartesianState>(
              state, new_state, [](CartesianPose& a, const CartesianState& b) { a.set_pose(b.get_pose()); });
        } else {
          safe_dynamic_cast<CartesianPose>(state, new_state);
        }
        break;
      }
      case StateType::CARTESIAN_TWIST: {
        if (new_state->get_type() == StateType::CARTESIAN_STATE) {
          safe_spatial_state_conversion<CartesianTwist, CartesianState>(
              state, new_state, [](CartesianTwist& a, const CartesianState& b) { a.set_twist(b.get_twist()); });
        } else {
          safe_dynamic_cast<CartesianTwist>(state, new_state);
        }
        break;
      }
      case StateType::CARTESIAN_ACCELERATION: {
        if (new_state->get_type() == StateType::CARTESIAN_STATE) {
          safe_spatial_state_conversion<CartesianAcceleration, CartesianState>(
              state, new_state,
              [](CartesianAcceleration& a, const CartesianState& b) { a.set_acceleration(b.get_acceleration()); });
        } else {
          safe_dynamic_cast<CartesianAcceleration>(state, new_state);
        }
        break;
      }
      case StateType::CARTESIAN_WRENCH: {
        if (new_state->get_type() == StateType::CARTESIAN_STATE) {
          safe_spatial_state_conversion<CartesianWrench, CartesianState>(
              state, new_state, [](CartesianWrench& a, const CartesianState& b) { a.set_wrench(b.get_wrench()); });
        } else {
          safe_dynamic_cast<CartesianWrench>(state, new_state);
        }
        break;
      }
      case StateType::JOINT_STATE: {
        auto derived_state = safe_dynamic_pointer_cast<JointState>(state);
        if (new_state->get_type() == StateType::JOINT_POSITIONS) {
          safe_state_with_names_conversion<JointState, JointPositions>(
              state, new_state, [](JointState& a, const JointPositions& b) { a.set_positions(b.get_positions()); });
        } else if (new_state->get_type() == StateType::JOINT_VELOCITIES) {
          safe_state_with_names_conversion<JointState, JointVelocities>(
              state, new_state, [](JointState& a, const JointVelocities& b) { a.set_velocities(b.get_velocities()); });
        } else if (new_state->get_type() == StateType::JOINT_ACCELERATIONS) {
          safe_state_with_names_conversion<JointState, JointAccelerations>(
              state, new_state,
              [](JointState& a, const JointAccelerations& b) { a.set_accelerations(b.get_accelerations()); });
        } else if (new_state->get_type() == StateType::JOINT_TORQUES) {
          safe_state_with_names_conversion<JointState, JointTorques>(
              state, new_state, [](JointState& a, const JointTorques& b) { a.set_torques(b.get_torques()); });
        } else {
          safe_dynamic_cast<JointState>(state, new_state);
        }
        break;
      }
      case StateType::JOINT_POSITIONS: {
        if (new_state->get_type() == StateType::JOINT_STATE) {
          safe_state_with_names_conversion<JointPositions, JointState>(
              state, new_state, [](JointPositions& a, const JointState& b) { a.set_positions(b.get_positions()); });
        } else {
          safe_dynamic_cast<JointPositions>(state, new_state);
        }
        break;
      }
      case StateType::JOINT_VELOCITIES: {
        if (new_state->get_type() == StateType::JOINT_STATE) {
          safe_state_with_names_conversion<JointVelocities, JointState>(
              state, new_state, [](JointVelocities& a, const JointState& b) { a.set_velocities(b.get_velocities()); });
        } else {
          safe_dynamic_cast<JointVelocities>(state, new_state);
        }
        break;
      }
      case StateType::JOINT_ACCELERATIONS: {
        if (new_state->get_type() == StateType::JOINT_STATE) {
          safe_state_with_names_conversion<JointAccelerations, JointState>(
              state, new_state,
              [](JointAccelerations& a, const JointState& b) { a.set_accelerations(b.get_accelerations()); });
        } else {
          safe_dynamic_cast<JointAccelerations>(state, new_state);
        }
        break;
      }
      case StateType::JOINT_TORQUES: {
        if (new_state->get_type() == StateType::JOINT_STATE) {
          safe_state_with_names_conversion<JointTorques, JointState>(
              state, new_state, [](JointTorques& a, const JointState& b) { a.set_torques(b.get_torques()); });
        } else {
          safe_dynamic_cast<JointTorques>(state, new_state);
        }
        break;
      }
      case StateType::JACOBIAN:
        safe_dynamic_cast<Jacobian>(state, new_state);
        break;
      case StateType::PARAMETER: {
        auto param_ptr = std::dynamic_pointer_cast<ParameterInterface>(state);
        switch (param_ptr->get_parameter_type()) {
          case ParameterType::BOOL:
            safe_dynamic_cast<Parameter<bool>>(state, new_state);
            break;
          case ParameterType::BOOL_ARRAY:
            safe_dynamic_cast<Parameter<std::vector<bool>>>(state, new_state);
            break;
          case ParameterType::INT:
            safe_dynamic_cast<Parameter<int>>(state, new_state);
            break;
          case ParameterType::INT_ARRAY:
            safe_dynamic_cast<Parameter<std::vector<int>>>(state, new_state);
            break;
          case ParameterType::DOUBLE:
            safe_dynamic_cast<Parameter<double>>(state, new_state);
            break;
          case ParameterType::DOUBLE_ARRAY:
            safe_dynamic_cast<Parameter<std::vector<double>>>(state, new_state);
            break;
          case ParameterType::STRING:
            safe_dynamic_cast<Parameter<std::string>>(state, new_state);
            break;
          case ParameterType::STRING_ARRAY:
            safe_dynamic_cast<Parameter<std::vector<std::string>>>(state, new_state);
            break;
          case ParameterType::VECTOR:
            safe_dynamic_cast<Parameter<Eigen::VectorXd>>(state, new_state);
            break;
          case ParameterType::MATRIX:
            safe_dynamic_cast<Parameter<Eigen::MatrixXd>>(state, new_state);
            break;
          default:
            throw exceptions::MessageTranslationException(
                "The ParameterType contained by parameter " + param_ptr->get_name() + " is unsupported.");
        }
        break;
      }
      default:
        throw exceptions::MessageTranslationException(
            "The StateType contained by state " + new_state->get_name() + " is unsupported.");
    }
  } catch (const exceptions::MessageTranslationException& ex) {
    throw;
  }
}
}// namespace modulo_core::translators