MsgUtils.h
Go to the documentation of this file.
1 //| This file is a part of the ORCA framework.
2 //|
3 //| Copyright 2018, Fuzzy Logic Robotics
4 //| Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
5 //|
6 //| Main contributor(s): Antoine Hoarau, Ryan Lober, and
7 //| Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
8 //|
9 //| ORCA is a whole-body reactive controller framework for robotics.
10 //|
11 //| This software is governed by the CeCILL-C license under French law and
12 //| abiding by the rules of distribution of free software. You can use,
13 //| modify and/ or redistribute the software under the terms of the CeCILL-C
14 //| license as circulated by CEA, CNRS and INRIA at the following URL
15 //| "http://www.cecill.info".
16 //|
17 //| As a counterpart to the access to the source code and rights to copy,
18 //| modify and redistribute granted by the license, users are provided only
19 //| with a limited warranty and the software's author, the holder of the
20 //| economic rights, and the successive licensors have only limited
21 //| liability.
22 //|
23 //| In this respect, the user's attention is drawn to the risks associated
24 //| with loading, using, modifying and/or developing or reproducing the
25 //| software by the user in light of its specific status of free software,
26 //| that may mean that it is complicated to manipulate, and that also
27 //| therefore means that it is reserved for developers and experienced
28 //| professionals having in-depth computer knowledge. Users are therefore
29 //| encouraged to load and test the software's suitability as regards their
30 //| requirements in conditions enabling the security of their systems and/or
31 //| data to be ensured and, more generally, to use and operate it in the
32 //| same conditions as regards security.
33 //|
34 //| The fact that you are presently reading this means that you have had
35 //| knowledge of the CeCILL-C license and that you accept its terms.
36 
37 #pragma once
38 
39 #include <orca/math/Utils.h>
40 #include <eigen_conversions/eigen_msg.h>
41 #include <geometry_msgs/Accel.h>
42 
43 namespace orca_ros
44 {
45 namespace utils
46 {
47 
48  template <class Derived>
49  void floatMultiArrayToEigen(std_msgs::Float64MultiArray &m, Eigen::MatrixBase<Derived> &e)
50  {
51  if (m.layout.dim.size() != 2)
52  {
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.");
54  return;
55  }
56 
57  int rows = m.layout.dim[0].size;
58  int cols = m.layout.dim[1].size;
59 
60  // Data is stored in row major order
61  e = Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>>(m.data.data(), rows, cols);
62 
63  // e = Eigen::Map<Derived>(m.data.data(), rows, cols);
64  }
65 
66  inline void matrix4dEigenToPoseMsg(const Eigen::Matrix4d& e, geometry_msgs::Pose& m)
67  {
68  Eigen::Affine3d a(e);
69  tf::poseEigenToMsg(a, m);
70  }
71 
72  inline void transformEigenToMsg(const Eigen::Matrix4d& t, geometry_msgs::Transform& m)
73  {
74  Eigen::Affine3d a(t);
76  }
77 
78  inline void poseMsgToMatrix4dEigen(const geometry_msgs::Pose& m, Eigen::Matrix4d& e)
79  {
80  Eigen::Affine3d a;
81  tf::poseMsgToEigen(m, a);
82  e = a.matrix();
83  }
84 
85  inline void transformMsgToEigen(const geometry_msgs::Transform& t, Eigen::Matrix4d& e)
86  {
87  Eigen::Affine3d a;
89  e = a.matrix();
90  }
91 
92  inline void accelMsgToEigen(const geometry_msgs::Accel &m, Eigen::Matrix<double,6,1> &e)
93  {
94  e[0] = m.linear.x;
95  e[1] = m.linear.y;
96  e[2] = m.linear.z;
97  e[3] = m.angular.x;
98  e[4] = m.angular.y;
99  e[5] = m.angular.z;
100  }
101 
102  inline void accelEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Accel &m)
103  {
104  m.linear.x = e[0];
105  m.linear.y = e[1];
106  m.linear.z = e[2];
107  m.angular.x = e[3];
108  m.angular.y = e[4];
109  m.angular.z = e[5];
110  }
111 
112 }//namespace orca_ros
113 }//namespace utils
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