Merge pull request gazebosim#320 from gazebosim/arjo/feat/environment…

…_data_messages Adds a message that allows loading environments via a topic
iradionov · Aug 4, 2023 · 7fc5805 · 7fc5805
2 parents c7e0323 + 97531a0
commit 7fc5805
Show file tree

Hide file tree

Showing 5 changed files with 184 additions and 0 deletions.
diff --git a/include/gz/msgs/Utility.hh b/include/gz/msgs/Utility.hh
@@ -94,6 +94,14 @@ namespace gz
     math::SphericalCoordinates Convert(
         const msgs::SphericalCoordinates &_coord);
 
+    /// \brief Convert a msgs::SphericalCoordinatesType to an
+    /// gz::math::SphericalCoordinates::CoordinateTpye
+    /// \param[in] _sc The spherical coordinate type to convert
+    /// \return A gz::math::SphericalCoordinatesType object
+    GZ_MSGS_VISIBLE
+    math::SphericalCoordinates::CoordinateType Convert(
+      const msgs::SphericalCoordinatesType &_sc);
+
     /// \brief Convert a msgs::AxisAlignedBox to an
     /// gz::math::AxisAlignedBox
     /// \param[in] _b The axis aligned box to convert
@@ -212,6 +220,14 @@ namespace gz
     msgs::SphericalCoordinates Convert(
         const math::SphericalCoordinates &_coord);
 
+    /// \brief Convert a gz::math::SphericalCoordinates::CoordinateType
+    /// to a msgs::SphericalCoordinatesType
+    /// \param[in] _coord The spherical coordinates type to convert
+    /// \return A gz::msgs::SphericalCoordinatesType object
+    GZ_MSGS_VISIBLE
+    msgs::SphericalCoordinatesType ConvertCoord(
+      const math::SphericalCoordinates::CoordinateType &_coord);
+
     /// \brief Convert a gz::math::Planed to a msgs::PlaneGeom
     /// \param[in] _p The plane to convert
     /// \return A msgs::PlaneGeom object

diff --git a/proto/gz/msgs/data_load_options.proto b/proto/gz/msgs/data_load_options.proto
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2023 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+syntax = "proto3";
+package gz.msgs;
+option java_package = "com.gz.msgs";
+option java_outer_classname = "DataLoadPathOptions";
+
+import "gz/msgs/spherical_coordinates.proto";
+
+/// \brief Used for specifying how to load environmental data
+message DataLoadPathOptions
+{
+    /// \brief Units used by spherical coordinates
+    enum DataAngularUnits
+    {
+        RADIANS = 0;
+        DEGREES = 1;
+    }
+
+    /// \brief File path to load
+    string path = 1;
+
+    /// \brief Name of time axis
+    string time = 2;
+
+    /// \brief Is the data static in time
+    bool static_time = 3;
+
+    /// \brief Name of x axis
+    string x = 4;
+
+    /// \brief Name of y axis
+    string y = 5;
+
+    /// \brief Name of z axis
+    string z = 6;
+
+    /// Units
+    DataAngularUnits units = 7;
+
+    /// Spherical Coodinate type
+    gz.msgs.SphericalCoordinatesType coordinate_type = 8;
+}
diff --git a/proto/gz/msgs/spherical_coordinates.proto b/proto/gz/msgs/spherical_coordinates.proto
@@ -27,6 +27,26 @@ option java_outer_classname = "SphericalCoordinatesProtos";
 import "gz/msgs/entity.proto";
 import "gz/msgs/header.proto";
 
+enum SphericalCoordinatesType
+{
+  /// \brief Latitude, Longitude and Altitude by SurfaceType
+  SPHERICAL = 0;
+
+  /// \brief Earth centered, earth fixed Cartesian
+  ECEF = 1;
+
+  /// \brief Local tangent plane (East, North, Up)
+  GLOBAL = 2;
+
+  /// \brief Heading-adjusted tangent plane (X, Y, Z)
+  /// This has kept a bug for backwards compatibility, use
+  /// LOCAL2 for the correct behaviour.
+  LOCAL = 3;
+
+  /// \brief Heading-adjusted tangent plane (X, Y, Z)
+  LOCAL2 = 4;
+}
+
 message SphericalCoordinates
 {
   /// \brief Planetary surface models.

diff --git a/src/Utility.cc b/src/Utility.cc
@@ -176,6 +176,28 @@ namespace gz
       return out;
     }
 
+    /////////////////////////////////////////////
+    math::SphericalCoordinates::CoordinateType Convert(
+      const msgs::SphericalCoordinatesType &_sc)
+    {
+      switch (_sc)
+      {
+        case msgs::SphericalCoordinatesType::ECEF:
+          return math::SphericalCoordinates::CoordinateType::ECEF;
+        case msgs::SphericalCoordinatesType::GLOBAL:
+          return math::SphericalCoordinates::CoordinateType::GLOBAL;
+        case msgs::SphericalCoordinatesType::SPHERICAL:
+          return math::SphericalCoordinates::CoordinateType::SPHERICAL;
+        case msgs::SphericalCoordinatesType::LOCAL:
+          return math::SphericalCoordinates::CoordinateType::LOCAL;
+        case msgs::SphericalCoordinatesType::LOCAL2:
+          return math::SphericalCoordinates::CoordinateType::LOCAL2;
+        default:
+          std::cerr << "Invalid coordinate type passed" << std::endl;
+      }
+      return math::SphericalCoordinates::CoordinateType::LOCAL2;
+    }
+
     /////////////////////////////////////////////
     math::AxisAlignedBox Convert(const msgs::AxisAlignedBox &_b)
     {
@@ -342,6 +364,34 @@ namespace gz
       return result;
     }
 
+    /////////////////////////////////////////////
+    msgs::SphericalCoordinatesType ConvertCoord(
+      const math::SphericalCoordinates::CoordinateType &_sc)
+    {
+      auto result = msgs::SphericalCoordinatesType::LOCAL2;
+      switch (_sc)
+      {
+        case math::SphericalCoordinates::CoordinateType::ECEF:
+          result = msgs::SphericalCoordinatesType::ECEF;
+          break;
+        case math::SphericalCoordinates::CoordinateType::GLOBAL:
+          result = msgs::SphericalCoordinatesType::GLOBAL;
+          break;
+        case math::SphericalCoordinates::CoordinateType::SPHERICAL:
+          result = msgs::SphericalCoordinatesType::SPHERICAL;
+          break;
+        case math::SphericalCoordinates::CoordinateType::LOCAL:
+          result = msgs::SphericalCoordinatesType::LOCAL;
+          break;
+        case math::SphericalCoordinates::CoordinateType::LOCAL2:
+          result = msgs::SphericalCoordinatesType::LOCAL2;
+          break;
+        default:
+          std::cerr << "Invalid coordinate type passed" << std::endl;
+      }
+      return result;
+    }
+
     /////////////////////////////////////////////
     msgs::PlaneGeom Convert(const gz::math::Planed &_p)
     {

diff --git a/src/Utility_TEST.cc b/src/Utility_TEST.cc
@@ -398,6 +398,46 @@ TEST(MsgsTest, ConvertMathSphericalCoordinatesToMsgs)
   EXPECT_DOUBLE_EQ(10000, mathCustom.SurfaceAxisPolar());
 }
 
+/////////////////////////////////////////////////
+TEST(MsgsTest, ConvertMsgsSphericalCoordinatesTypeToMath)
+{
+  EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::ECEF),
+    math::SphericalCoordinates::CoordinateType::ECEF);
+  EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::GLOBAL),
+    math::SphericalCoordinates::CoordinateType::GLOBAL);
+  EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::SPHERICAL),
+    math::SphericalCoordinates::CoordinateType::SPHERICAL);
+  EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::LOCAL),
+    math::SphericalCoordinates::CoordinateType::LOCAL);
+  EXPECT_EQ(Convert(msgs::SphericalCoordinatesType::LOCAL2),
+    math::SphericalCoordinates::CoordinateType::LOCAL2);
+  EXPECT_EQ(Convert((msgs::SphericalCoordinatesType)500000),
+    math::SphericalCoordinates::CoordinateType::LOCAL2);
+}
+
+/////////////////////////////////////////////////
+TEST(MsgsTest, ConvertMathSphericalCoordinatedTypeToMsg)
+{
+  EXPECT_EQ(msgs::ConvertCoord(
+      math::SphericalCoordinates::CoordinateType::ECEF),
+    msgs::SphericalCoordinatesType::ECEF);
+  EXPECT_EQ(msgs::ConvertCoord(
+      math::SphericalCoordinates::CoordinateType::GLOBAL),
+    msgs::SphericalCoordinatesType::GLOBAL);
+  EXPECT_EQ(msgs::ConvertCoord(
+      math::SphericalCoordinates::CoordinateType::SPHERICAL),
+    msgs::SphericalCoordinatesType::SPHERICAL);
+  EXPECT_EQ(msgs::ConvertCoord(
+      math::SphericalCoordinates::CoordinateType::LOCAL),
+    msgs::SphericalCoordinatesType::LOCAL);
+  EXPECT_EQ(msgs::ConvertCoord(
+      math::SphericalCoordinates::CoordinateType::LOCAL2),
+    msgs::SphericalCoordinatesType::LOCAL2);
+  EXPECT_EQ(msgs::ConvertCoord(
+    (math::SphericalCoordinates::CoordinateType)500000),
+    msgs::SphericalCoordinatesType::LOCAL2);
+}
+
 /////////////////////////////////////////////////
 TEST(UtilityTest, ConvertStringMsg)
 {