39 #include <orca/math/Utils.h> 40 #include <eigen_conversions/eigen_msg.h> 41 #include <geometry_msgs/Accel.h> 48 template <
class Derived>
51 if (m.layout.dim.size() != 2)
53 ROS_ERROR_STREAM(
"Float64MultiArray must have 2 dimensions to be converted to an Eigen Vector or Matrix. This message has "<< m.layout.dim.size() <<
" dimensions.");
57 int rows = m.layout.dim[0].size;
58 int cols = m.layout.dim[1].size;
61 e = Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>>(m.data.data(), rows, cols);
69 tf::poseEigenToMsg(a, m);
81 tf::poseMsgToEigen(m, a);
92 inline void accelMsgToEigen(
const geometry_msgs::Accel &m, Eigen::Matrix<double,6,1> &e)
102 inline void accelEigenToMsg(
const Eigen::Matrix<double,6,1> &e, geometry_msgs::Accel &m)
void matrix4dEigenToPoseMsg(const Eigen::Matrix4d &e, geometry_msgs::Pose &m)
Definition: MsgUtils.h:66
void transformMsgToEigen(const geometry_msgs::Transform &t, Eigen::Matrix4d &e)
Definition: MsgUtils.h:85
Definition: RosCartesianAccelerationPID.h:42
void floatMultiArrayToEigen(std_msgs::Float64MultiArray &m, Eigen::MatrixBase< Derived > &e)
Definition: MsgUtils.h:49
void transformEigenToMsg(const Eigen::Matrix4d &t, geometry_msgs::Transform &m)
Definition: MsgUtils.h:72
void accelEigenToMsg(const Eigen::Matrix< double, 6, 1 > &e, geometry_msgs::Accel &m)
Definition: MsgUtils.h:102
void accelMsgToEigen(const geometry_msgs::Accel &m, Eigen::Matrix< double, 6, 1 > &e)
Definition: MsgUtils.h:92
void poseMsgToMatrix4dEigen(const geometry_msgs::Pose &m, Eigen::Matrix4d &e)
Definition: MsgUtils.h:78