conversions.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  *     http://www.apache.org/licenses/LICENSE-2.0
00011 
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 #ifndef PLANNING_MSGS_CONVERSIONS_H
00020 #define PLANNING_MSGS_CONVERSIONS_H
00021 
00022 #include <geometry_msgs/Point.h>
00023 #include <geometry_msgs/Quaternion.h>
00024 #include <geometry_msgs/Vector3.h>
00025 
00026 #include "planning_msgs/PolynomialSegment4D.h"
00027 #include "planning_msgs/PolynomialTrajectory4D.h"
00028 #include "planning_msgs/eigen_planning_msgs.h"
00029 
00030 namespace planning_msgs {
00031 
00033 inline void vectorFromMsgArray(const PolynomialSegment4D::_x_type& array,
00034                                Eigen::VectorXd* x) {
00035   *x = Eigen::Map<const Eigen::VectorXd>(&(array[0]), array.size());
00036 }
00037 
00039 inline void msgArrayFromVector(const Eigen::VectorXd& x,
00040                                PolynomialSegment4D::_x_type* array) {
00041   array->resize(x.size());
00042   Eigen::Map<Eigen::VectorXd> map =
00043       Eigen::Map<Eigen::VectorXd>(&((*array)[0]), array->size());
00044   map = x;
00045 }
00046 
00048 inline void eigenPolynomialSegmentFromMsg(
00049     const PolynomialSegment4D& msg, EigenPolynomialSegment* segment) {
00050   assert(segment != NULL);
00051 
00052   vectorFromMsgArray(msg.x, &(segment->x));
00053   vectorFromMsgArray(msg.y, &(segment->y));
00054   vectorFromMsgArray(msg.z, &(segment->z));
00055   vectorFromMsgArray(msg.yaw, &(segment->yaw));
00056 
00057   segment->segment_time_ns = msg.segment_time.toNSec();
00058   segment->num_coeffs = msg.num_coeffs;
00059 }
00060 
00062 inline void eigenPolynomialTrajectoryFromMsg(
00063     const PolynomialTrajectory4D& msg,
00064     EigenPolynomialTrajectory* eigen_trajectory) {
00065   assert(eigen_trajectory != NULL);
00066   eigen_trajectory->clear();
00067   eigen_trajectory->reserve(msg.segments.size());
00068   for (PolynomialTrajectory4D::_segments_type::const_iterator it =
00069            msg.segments.begin();
00070        it != msg.segments.end(); ++it) {
00071     EigenPolynomialSegment segment;
00072     eigenPolynomialSegmentFromMsg(*it, &segment);
00073     eigen_trajectory->push_back(segment);
00074   }
00075 }
00076 
00079 inline void polynomialSegmentMsgFromEigen(
00080     const EigenPolynomialSegment& segment, PolynomialSegment4D* msg) {
00081   assert(msg != NULL);
00082   msgArrayFromVector(segment.x, &(msg->x));
00083   msgArrayFromVector(segment.y, &(msg->y));
00084   msgArrayFromVector(segment.z, &(msg->z));
00085   msgArrayFromVector(segment.yaw, &(msg->yaw));
00086 
00087   msg->segment_time.fromNSec(segment.segment_time_ns);
00088   msg->num_coeffs = segment.num_coeffs;
00089 }
00090 
00093 inline void polynomialTrajectoryMsgFromEigen(
00094     const EigenPolynomialTrajectory& eigen_trajectory,
00095     PolynomialTrajectory4D* msg) {
00096   assert(msg != NULL);
00097   msg->segments.reserve(eigen_trajectory.size());
00098   for (EigenPolynomialTrajectory::const_iterator it = eigen_trajectory.begin();
00099        it != eigen_trajectory.end(); ++it) {
00100     PolynomialSegment4D segment;
00101     polynomialSegmentMsgFromEigen(*it, &segment);
00102     msg->segments.push_back(segment);
00103   }
00104 }
00105 }
00106 
00107 #endif


planning_msgs
Author(s): Simon Lynen, Markus Achtelik, Pascal Gohl, Sammy Omari, Michael Burri, Fadri Furrer, Helen Oleynikova
autogenerated on Thu Aug 17 2017 02:31:48