Supply spherical coords when loading DEMs

examples/worlds/dem_moon.sdf

@@ -0,0 +1,191 @@
+<?xml version="1.0" ?>
+<!--
+
+  Demonstrates a Digital Elevation Model (DEM) heightmap of Earth's moon
+
+-->
+<sdf version="1.9">
+  <world name="dem_heightmap">
+
+    <spherical_coordinates>
+      <surface_model>MOON_SCS</surface_model>
+    </spherical_coordinates>
+
+    <!-- gravity of the moon -->
+    <gravity>0 0 -1.62</gravity>
+
+    <physics name="1ms" type="ignored">
+      <dart>
+        <!-- Heightmaps behave better with the bullet collision detector -->
+        <collision_detector>bullet</collision_detector>
+      </dart>
+    </physics>
+
+    <gui fullscreen="0">
+      <plugin filename="MinimalScene" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+
+        <engine>ogre</engine>
+        <scene>scene</scene>
+        <ambient_light>0 0 0</ambient_light>
+        <background_color>0.8 0.8 0.8</background_color>
+        <camera_pose>-11.526359 19.527456 24.174088 0 0.559400 -0.989541</camera_pose>
+      </plugin>
+
+      <!-- Plugins that add functionality to the scene -->
+      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
+        <ignition-gui>
+          <property key="state" type="string">floating</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </ignition-gui>
+      </plugin>
+      <plugin filename="GzSceneManager" name="Scene Manager">
+        <ignition-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </ignition-gui>
+      </plugin>
+      <plugin filename="InteractiveViewControl" name="Interactive view control">
+        <ignition-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </ignition-gui>
+      </plugin>
+      <plugin filename="CameraTracking" name="Camera Tracking">
+        <ignition-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </ignition-gui>
+      </plugin>
+      <plugin filename="WorldControl" name="World control">
+        <ignition-gui>
+          <title>World control</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">72</property>
+          <property type="double" key="width">121</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="left" target="left"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <play_pause>true</play_pause>
+        <step>true</step>
+        <start_paused>true</start_paused>
+        <use_event>true</use_event>
+
+      </plugin>
+
+      <plugin filename="WorldStats" name="World stats">
+        <ignition-gui>
+          <title>World stats</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">110</property>
+          <property type="double" key="width">290</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="right" target="right"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <sim_time>true</sim_time>
+        <real_time>true</real_time>
+        <real_time_factor>true</real_time_factor>
+        <iterations>true</iterations>
+      </plugin>
+
+      <plugin filename="ComponentInspector" name="Component inspector">
+        <ignition-gui>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+      </plugin>
+
+      <plugin filename="EntityTree" name="Entity tree">
+        <ignition-gui>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+      </plugin>
+    </gui>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="box">
+      <pose>0 0 25 0 0 0</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>0.16666</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.16666</iyy>
+            <iyz>0</iyz>
+            <izz>0.16666</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <visual name="box_visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>1 0 0 1</diffuse>
+            <specular>1 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <include>
+      <uri>
+        https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Moon DEM
+      </uri>
+    </include>
+
+  </world>
+</sdf>

examples/worlds/dem_volcano.sdf

@@ -8,6 +8,7 @@
   <world name="dem_heightmap">
     <physics name="1ms" type="ignored">
       <dart>
+        <!-- Heightmaps behave better with the bullet collision detector -->
         <collision_detector>bullet</collision_detector>
       </dart>
     </physics>

include/gz/sim/rendering/SceneManager.hh

@@ -118,6 +118,12 @@ inline namespace GZ_SIM_VERSION_NAMESPACE {
     /// \return World ID
     public: Entity WorldId() const;
 
+    /// \brief Set the spherical coordinates from world.
+    /// \param[in] _sphericalCoordinates SphericalCoordinates
+    /// from the world.
+    public: void SetSphericalCoordinates(
+        const math::SphericalCoordinates &_sphericalCoordinates);
+
     /// \brief Create a model
     /// \param[in] _id Unique model id
     /// \param[in] _model Model sdf dom

src/rendering/RenderUtil.cc

@@ -79,6 +79,7 @@
 #include "gz/sim/components/SegmentationCamera.hh"
 #include "gz/sim/components/SemanticLabel.hh"
 #include "gz/sim/components/SourceFilePath.hh"
+#include "gz/sim/components/SphericalCoordinates.hh"
 #include "gz/sim/components/Temperature.hh"
 #include "gz/sim/components/TemperatureRange.hh"
 #include "gz/sim/components/ThermalCamera.hh"
@@ -777,6 +778,18 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info,
   this->dataPtr->FindInertialLinks(_ecm);
   this->dataPtr->FindJointModels(_ecm);
   this->dataPtr->FindCollisionLinks(_ecm);
+
+  // Get the SphericalCoordinate object from the world
+  // and supply it to the SceneManager
+  auto worldEntity = _ecm.EntityByComponents(components::World());
+  auto sphericalCoordinatesComponent =
+    _ecm.Component<components::SphericalCoordinates>(
+        worldEntity);
+  if (sphericalCoordinatesComponent)
+  {
+    this->dataPtr->sceneManager.SetSphericalCoordinates(
+        sphericalCoordinatesComponent->Data());
+  }
 }
 
 //////////////////////////////////////////////////

src/rendering/SceneManager.cc

@@ -128,6 +128,9 @@ class gz::sim::SceneManagerPrivate
   /// also sets the time point in which the animation should be played
   public: AnimationUpdateData ActorTrajectoryAt(
       Entity _id, const std::chrono::steady_clock::duration &_time) const;
+
+  /// \brief Holds the spherical coordinates from the world.
+  public: math::SphericalCoordinates sphericalCoordinates;
 };
 
 
@@ -618,6 +621,14 @@ rendering::VisualPtr SceneManager::CreateCollision(Entity _id,
   collisionVis->SetUserData("gui-only", static_cast<bool>(true));
   return collisionVis;
 }
+
+/////////////////////////////////////////////////
+void SceneManager::SetSphericalCoordinates(
+    const math::SphericalCoordinates &_sphericalCoordinates)
+{
+  this->dataPtr->sphericalCoordinates = _sphericalCoordinates;
+}
+
 /////////////////////////////////////////////////
 rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom,
     math::Vector3d &_scale, math::Pose3d &_localPose)
@@ -726,6 +737,7 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom,
     else
     {
       auto dem = std::make_shared<common::Dem>();
+      dem->SetSphericalCoordinates(this->dataPtr->sphericalCoordinates);
       if (dem->Load(fullPath) < 0)
       {
         gzerr << "Failed to load heightmap dem data from ["

src/systems/physics/Physics.cc

@@ -134,6 +134,7 @@
 #include "gz/sim/components/Recreate.hh"
 #include "gz/sim/components/SelfCollide.hh"
 #include "gz/sim/components/SlipComplianceCmd.hh"
+#include "gz/sim/components/SphericalCoordinates.hh"
 #include "gz/sim/components/Static.hh"
 #include "gz/sim/components/ThreadPitch.hh"
 #include "gz/sim/components/World.hh"
@@ -1384,7 +1385,17 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm,
           // DEM
           else
           {
+            auto worldEntity = _ecm.EntityByComponents(components::World());
+            auto sphericalCoordinatesComponent =
+              _ecm.Component<components::SphericalCoordinates>(
+                worldEntity);
+
             auto dem = std::make_shared<common::Dem>();
+            if (sphericalCoordinatesComponent)
+            {
+              dem->SetSphericalCoordinates(
+                  sphericalCoordinatesComponent->Data());
+            }
             if (dem->Load(fullPath) < 0)
             {
               gzerr << "Failed to load heightmap dem data from ["

tutorials/spherical_coordinates.md

@@ -3,11 +3,13 @@
 Gazebo supports the use of real world latitude and longitude coordinates in its
 simulations using the
 [WGS84](https://en.wikipedia.org/wiki/World_Geodetic_System#1984_version)
-geodetic system.
+geodetic system for the Earth, and
+[MOON_SCS](https://en.wikipedia.org/wiki/Selenographic_coordinate_system)
+for the Moon.
 
 Gazebo Sim's simulation is always performed in Cartesian coordinates (good old XYZ).
 Therefore, in order to use spherical coordinates, it's necessary to project
-coordinates expressed in the `WGS84` frame to Cartesian and back.
+coordinates expressed in the `WGS84` frame (for Earth) to Cartesian and back.
 
 This tutorial will go over how to:
 
@@ -53,8 +55,12 @@ For example:
 </spherical_coordinates>
 ```
 
-At the moment, the only surface model supported is WGS84 and the only world
-frame orientation is ENU.
+Currently, the supported surface models are WGS84 for the Earth,
+MOON_SCS for the Moon, and CUSTOM_SURFACE for other celestial bodies.
+CUSTOM_SURFACE requires the equatorial and polar axes to be specified
+in meters.
+
+The only world frame currently supported is ENU.
 
 Try out an example world that ships with Gazebo and has the coordinates above
 as follows:
@@ -68,6 +74,32 @@ that the coordinates were set correctly:
 
 @image html files/spherical_coordinates/inspector.png
 
+For loading lunar DEMs, use the `MOON_SCS` surface tag:
+
+```.xml
+<spherical_coordinates>
+  <surface_model>MOON_SCS</surface_model>
+</spherical_coordinates>
+```
+
+Try the Gazebo moon example:
+
+```
+gz sim dem_moon.sdf
+```
+
+Similarly, ``CUSTOM_SURFACE`` is also accepted as a surface model.
+So equivalently, one can have the following instead of
+the ``MOON_SCS`` tag :
+
+```.xml
+<spherical_coordinates>
+  <surface_model>CUSTOM_SURFACE</surface_model>
+  <surface_axis_equatorial>1738100.0</surface_axis_equatorial>
+  <surface_axis_polar>1736000.0</surface_axis_polar>
+</spherical_coordinates>
+```
+
 ### GUI
 
 To change the world origin through the GUI, edit values within the component
@@ -147,4 +179,3 @@ gz service -s /world/spherical_coordinates/set_spherical_coordinates \
 When writing plugins, developers can use the
 `gz::sim::sphericalCoordinates` helper function to query the current
 coordinates for any entity that has a pose.
-