conversions.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
3  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
4  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11 
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  */
18 
19 #ifndef MAV_PLANNING_MSGS_CONVERSIONS_H
20 #define MAV_PLANNING_MSGS_CONVERSIONS_H
21 
22 #include <geometry_msgs/Point.h>
23 #include <geometry_msgs/Quaternion.h>
24 #include <geometry_msgs/Vector3.h>
25 
26 #include "mav_planning_msgs/PolynomialSegment.h"
27 #include "mav_planning_msgs/PolynomialTrajectory.h"
29 
30 // deprecated
32 
33 namespace mav_planning_msgs {
34 
36 inline void vectorFromMsgArray(const PolynomialSegment::_x_type& array,
37  Eigen::VectorXd* x) {
38  *x = Eigen::Map<const Eigen::VectorXd>(&(array[0]), array.size());
39 }
40 
42 inline void msgArrayFromVector(const Eigen::VectorXd& x,
43  PolynomialSegment::_x_type* array) {
44  array->resize(x.size());
45  Eigen::Map<Eigen::VectorXd> map =
46  Eigen::Map<Eigen::VectorXd>(&((*array)[0]), array->size());
47  map = x;
48 }
49 
51 inline void eigenPolynomialSegmentFromMsg(const PolynomialSegment& msg,
52  EigenPolynomialSegment* segment) {
53  assert(segment != NULL);
54 
55  vectorFromMsgArray(msg.x, &(segment->x));
56  vectorFromMsgArray(msg.y, &(segment->y));
57  vectorFromMsgArray(msg.z, &(segment->z));
58  vectorFromMsgArray(msg.yaw, &(segment->yaw));
59  vectorFromMsgArray(msg.rx, &(segment->rx));
60  vectorFromMsgArray(msg.ry, &(segment->ry));
61  vectorFromMsgArray(msg.rz, &(segment->rz));
62 
63  segment->segment_time_ns = msg.segment_time.toNSec();
64  segment->num_coeffs = msg.num_coeffs;
65 }
66 
69  const PolynomialTrajectory& msg,
70  EigenPolynomialTrajectory* eigen_trajectory) {
71  assert(eigen_trajectory != NULL);
72  eigen_trajectory->clear();
73  eigen_trajectory->reserve(msg.segments.size());
74  for (PolynomialTrajectory::_segments_type::const_iterator it =
75  msg.segments.begin();
76  it != msg.segments.end(); ++it) {
77  EigenPolynomialSegment segment;
78  eigenPolynomialSegmentFromMsg(*it, &segment);
79  eigen_trajectory->push_back(segment);
80  }
81 }
82 
86  PolynomialSegment* msg) {
87  assert(msg != NULL);
88  msgArrayFromVector(segment.x, &(msg->x));
89  msgArrayFromVector(segment.y, &(msg->y));
90  msgArrayFromVector(segment.z, &(msg->z));
91  msgArrayFromVector(segment.yaw, &(msg->yaw));
92  msgArrayFromVector(segment.rx, &(msg->rx));
93  msgArrayFromVector(segment.ry, &(msg->ry));
94  msgArrayFromVector(segment.rz, &(msg->rz));
95 
96  msg->segment_time.fromNSec(segment.segment_time_ns);
97  msg->num_coeffs = segment.num_coeffs;
98 }
99 
103  const EigenPolynomialTrajectory& eigen_trajectory,
104  PolynomialTrajectory* msg) {
105  assert(msg != NULL);
106  msg->segments.reserve(eigen_trajectory.size());
107  for (EigenPolynomialTrajectory::const_iterator it = eigen_trajectory.begin();
108  it != eigen_trajectory.end(); ++it) {
109  PolynomialSegment segment;
110  polynomialSegmentMsgFromEigen(*it, &segment);
111  msg->segments.push_back(segment);
112  }
113 }
114 
115 } // namespace mav_planning_msgs
116 
117 #endif // MAV_PLANNING_MSGS_CONVERSIONS_H
void polynomialTrajectoryMsgFromEigen(const EigenPolynomialTrajectory &eigen_trajectory, PolynomialTrajectory *msg)
Definition: conversions.h:102
void vectorFromMsgArray(const PolynomialSegment::_x_type &array, Eigen::VectorXd *x)
Converts a PolynomialSegment double array to an Eigen::VectorXd.
Definition: conversions.h:36
void eigenPolynomialSegmentFromMsg(const PolynomialSegment &msg, EigenPolynomialSegment *segment)
Converts a PolynomialSegment message to an EigenPolynomialSegment structure.
Definition: conversions.h:51
void eigenPolynomialTrajectoryFromMsg(const PolynomialTrajectory &msg, EigenPolynomialTrajectory *eigen_trajectory)
Converts a PolynomialTrajectory message to a EigenPolynomialTrajectory.
Definition: conversions.h:68
std::vector< EigenPolynomialSegment > EigenPolynomialTrajectory
void polynomialSegmentMsgFromEigen(const EigenPolynomialSegment &segment, PolynomialSegment *msg)
Definition: conversions.h:85
void msgArrayFromVector(const Eigen::VectorXd &x, PolynomialSegment::_x_type *array)
Converts an Eigen::VectorXd to a PolynomialSegment double array.
Definition: conversions.h:42


mav_planning_msgs
Author(s): Simon Lynen, Markus Achtelik, Pascal Gohl, Sammy Omari, Michael Burri, Fadri Furrer, Helen Oleynikova, Karen Bodie, Rik Bähnemann
autogenerated on Wed Dec 14 2022 03:14:22