19 #ifndef MAV_PLANNING_MSGS_CONVERSIONS_H 20 #define MAV_PLANNING_MSGS_CONVERSIONS_H 22 #include <geometry_msgs/Point.h> 23 #include <geometry_msgs/Quaternion.h> 24 #include <geometry_msgs/Vector3.h> 26 #include "mav_planning_msgs/PolynomialSegment4D.h" 27 #include "mav_planning_msgs/PolynomialTrajectory4D.h" 35 *x = Eigen::Map<const Eigen::VectorXd>(&(array[0]), array.size());
40 PolynomialSegment4D::_x_type* array) {
41 array->resize(x.size());
42 Eigen::Map<Eigen::VectorXd> map =
43 Eigen::Map<Eigen::VectorXd>(&((*array)[0]), array->size());
50 assert(segment != NULL);
63 const PolynomialTrajectory4D& msg,
65 assert(eigen_trajectory != NULL);
66 eigen_trajectory->clear();
67 eigen_trajectory->reserve(msg.segments.size());
68 for (PolynomialTrajectory4D::_segments_type::const_iterator it =
70 it != msg.segments.end(); ++it) {
73 eigen_trajectory->push_back(segment);
80 PolynomialSegment4D* msg) {
95 PolynomialTrajectory4D* msg) {
97 msg->segments.reserve(eigen_trajectory.size());
98 for (EigenPolynomialTrajectory::const_iterator it = eigen_trajectory.begin();
99 it != eigen_trajectory.end(); ++it) {
100 PolynomialSegment4D segment;
102 msg->segments.push_back(segment);
108 #endif // MAV_PLANNING_MSGS_CONVERSIONS_H
void eigenPolynomialSegmentFromMsg(const PolynomialSegment4D &msg, EigenPolynomialSegment *segment)
Converts a PolynomialSegment message to an EigenPolynomialSegment structure.
void msgArrayFromVector(const Eigen::VectorXd &x, PolynomialSegment4D::_x_type *array)
Converts an Eigen::VectorXd to a PolynomialSegment double array.
void polynomialSegmentMsgFromEigen(const EigenPolynomialSegment &segment, PolynomialSegment4D *msg)
std::vector< EigenPolynomialSegment > EigenPolynomialTrajectory
void eigenPolynomialTrajectoryFromMsg(const PolynomialTrajectory4D &msg, EigenPolynomialTrajectory *eigen_trajectory)
Converts a PolynomialTrajectory message to a EigenPolynomialTrajectory.
void vectorFromMsgArray(const PolynomialSegment4D::_x_type &array, Eigen::VectorXd *x)
Converts a PolynomialSegment double array to an Eigen::VectorXd.
void polynomialTrajectoryMsgFromEigen(const EigenPolynomialTrajectory &eigen_trajectory, PolynomialTrajectory4D *msg)