message_writers.hpp

#pragma once
 
#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 <rclcpp/time.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 <tf2_msgs/msg/tf_message.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 "modulo_core/EncodedState.hpp"
#include "modulo_core/exceptions.hpp"
 
namespace modulo_core::translators {
 
void write_message(
    geometry_msgs::msg::Accel& message, const state_representation::CartesianState& state, const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::AccelStamped& message, const state_representation::CartesianState& state,
    const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::Pose& message, const state_representation::CartesianState& state, const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::PoseStamped& message, const state_representation::CartesianState& state,
    const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::Transform& message, const state_representation::CartesianState& state,
    const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::TransformStamped& message, const state_representation::CartesianState& state,
    const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::Twist& message, const state_representation::CartesianState& state, const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::TwistStamped& message, const state_representation::CartesianState& state,
    const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::Wrench& message, const state_representation::CartesianState& state, const rclcpp::Time& time);
 
void write_message(
    geometry_msgs::msg::WrenchStamped& message, const state_representation::CartesianState& state,
    const rclcpp::Time& time);
 
void write_message(
    sensor_msgs::msg::JointState& message, const state_representation::JointState& state, const rclcpp::Time& time);
 
void write_message(
    tf2_msgs::msg::TFMessage& message, const state_representation::CartesianState& state, const rclcpp::Time& time);
 
template<typename U, typename T>
void write_message(U& message, const state_representation::Parameter<T>& state, const rclcpp::Time&);
 
void write_message(std_msgs::msg::Bool& message, const bool& state, const rclcpp::Time& time);
 
void write_message(std_msgs::msg::Float64& message, const double& state, const rclcpp::Time& time);
 
void write_message(
    std_msgs::msg::Float64MultiArray& message, const std::vector<double>& state, const rclcpp::Time& time);
 
void write_message(std_msgs::msg::Int32& message, const int& state, const rclcpp::Time& time);
 
void write_message(std_msgs::msg::String& message, const std::string& state, const rclcpp::Time& time);
 
template<typename T>
inline void write_message(EncodedState& message, const T& state, const rclcpp::Time&) {
  try {
    std::string tmp = clproto::encode<T>(state);
    message.data = std::vector<unsigned char>(tmp.begin(), tmp.end());
  } catch (const std::exception& ex) {
    throw exceptions::MessageTranslationException(ex.what());
  }
}
}// namespace modulo_core::translators