Science data: lat/lon to cartesian

lrauv_ignition_plugins/src/ScienceSensorsSystem.cc

@@ -43,6 +43,20 @@ using namespace tethys;
 
 class tethys::ScienceSensorsSystemPrivate
 {
+  /// \brief Initialize world origin in spherical coordinates
+  public: void UpdateWorldSphericalOrigin(
+    ignition::gazebo::EntityComponentManager &_ecm);
+
+  /// \brief Convert (lat, lon, 0) to Cartesian XYZ, including shifting by
+  /// world origin
+  public: void ConvertLatLonToCart(
+    const ignition::math::Vector3d &_latLonEle,
+    ignition::math::Vector3d &_cart);
+
+  /// \brief Shift point cloud with respect to the world origin in spherical
+  /// coordinates, if available.
+  public: void ShiftDataToNewSphericalOrigin();
+
   /// \brief Reads csv file and populate various data fields
   public: void ReadData();
 
@@ -107,9 +121,29 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Indicates whether data has been loaded
   public: bool initialized {false};
 
+  /// \brief Set to true after the spherical coordinates have been initialized.
+  /// This may happen at startup if the SDF file has them hardcoded, or at
+  /// runtime when the first vehicle is spawned. Assume the coordinates are
+  /// only shifted once.
+  public: bool worldSphericalCoordsInitialized {false};
+
   /// \brief Mutex for writing to world origin association to lat/long
   public: std::mutex mtx;
 
+  /// \brief Spherical coordinates of world origin. Can change at any time.
+  public: ignition::math::SphericalCoordinates worldOriginSphericalCoords;
+
+  /// \brief World origin in spherical position (latitude, longitude,
+  /// elevation), angles in degrees
+  public: ignition::math::Vector3d worldOriginSphericalPos = {0, 0, 0};
+
+  /// \brief World origin in Cartesian coordinates converted from spherical
+  /// coordinates
+  public: ignition::math::Vector3d worldOriginCartesianCoords = {0, 0, 0};
+
+  /// \brief For conversions
+  public: ignition::math::SphericalCoordinates sphCoord;
+
   /// \brief Whether using more than one time slices of data
   public: bool multipleTimeSlices {false};
 
@@ -190,9 +224,9 @@ class tethys::ScienceSensorsSystemPrivate
   // For 2003080103_mb_l3_las_1x1km.csv
   //public: const float MINIATURE_SCALE = 0.01;
   // For 2003080103_mb_l3_las.csv
-  //public: const float MINIATURE_SCALE = 0.0001;
+  public: const float MINIATURE_SCALE = 0.0001;
   // For simple_test.csv
-  public: const float MINIATURE_SCALE = 1000.0;
+  // public: const float MINIATURE_SCALE = 1000.0;
 
   // TODO This is a workaround pending upstream Marker performance improvements.
   // \brief Performance trick. Skip depths below this z, so have memory to
@@ -437,8 +471,9 @@ void ScienceSensorsSystemPrivate::ReadData()
           continue;
         }
 
-        // TODO: Convert spherical coordinates to Cartesian
-        ignition::math::Vector3d cart = {latitude, longitude, -depth};
+        // Convert spherical coordinates to Cartesian
+        ignition::math::Vector3d cart;
+        this->ConvertLatLonToCart({latitude, longitude, -depth}, cart);
 
         // Performance trick. Scale down to see in view
         cart *= this->MINIATURE_SCALE;
@@ -453,6 +488,7 @@ void ScienceSensorsSystemPrivate::ReadData()
 
         // Gather spatial coordinates, 3 fields in the line, into point cloud
         // for indexing this time slice of data.
+        // Flip sign of z, because positive depth is negative z.
         this->timeSpaceCoords[lineTimeIdx]->push_back(
           pcl::PointXYZ(cart.X(), cart.Y(), cart.Z()));
 
@@ -487,6 +523,93 @@ void ScienceSensorsSystemPrivate::ReadData()
   this->initialized = true;
 }
 
+/////////////////////////////////////////////////
+void ScienceSensorsSystemPrivate::UpdateWorldSphericalOrigin(
+  ignition::gazebo::EntityComponentManager &_ecm)
+{
+  // Lock for changes to worldOriginSpherical* until update complete
+  std::lock_guard<std::mutex> lock(mtx);
+
+  // Data positions have not been transformed wrt world origin lat/long
+  if (!this->worldSphericalCoordsInitialized)
+  {
+    auto latLon = this->world.SphericalCoordinates(_ecm);
+    if (latLon)
+    {
+      ignwarn << "New spherical coordinates detected: "
+          << latLon.value().LatitudeReference().Degree() << ", "
+          << latLon.value().LongitudeReference().Degree() << std::endl;
+
+      this->worldOriginSphericalCoords = latLon.value();
+
+      // Extract world origin in (lat, long, elevation) from spherical coords
+      this->worldOriginSphericalPos = ignition::math::Vector3d(
+        this->worldOriginSphericalCoords.LatitudeReference().Degree(),
+        this->worldOriginSphericalCoords.LongitudeReference().Degree(),
+        // TODO look into why converting with elevation don't give depth as z
+        // Details https://github.com/osrf/lrauv/pull/70#discussion_r755679895
+        //this->worldOriginSphericalCoords.ElevationReference());
+        0);
+      // Convert spherical coordinates to Cartesian
+      this->sphCoord = ignition::math::SphericalCoordinates(
+        this->worldOriginSphericalCoords.Surface());
+      this->worldOriginCartesianCoords =
+        // TODO look into why converting with worldOriginSphericalCoords gives
+        // unexpected positions.
+        // Details https://github.com/osrf/lrauv/pull/70#discussion_r755681564
+        //this->worldOriginSphericalCoords.LocalFromSphericalPosition(
+        this->sphCoord.LocalFromSphericalPosition(
+          this->worldOriginSphericalPos);
+
+      igndbg << "Data will be transformed wrt world origin in Cartesian ("
+             << this->worldOriginCartesianCoords.X() << ", "
+             << this->worldOriginCartesianCoords.Y() << ", "
+             << this->worldOriginCartesianCoords.Z() << ")" << std::endl;
+
+      this->worldSphericalCoordsInitialized = true;
+    }
+  }
+
+  // TODO(chapulina) Shift science data to new coordinates. This logic might
+  // need to be updated.
+  // If data had been loaded before origin was updated, need to shift data
+  if (this->initialized)
+  {
+    this->ShiftDataToNewSphericalOrigin();
+  }
+}
+
+/////////////////////////////////////////////////
+void ScienceSensorsSystemPrivate::ConvertLatLonToCart(
+  const ignition::math::Vector3d &_latLonEle,
+  ignition::math::Vector3d &_cart)
+{
+  // Convert spherical coordinates to Cartesian
+  _cart = this->sphCoord.LocalFromSphericalPosition(
+    ignition::math::Vector3d(_latLonEle.X(), _latLonEle.Y(), 0));
+
+  // Shift to be relative to world origin spherical coordinates
+  _cart -= this->worldOriginCartesianCoords;
+
+  // Set depth
+  _cart.Z() = _latLonEle.Z();
+
+  /*
+  igndbg << "Data point at Cartesian ("
+         << _cart.X() << ", "
+         << _cart.Y() << ", "
+         << _cart.Z() << ")" << std::endl;
+  */
+}
+
+/////////////////////////////////////////////////
+void ScienceSensorsSystemPrivate::ShiftDataToNewSphericalOrigin()
+{
+  // Lock modifications to world origin spherical association until finish
+  // transforming data
+  std::lock_guard<std::mutex> lock(mtx);
+}
+
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::GenerateOctrees()
 {
@@ -602,6 +725,10 @@ void ScienceSensorsSystem::Configure(
   this->dataPtr->salPub = this->dataPtr->node.Advertise<
       ignition::msgs::Float_V>("/salinity");
 
+  // Initialize world origin in spherical coordinates, so data is loaded to the
+  // correct Cartesian positions.
+  this->dataPtr->UpdateWorldSphericalOrigin(_ecm);
+
   this->dataPtr->ReadData();
   this->dataPtr->GenerateOctrees();
 
@@ -614,6 +741,12 @@ void ScienceSensorsSystem::PreUpdate(
   const ignition::gazebo::UpdateInfo &,
   ignition::gazebo::EntityComponentManager &_ecm)
 {
+  // TODO: Test this logic once ShiftDataToNewSphericalOrigin() is implemented
+  if (!this->dataPtr->worldSphericalCoordsInitialized)
+  {
+    this->dataPtr->UpdateWorldSphericalOrigin(_ecm);
+  }
+
   _ecm.EachNew<ignition::gazebo::components::CustomSensor,
                ignition::gazebo::components::ParentEntity>(
     [&](const ignition::gazebo::Entity &_entity,

lrauv_ignition_plugins/src/VisualizePointCloud.cc

@@ -82,7 +82,7 @@ namespace tethys
 
     /// \brief Performance trick. Render only every other n. Increase to render
     /// fewer markers (faster performance). Recalulated in function.
-    public: int renderEvery = 1;
+    public: int renderEvery = 20;
 
     /// \brief Performance trick. Skip depths below this z, so have memory to
     /// visualize higher layers at higher resolution.
@@ -95,40 +95,42 @@ namespace tethys
     // For 2003080103_mb_l3_las_1x1km.csv
     //public: const float MINIATURE_SCALE = 0.01;
     // For 2003080103_mb_l3_las.csv
-    //public: const float MINIATURE_SCALE = 0.0001;
+    public: const float MINIATURE_SCALE = 0.0001;
     // For simple_test.csv
-    public: const float MINIATURE_SCALE = 1000.0;
+    // public: const float MINIATURE_SCALE = 1000.0;
 
     /// \brief Performance trick. Factor to multiply in calculating Marker sizes
     // For 2003080103_mb_l3_las*.csv
-    //public: float dimFactor = 0.03;
+    public: float dimFactor = 0.03;
     // For simple_test.csv
-    public: float dimFactor = 0.003;
+    // public: float dimFactor = 0.003;
 
     /// \brief Parameter to calculate Marker size in x.
     /// Performance trick. Hardcode resolution to make markers resemble voxels.
     // For 2003080103_mb_l3_las_1x1km.csv
     //public: const float RES_X = 15 * MINIATURE_SCALE;
+    //public: const float RES_Y = 22 * MINIATURE_SCALE;
+    //public: const float RES_Z = 5 * MINIATURE_SCALE;
     // For 2003080103_mb_l3_las.csv
-    //public: const float RES_X = 15;
+    public: const float RES_X = 15;
     // For simple_test.csv
-    public: const float RES_X = 0.15;
+    // public: const float RES_X = 0.15;
 
     /// \brief Parameter to calculate Marker size in y.
     // For 2003080103_mb_l3_las_1x1km.csv
     //public: const float RES_Y = 22 * MINIATURE_SCALE;
     // For 2003080103_mb_l3_las.csv
-    //public: const float RES_Y = 22;
+    public: const float RES_Y = 22;
     // For simple_test.csv
-    public: const float RES_Y = 0.22;
+    // public: const float RES_Y = 0.22;
 
     /// \brief Parameter to calculate Marker size in z.
     // For 2003080103_mb_l3_las_1x1km.csv
     //public: const float RES_Z = 5 * MINIATURE_SCALE;
     // For 2003080103_mb_l3_las.csv
-    //public: const float RES_Z = 10;
+    public: const float RES_Z = 10;
     // For simple_test.csv
-    public: const float RES_Z = 0.10;
+    // public: const float RES_Z = 0.10;
 
     /// \brief Minimum value in latest float vector
     public: float minFloatV{std::numeric_limits<float>::max()};
@@ -380,8 +382,6 @@ void VisualizePointCloud::OnFloatVService(
 //////////////////////////////////////////////////
 void VisualizePointCloud::PublishMarkers()
 {
-  IGN_PROFILE("VisualizePointCloud::PublishMarkers");
-
   if (!this->dataPtr->showing)
     return;
 

lrauv_ignition_plugins/worlds/buoyant_tethys.sdf

@@ -71,18 +71,18 @@
       <longitude_deg>-121.829647676843</longitude_deg-->
 
       <!-- For simple_test.csv -->
-      <latitude_deg>0</latitude_deg>
-      <longitude_deg>0</longitude_deg>
+      <!--latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg-->
 
       <elevation>0</elevation>
       <heading_deg>0</heading_deg>
     </spherical_coordinates>
     <plugin
       filename="ScienceSensorsSystem"
       name="tethys::ScienceSensorsSystem">
-      <!--data_path>2003080103_mb_l3_las.csv</data_path-->
+      <data_path>2003080103_mb_l3_las.csv</data_path>
       <!--data_path>2003080103_mb_l3_las_1x1km.csv</data_path-->
-      <data_path>simple_test.csv</data_path>
+      <!--data_path>simple_test.csv</data_path-->
     </plugin>
 
     <gui fullscreen="0">