Merge pull request #25 from ethz-asl/feature/rename_planning_msgs

Rename planning messages, delete old ones

Author: helenol

mav_comm/package.xml

@@ -11,6 +11,7 @@
   <author>Sammy Omari</author>
   <author>Michael Burri</author>
   <author>Fadri Furrer</author>
+  <author>Helen Oleynikova</author>
 
   <license>ASL 2.0</license>
 

mav_msgs/package.xml

@@ -11,6 +11,7 @@
   <author>Sammy Omari</author>
   <author>Michael Burri</author>
   <author>Fadri Furrer</author>
+  <author>Helen Oleynikova</author>
 
   <license>ASL 2.0</license>
 

planning_msgs/CMakeLists.txt

@@ -1,34 +1,29 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(planning_msgs)
 
-find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs message_generation octomap_msgs trajectory_msgs cmake_modules)
+find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs message_generation mav_msgs trajectory_msgs cmake_modules)
 find_package(Eigen REQUIRED)
 
 include_directories(include ${catkin_INCLUDE_DIRS})
 include_directories(${Eigen_INCLUDE_DIRS})
 
 add_message_files(
   FILES
-  OctomapScan.msg
-  PlanningResponse.msg
-  WayPointArray.msg
-  WayPoint.msg
-  WaypointType.msg
-  GPSWayPoint.msg
-  GPSWayPointArray.msg
+  PointCloudWithPose.msg
+  PolynomialSegment4D.msg
+  PolynomialTrajectory4D.msg
 )
 
 add_service_files(
   FILES
   PlannerService.srv
-  Octomap.srv
 )
 
 generate_messages(
-  DEPENDENCIES geometry_msgs sensor_msgs octomap_msgs trajectory_msgs
+  DEPENDENCIES geometry_msgs sensor_msgs mav_msgs trajectory_msgs
 )
 
 catkin_package(
   INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
-  CATKIN_DEPENDS message_runtime geometry_msgs sensor_msgs octomap_msgs trajectory_msgs
+  CATKIN_DEPENDS message_runtime geometry_msgs sensor_msgs mav_msgs trajectory_msgs
 )

planning_msgs/include/planning_msgs/conversions.h

@@ -23,79 +23,83 @@
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
 
-#include "planning_msgs/WayPoint.h"
-#include "planning_msgs/WayPointArray.h"
-#include "planning_msgs/WaypointType.h"
+#include "planning_msgs/PolynomialSegment4D.h"
+#include "planning_msgs/PolynomialTrajectory4D.h"
 #include "planning_msgs/eigen_planning_msgs.h"
 
 namespace planning_msgs {
 
-/// Converts a WayPoint double array to an Eigen::VectorXd.
-inline void vectorFromMsgArray(const WayPoint::_x_type& array,
+/// Converts a PolynomialSegment double array to an Eigen::VectorXd.
+inline void vectorFromMsgArray(const PolynomialSegment4D::_x_type& array,
                                Eigen::VectorXd* x) {
   *x = Eigen::Map<const Eigen::VectorXd>(&(array[0]), array.size());
 }
 
-/// Converts an Eigen::VectorXd to a WayPoint double array.
+/// Converts an Eigen::VectorXd to a PolynomialSegment double array.
 inline void msgArrayFromVector(const Eigen::VectorXd& x,
-                               WayPoint::_x_type* array) {
+                               PolynomialSegment4D::_x_type* array) {
   array->resize(x.size());
   Eigen::Map<Eigen::VectorXd> map =
       Eigen::Map<Eigen::VectorXd>(&((*array)[0]), array->size());
   map = x;
 }
 
-/// Converts a WayPoint message to an EigenWayPoint structure.
-inline void eigenWaypointFromMsg(const WayPoint& msg, EigenWayPoint* waypoint) {
-  assert(waypoint != NULL);
+/// Converts a PolynomialSegment message to an EigenPolynomialSegment structure.
+inline void eigenPolynomialSegmentFromMsg(
+    const PolynomialSegment4D& msg, EigenPolynomialSegment* segment) {
+  assert(segment != NULL);
 
-  vectorFromMsgArray(msg.x, &(waypoint->x));
-  vectorFromMsgArray(msg.y, &(waypoint->y));
-  vectorFromMsgArray(msg.z, &(waypoint->z));
-  vectorFromMsgArray(msg.yaw, &(waypoint->yaw));
+  vectorFromMsgArray(msg.x, &(segment->x));
+  vectorFromMsgArray(msg.y, &(segment->y));
+  vectorFromMsgArray(msg.z, &(segment->z));
+  vectorFromMsgArray(msg.yaw, &(segment->yaw));
 
-  waypoint->time = msg.time;
-  waypoint->type = msg.type;
+  segment->segment_time_ns = msg.segment_time.toNSec();
+  segment->num_coeffs = msg.num_coeffs;
 }
 
-/// Converts a WayPointArray message to a EigenWayPointArray
-inline void eigenWaypointArrayFromMsg(const WayPointArray& msg,
-                                      EigenWaypointArray* waypoint_array) {
-  assert(waypoint_array != NULL);
-  waypoint_array->clear();
-  waypoint_array->reserve(msg.waypoints.size());
-  for (WayPointArray::_waypoints_type::const_iterator it =
-           msg.waypoints.begin();
-       it != msg.waypoints.end(); ++it) {
-    EigenWayPoint wp;
-    eigenWaypointFromMsg(*it, &wp);
-    waypoint_array->push_back(wp);
+/// Converts a PolynomialTrajectory message to a EigenPolynomialTrajectory
+inline void eigenPolynomialTrajectoryFromMsg(
+    const PolynomialTrajectory4D& msg,
+    EigenPolynomialTrajectory* eigen_trajectory) {
+  assert(eigen_trajectory != NULL);
+  eigen_trajectory->clear();
+  eigen_trajectory->reserve(msg.segments.size());
+  for (PolynomialTrajectory4D::_segments_type::const_iterator it =
+           msg.segments.begin();
+       it != msg.segments.end(); ++it) {
+    EigenPolynomialSegment segment;
+    eigenPolynomialSegmentFromMsg(*it, &segment);
+    eigen_trajectory->push_back(segment);
   }
 }
 
-/// Converts an EigenWayPoint to a WayPoint message. Does NOT set the header!
-inline void wayPointMsgFromEigen(const EigenWayPoint& waypoint, WayPoint* msg) {
+/// Converts an EigenPolynomialSegment to a PolynomialSegment message. Does NOT
+/// set the header!
+inline void polynomialSegmentMsgFromEigen(
+    const EigenPolynomialSegment& segment, PolynomialSegment4D* msg) {
   assert(msg != NULL);
-  msgArrayFromVector(waypoint.x, &(msg->x));
-  msgArrayFromVector(waypoint.y, &(msg->y));
-  msgArrayFromVector(waypoint.z, &(msg->z));
-  msgArrayFromVector(waypoint.yaw, &(msg->yaw));
+  msgArrayFromVector(segment.x, &(msg->x));
+  msgArrayFromVector(segment.y, &(msg->y));
+  msgArrayFromVector(segment.z, &(msg->z));
+  msgArrayFromVector(segment.yaw, &(msg->yaw));
 
-  msg->time = waypoint.time;
-  msg->type = waypoint.type;
+  msg->segment_time.fromNSec(segment.segment_time_ns);
+  msg->num_coeffs = segment.num_coeffs;
 }
 
-/// Converts an EigenWayPointArray to a WayPointArray message. Does NOT set the
-/// header!
-inline void waypointArrayMsgFromEigen(const EigenWaypointArray& waypoint_array,
-                                      WayPointArray* msg) {
+/// Converts an EigenPolynomialTrajectory to a PolynomialTrajectory message.
+/// Does NOT set the header!
+inline void polynomialTrajectoryMsgFromEigen(
+    const EigenPolynomialTrajectory& eigen_trajectory,
+    PolynomialTrajectory4D* msg) {
   assert(msg != NULL);
-  msg->waypoints.reserve(waypoint_array.size());
-  for (EigenWaypointArray::const_iterator it = waypoint_array.begin();
-       it != waypoint_array.end(); ++it) {
-    WayPoint wp;
-    wayPointMsgFromEigen(*it, &wp);
-    msg->waypoints.push_back(wp);
+  msg->segments.reserve(eigen_trajectory.size());
+  for (EigenPolynomialTrajectory::const_iterator it = eigen_trajectory.begin();
+       it != eigen_trajectory.end(); ++it) {
+    PolynomialSegment4D segment;
+    polynomialSegmentMsgFromEigen(*it, &segment);
+    msg->segments.push_back(segment);
   }
 }
 }

planning_msgs/include/planning_msgs/eigen_planning_msgs.h

@@ -24,18 +24,19 @@
 
 namespace planning_msgs {
 
-struct EigenWayPoint {
-  EigenWayPoint() : time(0.0), type(0){};
+struct EigenPolynomialSegment {
+  EigenPolynomialSegment() : segment_time_ns(0), num_coeffs(0) {};
 
   Eigen::VectorXd x;
   Eigen::VectorXd y;
   Eigen::VectorXd z;
   Eigen::VectorXd yaw;
-  double time;
-  int type;
+  uint64_t segment_time_ns;
+  int num_coeffs;
 };
 
-typedef std::vector<EigenWayPoint> EigenWaypointArray;
+typedef std::vector<EigenPolynomialSegment> EigenPolynomialTrajectory;
+
 }
 
 #endif

planning_msgs/msg/GPSWayPoint.msg

@@ -0,0 +1,7 @@
+Header header
+int32 num_coeffs        # order of the polynomial + 1, should match size of x[]
+duration segment_time   # duration of the segment
+float64[] x             # coefficients for the x-axis, INCREASING order
+float64[] y             # coefficients for the y-axis, INCREASING order
+float64[] z             # coefficients for the z-axis, INCREASING order
+float64[] yaw           # coefficients for the yaw,    INCREASING order