From bf25fce752e7a8cf3b2f47a54a4b5ea5a775d1cc Mon Sep 17 00:00:00 2001
From: Timothy Wiley <timothyw@cse.unsw.edu.au>
Date: Thu, 20 Apr 2017 15:20:57 +1000
Subject: [PATCH] Convert crosbot::Point3D to inherit from ROS
 geometry_msgs::Point

---
 crosbot/include/crosbot/geometry/points.hpp | 46 +++++++++++++++------
 1 file changed, 33 insertions(+), 13 deletions(-)

diff --git a/crosbot/include/crosbot/geometry/points.hpp b/crosbot/include/crosbot/geometry/points.hpp
index 98c4faf..1afc273 100644
--- a/crosbot/include/crosbot/geometry/points.hpp
+++ b/crosbot/include/crosbot/geometry/points.hpp
@@ -157,13 +157,33 @@ inline std::string Point2D::toString() const {
 /**
  * 3D Cartesian point (x,y,z) to double (floating-point) precision
  */
+#ifdef ROS_VERSION
+struct Point3D : public geometry_msgs::Point {
+public:
+    Point3D(const double& x, const double& y, const double& z) : geometry_msgs::Point() {
+        this->x = x;
+        this->y = y;
+        this->z = z;
+    }
+
+    Point3D(const geometry_msgs::Point& p) : Point3D(p.x, p.y, p.z) {}
+    Point3D(const geometry_msgs::PointStamped& p) : Point3D(p.point.x, p.point.y, p.point.z) {}
+    Point3D(const geometry_msgs::Point32& p) : Point3D(p.x, p.y, p.z) {}
+    Point3D(const geometry_msgs::Vector3& v) : Point3D(v.x, v.y, v.z) {}
+    Point3D(const tf::Vector3& v) : Point3D(v.x(), v.y(), v.z()) {}
+
+#else
 struct Point3D {
 public:
     double x, y, z;
 
-    Point3D() : x(0), y(0), z(0) {}
     Point3D(const double& x, const double& y, const double& z) : x(x), y(y), z(z) {}
-    Point3D(const Point3D& p) : x(p.x), y(p.y), z(p.z) {}
+
+#endif
+
+    // Common constructors
+    Point3D() : Point3D(0, 0, 0) {}
+    Point3D(const Point3D& p) : Point3D(p.x, p.y, p.z) {}
 
     inline Point3D& operator=(const Point3D& p) {
         x = p.x; y = p.y; z = p.z;
@@ -270,42 +290,43 @@ public:
 
 #ifdef ROS_VERSION
 
-    Point3D(const geometry_msgs::Point& p) : x(p.x), y(p.y), z(p.z) {}
     inline Point3D& operator=(const geometry_msgs::Point& p) {
         x = p.x; y = p.y; z = p.z;
         return *this;
     }
 
-    Point3D(const geometry_msgs::PointStamped& p) : x(p.point.x), y(p.point.y), z(p.point.z) {}
     inline Point3D& operator=(const geometry_msgs::PointStamped& p) {
         x = p.point.x; y = p.point.y; z = p.point.z;
         return *this;
     }
 
-    Point3D(const geometry_msgs::Point32& p) : x(p.x), y(p.y), z(p.z) {}
     inline Point3D& operator=(const geometry_msgs::Point32& p) {
         x = p.x; y = p.y; z = p.z;
         return *this;
     }
 
-    Point3D(const geometry_msgs::Vector3& v) : x(v.x), y(v.y), z(v.z) {}
     inline Point3D& operator=(const geometry_msgs::Vector3& v) {
         x = v.x; y = v.y; z = v.z;
         return *this;
     }
 
-    Point3D(const tf::Vector3& v) : x(v.x()), y(v.y()), z(v.z()) {}
     inline Point3D& operator=(const tf::Vector3& v) {
         x = v.x(); y = v.y(); z = v.z();
         return *this;
     }
 
+    /**
+     * Legacy compatibility function - Creates a duplicate geometry_msgs::Point ROS message.
+     * Future software should just published a crosbot::Point3D.
+     */
     inline geometry_msgs::Point toROS() const {
-        geometry_msgs::Point rval;
-        rval.x = x; rval.y = y; rval.z = z;
+        geometry_msgs::Point rval = *this;
         return rval;
     }
 
+    /**
+     * Create a duplicate geometry_msgs::Point32, in float point format.
+     */
     inline geometry_msgs::Point32 toROS32() const {
         geometry_msgs::Point32 rval;
         rval.x = x; rval.y = y; rval.z = z;
@@ -333,7 +354,7 @@ inline std::string Point3D::toString() const {
 }
 
 #ifdef ROS_VERSION
-// ROS not generate an equality operators
+// ROS does not generate equality operators
 
 inline bool operator==(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) {
     return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
@@ -402,9 +423,7 @@ public:
     inline crosbot_msgs::ColouredPointMsg toROS() const {
         crosbot_msgs::ColouredPointMsg rval;
 
-        rval.p.x = point.x;
-        rval.p.y = point.y;
-        rval.p.z = point.z;
+        rval.p = point;
 
         rval.c.r = colour.r;
         rval.c.g = colour.g;
@@ -443,6 +462,7 @@ inline bool hasNAN(const crosbot_msgs::ColouredPointMsg& cp) {
 
 
 /*
+// TODO: Confirm if the message_traits are required for crosbot::Point3D
 #ifdef ROS_VERSION
 
 // TODO: Confirm that these sizes are the same as the ROS message serialization sizes
-- 
GitLab