From 5ec046068ecb1f4bf09248bea5c02fd57a278b18 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Tue, 2 Nov 2021 00:32:54 -0700
Subject: [PATCH 01/12] Align lat long in science data visualization with
 spawned vehicle lat long

---
 .../src/ScienceSensorsSystem.cc               | 376 ++++++++++++++----
 .../src/VisualizePointCloud.cc                | 348 +++++++++++++---
 .../src/VisualizePointCloud.hh                |  11 +-
 .../src/VisualizePointCloud.qml               |   1 -
 lrauv_ignition_plugins/src/WorldCommPlugin.cc |  33 +-
 lrauv_ignition_plugins/src/WorldCommPlugin.hh |  12 +
 .../worlds/buoyant_tethys.sdf                 |  32 +-
 .../worlds/empty_environment.sdf              |  10 +-
 8 files changed, 694 insertions(+), 129 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index df5674d0..ee688b95 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -24,6 +24,7 @@
 
 #include <ignition/plugin/Register.hh>
 #include <ignition/common/SystemPaths.hh>
+#include <ignition/math/SphericalCoordinates.hh>
 #include <ignition/msgs/Utility.hh>
 #include <ignition/transport/Node.hh>
 
@@ -38,6 +39,10 @@ using namespace tethys;
 
 class tethys::ScienceSensorsSystemPrivate
 {
+  /// \brief Shift point cloud with respect to the world origin in spherical
+  /// coordinates, if available.
+  public: void GetWorldOriginSphericalCoords();
+
   /// \brief Reads csv file and populate various data fields
   public: void ReadData();
 
@@ -47,7 +52,9 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Publish the latest point cloud
   public: void PublishData();
 
-  /// \brief Service that provides the latest science data
+  /// \brief Service callback for a point cloud with the latest science data.
+  /// \param[in] _res Point cloud to return
+  /// \return True
   public: bool ScienceDataService(ignition::msgs::PointCloudPacked &_res);
 
   /// \brief Returns a point cloud message populated with the latest sensor data
@@ -93,6 +100,11 @@ class tethys::ScienceSensorsSystemPrivate
   public: const std::string NORTH_CURRENT {
     "northward_sea_water_velocity_meter_per_sec"};
 
+  /// \brief Service name for getting the spherical coordinates associated with
+  /// the world origin
+  public: std::string getWorldOriginSphericalService
+    {"/world_origin_spherical"};
+
   /// \brief Input data file name, relative to a path Ignition can find in its
   /// environment variables.
   public: std::string dataPath {"2003080103_mb_l3_las.csv"};
@@ -100,6 +112,12 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Indicates whether data has been loaded
   public: bool initialized = false;
 
+  /// World origin in spherical coordinates (latitude, longitude, elevation)
+  public: ignition::math::Vector3d worldOriginSphericalCoords = {0, 0, 0};
+
+  /// World origin in Cartesian coordinates converted from spherical coordinates
+  public: ignition::math::Vector3d worldOriginEarthCartesianCoords = {0, 0, 0};
+
   /// \brief Whether using more than one time slices of data
   public: bool multipleTimeSlices = false;
 
@@ -109,6 +127,10 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Timestamps to index slices of data
   public: std::vector<float> timestamps;
 
+  /// \brief Surface type, for converting spherical coordinates to Cartesian
+  public: ignition::math::SphericalCoordinates::SurfaceType surfaceType =
+    ignition::math::SphericalCoordinates::EARTH_WGS84;
+
   /// \brief Spatial coordinates of data
   /// Vector size: number of time slices. Indices correspond to those of
   /// this->timestamps.
@@ -140,13 +162,38 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Octree for data search based on spatial location of sensor. One
   /// octree per point cloud in timeSpaceCoords.
   public: std::vector<pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>>
-            spatialOctrees;
+    spatialOctrees;
 
-  /// \brief Node for communication
+  /// \brief Transport node
   public: ignition::transport::Node node;
 
-  /// \brief Publisher for point cloud data
+  /// \brief Publisher for point clouds representing positions for science data
   public: ignition::transport::Node::Publisher cloudPub;
+
+  // TODO TEMPORARY HACK publish float arrays instead of point cloud fields
+  public: ignition::transport::Node::Publisher tempPub;
+  public: ignition::transport::Node::Publisher chlorPub;
+  public: ignition::transport::Node::Publisher salPub;
+  public: ignition::msgs::Float_V tempMsg;
+  public: ignition::msgs::Float_V chlorMsg;
+  public: ignition::msgs::Float_V salMsg;
+
+  // TODO TEMPORARY HACK
+  /// \brief Publish a few more times for visualization plugin to get them
+  public: int repeatPubTimes = 1;
+
+  // TODO TEMPORARY HACK scale down to see in view to skip orbit tool limits
+  // 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;
+
+  // TODO TEMPORARY HACK Skip depths below this z, so have memory to
+  // visualize higher layers at higher resolution.
+  // This is only for visualization, so that MAX_PTS_VIS can calculate close
+  // to the actual number of points visualized.
+  // Sensors shouldn't use this.
+  public: const float SKIP_Z_BELOW = -20;
 };
 
 /////////////////////////////////////////////////
@@ -228,6 +275,9 @@ void ScienceSensorsSystemPrivate::ReadData()
 
   std::stringstream ss(line);
 
+  // Spherical coordinates object for Cartesian conversions
+  ignition::math::SphericalCoordinates sc(this->surfaceType);
+
   // Tokenize header line into columns
   while (std::getline(ss, word, ','))
   {
@@ -371,10 +421,33 @@ void ScienceSensorsSystemPrivate::ReadData()
       // Check validity of spatial coordinates
       if (!std::isnan(latitude) && !std::isnan(longitude) && !std::isnan(depth))
       {
+        // TODO TEMPORARY HACK skip points below a certain depth
+        if (-depth < this->SKIP_Z_BELOW)
+        {
+          continue;
+        }
+
+        // Convert spherical coordinates to Cartesian
+        ignition::math::Vector3d cart = sc.LocalFromSphericalPosition(
+          {latitude, longitude, 0});
+
+        // Shift to be relative to world origin spherical coordinates
+        cart -= this->worldOriginEarthCartesianCoords;
+
+        // TODO TEMPORARY HACK scale down to see in view
+        cart *= this->MINIATURE_SCALE;
+
+        // TODO TEMPORARY HACK skip points beyond some distance from origin
+        if (abs(cart.X()) > 1000 || abs(cart.Y()) > 1000)
+        {
+          continue;
+        }
+
         // 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(latitude, longitude, depth));
+          pcl::PointXYZ(cart.X(), cart.Y(), -depth));
 
         // Populate science data
         this->temperatureArr[lineTimeIdx].push_back(temp);
@@ -407,6 +480,81 @@ void ScienceSensorsSystemPrivate::ReadData()
   this->initialized = true;
 }
 
+/////////////////////////////////////////////////
+void ScienceSensorsSystemPrivate::GetWorldOriginSphericalCoords()
+{
+  // TODO TEMPORARY HACK hard code lat long.
+  // When have time, init default value from SDF <spherical_coordinates> tag
+  this->worldOriginSphericalCoords = ignition::math::Vector3d(
+    36.8024781413352, -121.829647676843, 0);
+  // Convert spherical coordinates to Cartesian
+  ignition::math::SphericalCoordinates sc(this->surfaceType);
+  this->worldOriginEarthCartesianCoords = sc.LocalFromSphericalPosition(
+    this->worldOriginSphericalCoords);
+
+  /*
+  ignition::msgs::Vector3d res;
+  bool success;
+
+  ignmsg << "Waiting for " << this->getWorldOriginSphericalService << "..."
+    << std::endl;
+
+  // Get world origin in spherical coordinates, if available.
+  if (!this->node.Request(this->getWorldOriginSphericalService, 5000, res,
+    success))
+  {
+    ignerr << "Failed to request service ["
+      << this->getWorldOriginSphericalService << "]" << std::endl;
+  }
+
+  // If unsuccessful, a first vehicle hasn't been spawned, no long/lat info
+  if (!success)
+  {
+    ignwarn << "No long/lat information. "
+      << "Has a vehicle been spawned using the WorldCommPlugin service? "
+      << "Science data will be loaded without correcting for long/lat at world "
+      << "origin."
+      << std::endl;
+  }
+  else
+  {
+    this->worldOriginSphericalCoords = ignition::math::Vector3d(
+      res.x(), res.y(), res.z());
+
+    // Convert spherical coordinates to Cartesian
+    ignition::math::SphericalCoordinates sc(this->surfaceType);
+    this->worldOriginEarthCartesianCoords = sc.LocalFromSphericalPosition(
+      this->worldOriginSphericalCoords);
+  }
+  */
+}
+
+
+/*
+  if (!this->node.Request(this->getWorldOriginSphericalService, 5000,
+      &ScienceSensorsSystemPrivate::WorldOriginSphericalCoordsCb(), this))
+  {
+    ignerr << "Failed to request service ["
+      << this->getWorldOriginSphericalService << "]" << std::endl;
+  }
+}
+
+/////////////////////////////////////////////////
+void ScienceSensorsSystemPrivate::WorldOriginSphericalCoordsCb(
+  const ignition::msgs::Vector3d &_res,
+  const bool _success)
+{
+  if (_success)
+  {
+    this->worldOriginSphericalCoords = _res;
+
+    // Convert spherical coordinates to Cartesian
+    this->worldOriginEarthCartesianCoords = this->sc.LocalFromSphericalPosition(
+      this->worldOriginSphericalCoords);
+  }
+}
+*/
+
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::GenerateOctrees()
 {
@@ -426,6 +574,9 @@ void ScienceSensorsSystemPrivate::GenerateOctrees()
 void ScienceSensorsSystemPrivate::PublishData()
 {
   this->cloudPub.Publish(this->PointCloudMsg());
+  this->tempPub.Publish(this->tempMsg);
+  this->chlorPub.Publish(this->chlorMsg);
+  this->salPub.Publish(this->salMsg);
 }
 
 /////////////////////////////////////////////////
@@ -503,11 +654,25 @@ void ScienceSensorsSystem::Configure(
   this->dataPtr->cloudPub = this->dataPtr->node.Advertise<
       ignition::msgs::PointCloudPacked>("/science_data");
 
-  this->dataPtr->node.Advertise("/science_data",
+  this->dataPtr->node.Advertise("/science_data_srv",
       &ScienceSensorsSystemPrivate::ScienceDataService, this->dataPtr.get());
 
+  // TODO TEMPORARY HACK publish float array instead of point cloud fields
+  this->dataPtr->tempPub = this->dataPtr->node.Advertise<
+      ignition::msgs::Float_V>("/temperature");
+  this->dataPtr->chlorPub = this->dataPtr->node.Advertise<
+      ignition::msgs::Float_V>("/chlorophyll");
+  this->dataPtr->salPub = this->dataPtr->node.Advertise<
+      ignition::msgs::Float_V>("/salinity");
+
+  // TODO: Offer a way to shift after ReadData(), in case the first vehicle is
+  // spawned much later than when ScienceSensorsSystem plugin is loaded.
+  this->dataPtr->GetWorldOriginSphericalCoords();
+
   this->dataPtr->ReadData();
   this->dataPtr->GenerateOctrees();
+
+  // Publish science data at the initial timestamp
   this->dataPtr->PublishData();
 }
 
@@ -550,16 +715,38 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
       {
         // Increment for next point in time
         this->dataPtr->timeIdx++;
+
+        // Publish science data at the next timestamp
         this->dataPtr->PublishData();
       }
     }
 
+    // TODO TEMPORARY DEBUG publishing more frequently to help debugging.
+    // Publish every n iters so that VisualizePointCloud plugin gets it.
+    if (this->dataPtr->repeatPubTimes % 10000 == 0)
+    {
+      this->dataPtr->PublishData();
+      // Reset
+      this->dataPtr->repeatPubTimes = 1;
+    }
+    else
+    {
+      this->dataPtr->repeatPubTimes++;
+    }
+
     // For each sensor, get its pose, search in the octree for the closest
     // neighbors, and interpolate to get approximate data at this sensor pose.
     for (auto &[entity, sensor] : this->entitySensorMap)
     {
       // Sensor pose in lat/lon, used to search for data by spatial coordinates
+      // TODO convert to Cartesian
       auto sensorLatLon = ignition::gazebo::sphericalCoordinates(entity, _ecm);
+      /*
+      // TODO TEMP DEBUG what is the sensor attached to?? World? Not robot?
+      ignerr << "sensor lat long: "
+             << sensorLatLon.value().X() << ", " << sensorLatLon.value().Y()
+             << std::endl;
+      */
       if (!sensorLatLon)
       {
         static std::unordered_set<ignition::gazebo::Entity> warnedEntities;
@@ -586,76 +773,80 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
       std::vector<float> spatialSqrDist;
 
       // Search in octree to find spatial index of science data
-      if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].nearestKSearch(
-        searchPoint, k, spatialIdx, spatialSqrDist) <= 0)
-      {
-        ignwarn << "No data found near sensor location " << sensorLatLon.value()
-                << std::endl;
-        continue;
-      }
-      // Debug output
-      /*
-      else
+      if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].getLeafCount()
+        > 0)
       {
-        igndbg << "kNN search for sensor pose (" << sensorPose.X() << ", "
-               << sensorPose.Y() << ", " << sensorPose.Z() << "):"
-               << std::endl;
-
-        for (std::size_t i = 0; i < spatialIdx.size(); i++)
+        if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].nearestKSearch(
+          searchPoint, k, spatialIdx, spatialSqrDist) <= 0)
         {
-          // Index the point cloud at the current time slice
-          pcl::PointXYZ nbrPt = (*(this->dataPtr->timeSpaceCoords[
-            this->dataPtr->timeIdx]))[spatialIdx[i]];
-
-          igndbg << "Neighbor at (" << nbrPt.x << ", " << nbrPt.y << ", "
-                 << nbrPt.z << "), squared distance " << spatialSqrDist[i]
-                 << " m" << std::endl;
+          ignwarn << "No data found near sensor location " << sensorLatLon.value()
+                  << std::endl;
+          continue;
         }
-      }
-      */
+        // Debug output
+        /*
+        else
+        {
+          igndbg << "kNN search for sensor pose (" << sensorPose.X() << ", "
+                 << sensorPose.Y() << ", " << sensorPose.Z() << "):"
+                 << std::endl;
+
+          for (std::size_t i = 0; i < spatialIdx.size(); i++)
+          {
+            // Index the point cloud at the current time slice
+            pcl::PointXYZ nbrPt = (*(this->dataPtr->timeSpaceCoords[
+              this->dataPtr->timeIdx]))[spatialIdx[i]];
+
+            igndbg << "Neighbor at (" << nbrPt.x << ", " << nbrPt.y << ", "
+                   << nbrPt.z << "), squared distance " << spatialSqrDist[i]
+                   << " m" << std::endl;
+          }
+        }
+        */
 
-      // For the correct sensor, grab closest neighbors and interpolate
-      if (auto casted = std::dynamic_pointer_cast<SalinitySensor>(sensor))
-      {
-        float sal = this->dataPtr->InterpolateData(
-          this->dataPtr->salinityArr[this->dataPtr->timeIdx], spatialIdx,
-          spatialSqrDist);
-        casted->SetData(sal);
-      }
-      else if (auto casted = std::dynamic_pointer_cast<TemperatureSensor>(
-        sensor))
-      {
-        float temp = this->dataPtr->InterpolateData(
-          this->dataPtr->temperatureArr[this->dataPtr->timeIdx], spatialIdx,
-          spatialSqrDist);
+        // For the correct sensor, grab closest neighbors and interpolate
+        if (auto casted = std::dynamic_pointer_cast<SalinitySensor>(sensor))
+        {
+          float sal = this->dataPtr->InterpolateData(
+            this->dataPtr->salinityArr[this->dataPtr->timeIdx], spatialIdx,
+            spatialSqrDist);
+          casted->SetData(sal);
+        }
+        else if (auto casted = std::dynamic_pointer_cast<TemperatureSensor>(
+          sensor))
+        {
+          float temp = this->dataPtr->InterpolateData(
+            this->dataPtr->temperatureArr[this->dataPtr->timeIdx], spatialIdx,
+            spatialSqrDist);
 
-        ignition::math::Temperature tempC;
-        tempC.SetCelsius(temp);
-        casted->SetData(tempC);
-      }
-      else if (auto casted = std::dynamic_pointer_cast<ChlorophyllSensor>(
-        sensor))
-      {
-        float chlor = this->dataPtr->InterpolateData(
-          this->dataPtr->chlorophyllArr[this->dataPtr->timeIdx], spatialIdx,
-          spatialSqrDist);
-        casted->SetData(chlor);
-      }
-      else if (auto casted = std::dynamic_pointer_cast<CurrentSensor>(sensor))
-      {
-        float eCurr = this->dataPtr->InterpolateData(
-          this->dataPtr->eastCurrentArr[this->dataPtr->timeIdx], spatialIdx,
-          spatialSqrDist);
-        float nCurr = this->dataPtr->InterpolateData(
-          this->dataPtr->northCurrentArr[this->dataPtr->timeIdx], spatialIdx,
-          spatialSqrDist);
-
-        auto curr = ignition::math::Vector3d(eCurr, nCurr, 0.0);
-        casted->SetData(curr);
-      }
-      else
-      {
-        ignerr << "Unsupported sensor type, failed to set data" << std::endl;
+          ignition::math::Temperature tempC;
+          tempC.SetCelsius(temp);
+          casted->SetData(tempC);
+        }
+        else if (auto casted = std::dynamic_pointer_cast<ChlorophyllSensor>(
+          sensor))
+        {
+          float chlor = this->dataPtr->InterpolateData(
+            this->dataPtr->chlorophyllArr[this->dataPtr->timeIdx], spatialIdx,
+            spatialSqrDist);
+          casted->SetData(chlor);
+        }
+        else if (auto casted = std::dynamic_pointer_cast<CurrentSensor>(sensor))
+        {
+          float eCurr = this->dataPtr->InterpolateData(
+            this->dataPtr->eastCurrentArr[this->dataPtr->timeIdx], spatialIdx,
+            spatialSqrDist);
+          float nCurr = this->dataPtr->InterpolateData(
+            this->dataPtr->northCurrentArr[this->dataPtr->timeIdx], spatialIdx,
+            spatialSqrDist);
+
+          auto curr = ignition::math::Vector3d(eCurr, nCurr, 0.0);
+          casted->SetData(curr);
+        }
+        else
+        {
+          ignerr << "Unsupported sensor type, failed to set data" << std::endl;
+        }
       }
       sensor->Update(_info.simTime, false);
     }
@@ -710,12 +901,14 @@ ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg()
 
   ignition::msgs::InitPointCloudPacked(msg, "world", true,
     {
-      {"xyz", ignition::msgs::PointCloudPacked::Field::FLOAT32}
-      // {"salinity", ignition::msgs::PointCloudPacked::Field::FLOAT32}
-      // TODO(louise) Add more fields
-      // https://github.com/osrf/lrauv/issues/85
+      {"xyz", ignition::msgs::PointCloudPacked::Field::FLOAT32},
     });
 
+  // TODO optimization for visualization:
+  // Use PCL methods to chop off points beyond some distance from sensor
+  // pose. Don't need to visualize beyond that. Might want to put that on a
+  // different topic specifically for visualization.
+
   msg.mutable_header()->mutable_stamp()->set_sec(this->timestamps[this->timeIdx]);
 
   pcl::PCLPointCloud2 pclPC2;
@@ -730,8 +923,41 @@ ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg()
 
   // FIXME: Include more than position data
   msg.mutable_data()->resize(pclPC2.data.size());
+  //  + temperatureArr[this->timeIdx].size());
   memcpy(msg.mutable_data()->data(), pclPC2.data.data(), pclPC2.data.size());
 
+  /*
+  // Interleaf fields to each point
+  for (int c = 0; c < pclPC2.width; ++c)
+  {
+    for (int r = 0; r < pclPC2.height; ++r)
+    {
+      memcpy(msg.mutable_data()->data() + ,
+        pclPC2.data.data() + ,
+        pclPC2.data.size());
+
+
+    }
+  }
+  */
+
+  // Wrong layout
+  // Temperature field, offset from beginning by previous data's size
+  //memcpy(msg.mutable_data()->data() + pclPC2.data.size(),
+  //  temperatureArr[this->timeIdx].data(), temperatureArr[this->timeIdx].size());
+
+  // TODO TEMPORARY HACK publish float arrays instead of point cloud fields
+  //this->tempMsg.mutable_data()->Resize(temperatureArr[this->timeIdx].size(), 0.0f);
+  //memcpy(this->tempMsg.mutable_data(),
+  //  temperatureArr[this->timeIdx].data(), temperatureArr[this->timeIdx].size());
+  // TODO did memory leak start after I started publishing float array?
+  *this->tempMsg.mutable_data() = {temperatureArr[this->timeIdx].begin(),
+    temperatureArr[this->timeIdx].end()};
+  *this->chlorMsg.mutable_data() = {chlorophyllArr[this->timeIdx].begin(),
+    chlorophyllArr[this->timeIdx].end()};
+  *this->salMsg.mutable_data() = {salinityArr[this->timeIdx].begin(),
+    salinityArr[this->timeIdx].end()};
+
   return msg;
 }
 
diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
index 5077180c..188785d6 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
@@ -51,17 +51,59 @@ namespace tethys
     /// \brief Transport node
     public: ignition::transport::Node node;
 
-    /// \brief Topic name to subscribe
+    /// \brief Science data type-specific topic name to subscribe
     public: std::string topicName{""};
 
-    /// \brief List of topics publishing LaserScan messages.
+    /// \brief List of science data topics
     public: QStringList topicList;
 
     /// \brief Protect variables changed from transport and the user
     public: std::recursive_mutex mutex;
 
-    /// \brief Latest point cloud message
+    /// \brief Generic point cloud topic name
+    public: std::string pcTopic = {"/science_data"};
+
+    /// \brief Generic point cloud service name
+    public: std::string pcSrv = {"/science_data_srv"};
+
+    /// \brief Point cloud message containing location of data
     public: ignition::msgs::PointCloudPacked pcMsg;
+
+    /// \brief Temperature mssage to visualize
+    // TODO TEMPORARY HACK 1D array instead of point cloud fields
+    public: ignition::msgs::Float_V tempMsg;
+    public: ignition::msgs::Float_V chlorMsg;
+    public: ignition::msgs::Float_V salMsg;
+    //public: ignition::msgs::Float_V valMsg;
+
+    // Cap of how many points to visualize, to save memory
+    public: const int MAX_PTS_VIS = 1000;
+
+    // TODO TEMPORARY HACK
+    // Render only every other n, to save performance. Increase to render fewer
+    // markers (faster performance).
+    public: int renderEvery = 20;
+
+    // TODO TEMPORARY HACK Skip depths below this z, so have memory to
+    // visualize higher layers at higher resolution.
+    public: const float SKIP_Z_BELOW = -40;
+
+    // TODO TEMPORARY HACK scale down to see in view to skip orbit tool limits
+    // 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: float dimFactor = 0.03;
+
+    // TODO TEMPORARY HACK 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_Y = 22;
+    public: const float RES_Z = 10;
   };
 }
 
@@ -86,6 +128,28 @@ void VisualizePointCloud::LoadConfig(const tinyxml2::XMLElement *)
   if (this->title.empty())
     this->title = "Visualize point cloud";
 
+  if (!this->dataPtr->node.Subscribe("/temperature",
+                            &VisualizePointCloud::OnTemperature, this))
+  {
+    ignerr << "Unable to subscribe to topic ["
+           << "/temperature" << "]\n";
+    return;
+  }
+  if (!this->dataPtr->node.Subscribe("/chlorophyll",
+                            &VisualizePointCloud::OnChlorophyll, this))
+  {
+    ignerr << "Unable to subscribe to topic ["
+           << "/chlorophyll" << "]\n";
+    return;
+  }
+  if (!this->dataPtr->node.Subscribe("/salinity",
+                            &VisualizePointCloud::OnSalinity, this))
+  {
+    ignerr << "Unable to subscribe to topic ["
+           << "/salinity" << "]\n";
+    return;
+  }
+
   ignition::gui::App()->findChild<
     ignition::gui::MainWindow *>()->installEventFilter(this);
 }
@@ -94,31 +158,47 @@ void VisualizePointCloud::LoadConfig(const tinyxml2::XMLElement *)
 void VisualizePointCloud::OnTopic(const QString &_topicName)
 {
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
+  // Unsubscribe from previous choice
+  /*
   if (!this->dataPtr->topicName.empty() &&
       !this->dataPtr->node.Unsubscribe(this->dataPtr->topicName))
   {
     ignerr << "Unable to unsubscribe from topic ["
            << this->dataPtr->topicName <<"]" <<std::endl;
   }
-
-  // Clear visualization
-  this->ClearMarkers();
-
+  */
   this->dataPtr->topicName = _topicName.toStdString();
 
   // Request service
-  this->dataPtr->node.Request(this->dataPtr->topicName,
+  this->dataPtr->node.Request(this->dataPtr->pcSrv,
       &VisualizePointCloud::OnService, this);
 
   // Create new subscription
+  if (!this->dataPtr->node.Subscribe(this->dataPtr->pcTopic,
+                            &VisualizePointCloud::OnCloud, this))
+  {
+    ignerr << "Unable to subscribe to topic ["
+           << this->dataPtr->pcTopic << "]\n";
+    return;
+  }
+  ignmsg << "Subscribed to " << this->dataPtr->pcTopic << std::endl;
+
+  // This doesn't work correctly. Values do not correspond to the right type
+  // of data. Maybe doesn't have time to subscribe befores markers go out.
+  // Better to subscribe individually - worked more reliably.
+  /*
   if (!this->dataPtr->node.Subscribe(this->dataPtr->topicName,
-                            &VisualizePointCloud::OnMessage, this))
+                            &VisualizePointCloud::OnFloatData, this))
   {
     ignerr << "Unable to subscribe to topic ["
            << this->dataPtr->topicName << "]\n";
     return;
   }
   ignmsg << "Subscribed to " << this->dataPtr->topicName << std::endl;
+  */
+
+  // Clear visualization
+  this->ClearMarkers();
 }
 
 //////////////////////////////////////////////////
@@ -143,6 +223,8 @@ void VisualizePointCloud::OnRefresh()
   // Clear
   this->dataPtr->topicList.clear();
 
+  bool gotCloud = false;
+
   // Get updated list
   std::vector<std::string> allTopics;
   this->dataPtr->node.TopicList(allTopics);
@@ -152,14 +234,21 @@ void VisualizePointCloud::OnRefresh()
     this->dataPtr->node.TopicInfo(topic, publishers);
     for (auto pub : publishers)
     {
+      // Have a fixed topic for point cloud locations. Let user choose
+      // which science data type to visualize
       if (pub.MsgTypeName() == "ignition.msgs.PointCloudPacked")
+      {
+        //this->dataPtr->topicList.push_back(QString::fromStdString(topic));
+        //break;
+        gotCloud = true;
+      }
+      else if (pub.MsgTypeName() == "ignition.msgs.Float_V")
       {
         this->dataPtr->topicList.push_back(QString::fromStdString(topic));
-        break;
       }
     }
   }
-  if (this->dataPtr->topicList.size() > 0)
+  if (gotCloud && this->dataPtr->topicList.size() > 0)
   {
     this->OnTopic(this->dataPtr->topicList.at(0));
   }
@@ -181,13 +270,43 @@ void VisualizePointCloud::SetTopicList(const QStringList &_topicList)
 }
 
 //////////////////////////////////////////////////
-void VisualizePointCloud::OnMessage(const ignition::msgs::PointCloudPacked &_msg)
+void VisualizePointCloud::OnCloud(const ignition::msgs::PointCloudPacked &_msg)
 {
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
   this->dataPtr->pcMsg = _msg;
   this->PublishMarkers();
 }
 
+/*
+//////////////////////////////////////////////////
+void VisualizePointCloud::OnFloatData(const ignition::msgs::Float_V &_msg)
+{
+  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
+  this->dataPtr->valMsg = _msg;
+}
+*/
+
+//////////////////////////////////////////////////
+void VisualizePointCloud::OnTemperature(const ignition::msgs::Float_V &_msg)
+{
+  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
+  this->dataPtr->tempMsg = _msg;
+}
+
+//////////////////////////////////////////////////
+void VisualizePointCloud::OnChlorophyll(const ignition::msgs::Float_V &_msg)
+{
+  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
+  this->dataPtr->chlorMsg = _msg;
+}
+
+//////////////////////////////////////////////////
+void VisualizePointCloud::OnSalinity(const ignition::msgs::Float_V &_msg)
+{
+  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
+  this->dataPtr->salMsg = _msg;
+}
+
 //////////////////////////////////////////////////
 void VisualizePointCloud::OnService(const ignition::msgs::PointCloudPacked &_res,
     bool _result)
@@ -213,48 +332,181 @@ void VisualizePointCloud::PublishMarkers()
     return;
   }
 
+  // Used to calculate cap of number of points to visualize, to save memory
+  int nPts = this->dataPtr->pcMsg.height() * this->dataPtr->pcMsg.width();
+  this->dataPtr->renderEvery = (int) round(
+    nPts / (double) this->dataPtr->MAX_PTS_VIS);
+
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
   ignition::msgs::Marker_V markers;
 
-  PointCloudPackedIterator<float> iter_x(this->dataPtr->pcMsg, "x");
-  PointCloudPackedIterator<float> iter_y(this->dataPtr->pcMsg, "y");
-  PointCloudPackedIterator<float> iter_z(this->dataPtr->pcMsg, "z");
+  PointCloudPackedIterator<float> iterX(this->dataPtr->pcMsg, "x");
+  PointCloudPackedIterator<float> iterY(this->dataPtr->pcMsg, "y");
+  PointCloudPackedIterator<float> iterZ(this->dataPtr->pcMsg, "z");
+  // FIXME: publish point cloud fields instead of float arrays
+  //PointCloudPackedIterator<float> iterTemp(this->dataPtr->pcMsg, "temperature");
+
+  // Type of data to visualize
+  std::string dataType = this->dataPtr->topicName;
+  // Ranges to scale marker colors
+  float minVal = 0.0f;
+  float maxVal = 10000.0f;
+  if (dataType == "/temperature")
+  {
+    minVal = 6.0f;
+    maxVal = 20.0f;
+  }
+  else if (dataType == "/chlorophyll")
+  {
+    //minVal = -6.0f;
+    minVal = 0.0f;
+    maxVal = 6.5f;
+  }
+  else if (dataType == "/salinity")
+  {
+    minVal = 32.0f;
+    maxVal = 34.5f;
+  }
 
-  int count{0};
-  while (iter_x != iter_x.end() &&
-         iter_y != iter_y.end() &&
-         iter_z != iter_z.end())
+  // TODO TEMPORARY DEBUG
+  ignerr << "First point in cloud (size "
+         << this->dataPtr->pcMsg.height() * this->dataPtr->pcMsg.width()
+         << "): " << *iterX << ", " << *iterY << ", " << *iterZ << std::endl;
+
+  // Index of point in point cloud, visualized or not
+  int ptIdx{0};
+  // Number of points actually visualized
+  int nPtsViz{0};
+  while (iterX != iterX.end() &&
+         iterY != iterY.end() &&
+         iterZ != iterZ.end())
   {
-    auto msg = markers.add_marker();
-
-    msg->set_ns(this->dataPtr->topicName);
-    msg->set_id(count++);
-    msg->mutable_material()->mutable_ambient()->set_b(1);
-    msg->mutable_material()->mutable_ambient()->set_a(0.3);
-    msg->mutable_material()->mutable_diffuse()->set_b(1);
-    msg->mutable_material()->mutable_diffuse()->set_a(0.3);
-    msg->set_action(ignition::msgs::Marker::ADD_MODIFY);
-    msg->set_type(ignition::msgs::Marker::BOX);
-    msg->set_visibility(ignition::msgs::Marker::GUI);
-    ignition::msgs::Set(msg->mutable_scale(),
-                    ignition::math::Vector3d::One);
-
-    ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
-      *iter_x,
-      *iter_y,
-      *iter_z,
-      0, 0, 0));
-
-    ++iter_x;
-    ++iter_y;
-    ++iter_z;
-
-    // FIXME: how to handle more points?
-    // https://github.com/osrf/lrauv/issues/85
-    if (count > 100)
-      break;
+    // TODO TEMPORARY Only publish every nth. Skip z below some depth
+    if (this->dataPtr->renderEvery != 0 &&
+        ptIdx % this->dataPtr->renderEvery == 0 &&
+        *iterZ > this->dataPtr->SKIP_Z_BELOW)
+    {
+      // Science data value
+      float dataVal = std::numeric_limits<float>::quiet_NaN();
+      /*
+      if (this->dataPtr->valMsg.data().size() > ptIdx)
+      {
+        dataVal = this->dataPtr->valMsg.data(ptIdx);
+      }
+      */
+      // Sanity check array size
+      if (dataType == "/temperature")
+      {
+        if (this->dataPtr->tempMsg.data().size() > ptIdx)
+        {
+          dataVal = this->dataPtr->tempMsg.data(ptIdx);
+        }
+      }
+      else if (dataType == "/chlorophyll")
+      {
+        if (this->dataPtr->chlorMsg.data().size() > ptIdx)
+        {
+          dataVal = this->dataPtr->chlorMsg.data(ptIdx);
+        }
+      }
+      else if (dataType == "/salinity")
+      {
+        if (this->dataPtr->salMsg.data().size() > ptIdx)
+        {
+          dataVal = this->dataPtr->salMsg.data(ptIdx);
+        }
+      }
+
+      // Don't visualize NaN
+      if (!std::isnan(dataVal))
+      {
+        auto msg = markers.add_marker();
+
+        msg->set_ns(this->dataPtr->pcTopic);
+        msg->set_id(nPtsViz + 1);
+
+        msg->mutable_material()->mutable_ambient()->set_r(
+          (dataVal - minVal) / (maxVal - minVal));
+        msg->mutable_material()->mutable_ambient()->set_g(
+          1 - (dataVal - minVal) / (maxVal - minVal));
+        msg->mutable_material()->mutable_ambient()->set_a(0.5);
+
+        msg->mutable_material()->mutable_diffuse()->set_r(
+          (dataVal - minVal) / (maxVal - minVal));
+        msg->mutable_material()->mutable_diffuse()->set_g(
+          1 - (dataVal - minVal) / (maxVal - minVal));
+        msg->mutable_material()->mutable_diffuse()->set_a(0.5);
+        msg->set_action(ignition::msgs::Marker::ADD_MODIFY);
+
+        // TODO: Use POINTS or LINE_LIST, but need per-vertex color
+        msg->set_type(ignition::msgs::Marker::BOX);
+        msg->set_visibility(ignition::msgs::Marker::GUI);
+        //ignition::msgs::Set(msg->mutable_scale(),
+        //                ignition::math::Vector3d::One);
+        // TODO TEMPORARY HACK make boxes exact dimension of x and y gaps to
+        // resemble "voxels". Then scale up by renderEvery to cover the space
+        // where all the points are skipped.
+        float dimX = this->dataPtr->RES_X * this->dataPtr->MINIATURE_SCALE
+          * this->dataPtr->renderEvery * this->dataPtr->renderEvery
+          * this->dataPtr->dimFactor;
+        float dimY = this->dataPtr->RES_Y * this->dataPtr->MINIATURE_SCALE
+          * this->dataPtr->renderEvery * this->dataPtr->renderEvery
+          * this->dataPtr->dimFactor;
+        float dimZ = this->dataPtr->RES_Z * this->dataPtr->MINIATURE_SCALE
+          * this->dataPtr->renderEvery * this->dataPtr->renderEvery
+          * this->dataPtr->dimFactor;
+        ignition::msgs::Set(msg->mutable_scale(),
+          ignition::math::Vector3d(dimX, dimY, dimZ));
+
+        // TODO TEMPORARY HACK Center the hack-scaled markers by shifting by
+        // half of box size
+        ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
+          *iterX,// + (0.5 * dimX),
+          *iterY,// + (0.5 * dimY),
+          *iterZ,// + (0.5 * dimZ),
+          0, 0, 0));
+
+        /*
+        // Use POINTS type and array for better performance, pending per-point
+        // color.
+        // One marker per point cloud, many points.
+        // TODO Implement in ign-gazebo per-point color like RViz point arrays, so
+        // can have just 1 marker, many points in it, each with a specified color,
+        // to improve performance. Color is the limiting factor that requires us
+        // to use many markers here, 1 point per marker.
+        ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
+          0, 0, 0, 0, 0, 0));
+        auto pt = msg->add_point();
+        pt->set_x(*iterX);
+        pt->set_y(*iterY);
+        pt->set_z(*iterZ);
+        */
+
+        // TODO TEMPORARY DEBUG
+        if (nPtsViz < 10)
+        {
+          ignerr << "Added point " << nPtsViz << " at "
+                 << msg->pose().position().x() << ", "
+                 << msg->pose().position().y() << ", "
+                 << msg->pose().position().z() << ", "
+                 << "value " << dataVal << ", "
+                 << "type " << dataType << ", "
+                 << "dimX " << dimX
+                 << std::endl;
+        }
+        ++nPtsViz;
+      }
+    }
+
+    ++iterX;
+    ++iterY;
+    ++iterZ;
+    ++ptIdx;
   }
 
+  ignerr << "Visualizing " << markers.marker().size() << " markers"
+    << std::endl;
+
   ignition::msgs::Boolean res;
   bool result;
   unsigned int timeout = 5000;
@@ -271,7 +523,7 @@ void VisualizePointCloud::ClearMarkers()
 {
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
   ignition::msgs::Marker msg;
-  msg.set_ns(this->dataPtr->topicName);
+  msg.set_ns(this->dataPtr->pcTopic);
   msg.set_id(0);
   msg.set_action(ignition::msgs::Marker::DELETE_ALL);
 
diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.hh b/lrauv_ignition_plugins/src/VisualizePointCloud.hh
index 3529ff75..3dd90f6e 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.hh
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.hh
@@ -25,6 +25,7 @@
 
 #include <memory>
 
+#include "ignition/msgs/float_v.pb.h"
 #include "ignition/msgs/pointcloud_packed.pb.h"
 
 #include <ignition/gui/Plugin.hh>
@@ -57,10 +58,16 @@ namespace tethys
 
     /// \brief Callback function to get data from the message
     /// \param[in]_msg Point cloud message
-    public: void OnMessage(const ignition::msgs::PointCloudPacked &_msg);
+    public: void OnCloud(const ignition::msgs::PointCloudPacked &_msg);
+
+    // TODO TEMPORARY HACK float arrays instead of point cloud fields
+    public: void OnTemperature(const ignition::msgs::Float_V &_msg);
+    public: void OnChlorophyll(const ignition::msgs::Float_V &_msg);
+    public: void OnSalinity(const ignition::msgs::Float_V &_msg);
+    public: void OnFloatData(const ignition::msgs::Float_V &_msg);
 
     /// \brief Callback function to get data from the message
-    /// \param[in]_res Point cloud message
+    /// \param[in]_msg Point cloud message
     /// \param[out]_result True on success.
     public: void OnService(const ignition::msgs::PointCloudPacked &_res,
         bool _result);
diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.qml b/lrauv_ignition_plugins/src/VisualizePointCloud.qml
index 146bd3e3..f5542260 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.qml
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.qml
@@ -80,7 +80,6 @@ GridLayout {
     ToolTip.text: qsTr("Ignition transport topics publishing PointCloudPacked messages")
   }
 
-
   Item {
     Layout.columnSpan: 6
     width: 10
diff --git a/lrauv_ignition_plugins/src/WorldCommPlugin.cc b/lrauv_ignition_plugins/src/WorldCommPlugin.cc
index 8dac654d..d7bec3a2 100644
--- a/lrauv_ignition_plugins/src/WorldCommPlugin.cc
+++ b/lrauv_ignition_plugins/src/WorldCommPlugin.cc
@@ -40,6 +40,7 @@
 
 using namespace tethys;
 
+/////////////////////////////////////////////////
 void WorldCommPlugin::Configure(
   const ignition::gazebo::Entity &_entity,
   const std::shared_ptr<const sdf::Element> &_sdf,
@@ -104,15 +105,22 @@ void WorldCommPlugin::Configure(
   this->createService = "/world/" + topicWorldName + "/create";
   this->setSphericalCoordsService = "/world/" + topicWorldName
     + "/set_spherical_coordinates";
+
+  // Advertise service for world origin in spherical coordinates
+  this->getWorldOriginSphericalService = "/world_origin_spherical";
+  this->node.Advertise(this->getWorldOriginSphericalService,
+    &WorldCommPlugin::WorldOriginSphericalService, this);
 }
 
+/////////////////////////////////////////////////
 void WorldCommPlugin::ServiceResponse(const ignition::msgs::Boolean &_rep,
-    const bool _result)
+  const bool _result)
 {
   if (!_result || !_rep.data())
     ignerr << "Error requesting some service." << std::endl;
 }
 
+/////////////////////////////////////////////////
 void WorldCommPlugin::SpawnCallback(
   const lrauv_ignition_plugins::msgs::LRAUVInit &_msg)
 {
@@ -143,6 +151,9 @@ void WorldCommPlugin::SpawnCallback(
     scReq.set_longitude_deg(lon);
     scReq.set_elevation(ele);
 
+    // Save world origin spherical coordinates for future service calls
+    this->worldOriginSphericalCoords = {lat, lon, ele};
+
     // Use zero heading so world is always aligned with lat / lon,
     // rotate vehicle instead.
     scReq.set_heading_deg(0.0);
@@ -174,6 +185,7 @@ void WorldCommPlugin::SpawnCallback(
   }
 }
 
+/////////////////////////////////////////////////
 std::string WorldCommPlugin::TethysSdfString(const std::string &_id)
 {
   const std::string sdfStr = R"(
@@ -230,6 +242,25 @@ std::string WorldCommPlugin::TethysSdfString(const std::string &_id)
   return sdfStr;
 }
 
+/////////////////////////////////////////////////
+bool WorldCommPlugin::WorldOriginSphericalService(
+  ignition::msgs::Vector3d &_res)
+{
+  if (this->spawnCount > 1)
+  {
+    _res.set_x(this->worldOriginSphericalCoords.X());
+    _res.set_y(this->worldOriginSphericalCoords.Y());
+    _res.set_z(this->worldOriginSphericalCoords.Z());
+    return true;
+  }
+  else
+  {
+    ignwarn << "No vehicles spawned yet. Request for world origin in "
+      << "spherical coordinates cannot be fulfilled." << std::endl;
+    return false;
+  }
+}
+
 IGNITION_ADD_PLUGIN(
   tethys::WorldCommPlugin,
   ignition::gazebo::System,
diff --git a/lrauv_ignition_plugins/src/WorldCommPlugin.hh b/lrauv_ignition_plugins/src/WorldCommPlugin.hh
index 7a839797..06d6b46f 100644
--- a/lrauv_ignition_plugins/src/WorldCommPlugin.hh
+++ b/lrauv_ignition_plugins/src/WorldCommPlugin.hh
@@ -65,6 +65,11 @@ namespace tethys
     private: void ServiceResponse(const ignition::msgs::Boolean &_rep,
         const bool _result);
 
+    /// Service callback for world origin in spherical coordinates. Service is
+    /// only valid after a first vehicle has been spawned.
+    /// \param[out] _res World origin in spherical coordinates
+    private: bool WorldOriginSphericalService(ignition::msgs::Vector3d &_res);
+
     /// Topic used to spawn robots
     private: std::string spawnTopic{"lrauv/init"};
 
@@ -77,8 +82,15 @@ namespace tethys
     /// Service to create entities
     private: std::string createService;
 
+    /// Service to get world origin spherical coordinates, after a first
+    /// vehicle has been spawned.
+    private: std::string getWorldOriginSphericalService;
+
     /// Count how many vehicles have been spawned
     private: int spawnCount{0};
+
+    /// World origin in spherical coordinates (latitude, longitude, elevation)
+    private: ignition::math::Vector3d worldOriginSphericalCoords;
   };
 }
 
diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
index 202555be..0467d677 100644
--- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
+++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
@@ -69,10 +69,26 @@
       name="tethys::TimeAnalysisPlugin">
     </plugin-->
 
+    <!-- Spawn at default in a location with science data in csv -->
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+
+      <!-- For 2003080103_mb_l3_las.csv -->
+      <latitude_deg>35.5999984741211</latitude_deg>
+      <longitude_deg>-121.779998779297</longitude_deg>
+
+      <!-- For 2003080103_mb_l3_las_1x1km.csv -->
+      <!--latitude_deg>36.8024781413352</latitude_deg>
+      <longitude_deg>-121.829647676843</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_1x1km.csv</data_path-->
       <!--data_path>simple_test.csv</data_path-->
     </plugin>
 
@@ -248,7 +264,7 @@
       <plugin filename="VisualizePointCloud" name="Visualize science data">
         <ignition-gui>
           <title>Visualize science data</title>
-          <property type="string" key="state">floating_collapsed</property>
+          <property type="string" key="state">floating</property>
           <property key="x" type="double">800</property>
         </ignition-gui>
       </plugin>
@@ -268,6 +284,20 @@
       <direction>-0.5 0.1 -0.9</direction>
     </light>
 
+    <model name="debug science data -200k -150k">
+      <pose>-200900 -159654 0 0 0 0</pose>
+      <static>true</static>
+      <link name="link">
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+
     <!-- Uncomment if you need a ground plane -->
     <!-- <model name="ground_plane">
       <static>true</static>
diff --git a/lrauv_ignition_plugins/worlds/empty_environment.sdf b/lrauv_ignition_plugins/worlds/empty_environment.sdf
index 5d7917e8..dc7d8dfc 100644
--- a/lrauv_ignition_plugins/worlds/empty_environment.sdf
+++ b/lrauv_ignition_plugins/worlds/empty_environment.sdf
@@ -62,7 +62,8 @@
       filename="ScienceSensorsSystem"
       name="tethys::ScienceSensorsSystem">
       <!--data_path>2003080103_mb_l3_las.csv</data_path-->
-      <data_path>dummy.csv</data_path>
+      <data_path>2003080103_mb_l3_las_1x1km.csv</data_path>
+      <!--data_path>dummy.csv</data_path-->
     </plugin>
 
     <!-- Interface with LRAUV Main Vehicle Application for the world -->
@@ -243,6 +244,13 @@
           <property type="double" key="x">400</property>
         </ignition-gui>
       </plugin>
+      <plugin filename="VisualizePointCloud" name="Visualize science data">
+        <ignition-gui>
+          <title>Visualize science data</title>
+          <property type="string" key="state">floating_collapsed</property>
+          <property key="x" type="double">800</property>
+        </ignition-gui>
+      </plugin>
     </gui>
 
     <light type="directional" name="sun">

From 7a89db435566124820e89ce8e66ef08693f61ef1 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Wed, 10 Nov 2021 01:36:54 -0500
Subject: [PATCH 02/12] resolve conflicts with main

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 lrauv_ignition_plugins/src/VisualizePointCloud.cc |  4 ++--
 lrauv_ignition_plugins/src/VisualizePointCloud.hh |  2 +-
 lrauv_ignition_plugins/worlds/buoyant_tethys.sdf  | 14 --------------
 3 files changed, 3 insertions(+), 17 deletions(-)

diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
index 188785d6..8cf216ed 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
@@ -308,7 +308,7 @@ void VisualizePointCloud::OnSalinity(const ignition::msgs::Float_V &_msg)
 }
 
 //////////////////////////////////////////////////
-void VisualizePointCloud::OnService(const ignition::msgs::PointCloudPacked &_res,
+void VisualizePointCloud::OnService(const ignition::msgs::PointCloudPacked &_msg,
     bool _result)
 {
   if (!_result)
@@ -318,7 +318,7 @@ void VisualizePointCloud::OnService(const ignition::msgs::PointCloudPacked &_res
   }
 
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
-  this->dataPtr->pcMsg = _res;
+  this->dataPtr->pcMsg = _msg;
   this->PublishMarkers();
 }
 
diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.hh b/lrauv_ignition_plugins/src/VisualizePointCloud.hh
index 3dd90f6e..5989ff86 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.hh
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.hh
@@ -69,7 +69,7 @@ namespace tethys
     /// \brief Callback function to get data from the message
     /// \param[in]_msg Point cloud message
     /// \param[out]_result True on success.
-    public: void OnService(const ignition::msgs::PointCloudPacked &_res,
+    public: void OnService(const ignition::msgs::PointCloudPacked &_msg,
         bool _result);
 
     /// \brief Get the topic list
diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
index 0467d677..67de445f 100644
--- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
+++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
@@ -284,20 +284,6 @@
       <direction>-0.5 0.1 -0.9</direction>
     </light>
 
-    <model name="debug science data -200k -150k">
-      <pose>-200900 -159654 0 0 0 0</pose>
-      <static>true</static>
-      <link name="link">
-        <visual name="visual">
-          <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
-          </geometry>
-        </visual>
-      </link>
-    </model>
-
     <!-- Uncomment if you need a ground plane -->
     <!-- <model name="ground_plane">
       <static>true</static>

From 7ecf0e0086170cfd80eb5b8808c7ab2208af4104 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Wed, 10 Nov 2021 02:14:14 -0500
Subject: [PATCH 03/12] replace TODO TEMPORARY HACK comments with Performance
 trick

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 .../src/ScienceSensorsSystem.cc               | 63 +++++++------------
 .../src/VisualizePointCloud.cc                | 61 ++++++++++--------
 .../worlds/buoyant_tethys.sdf                 | 15 +----
 3 files changed, 61 insertions(+), 78 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index ee688b95..46f6d475 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -170,25 +170,36 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Publisher for point clouds representing positions for science data
   public: ignition::transport::Node::Publisher cloudPub;
 
-  // TODO TEMPORARY HACK publish float arrays instead of point cloud fields
+  /// \brief Publisher for temperature
   public: ignition::transport::Node::Publisher tempPub;
+
+  /// \brief Publisher for chlorophyll
   public: ignition::transport::Node::Publisher chlorPub;
+
+  /// \brief Publisher for salinity
   public: ignition::transport::Node::Publisher salPub;
+
+  /// \brief Temperature message
   public: ignition::msgs::Float_V tempMsg;
+
+  /// \brief Chlorophyll message
   public: ignition::msgs::Float_V chlorMsg;
+
+  /// \brief Salinity message
   public: ignition::msgs::Float_V salMsg;
 
-  // TODO TEMPORARY HACK
   /// \brief Publish a few more times for visualization plugin to get them
   public: int repeatPubTimes = 1;
 
-  // TODO TEMPORARY HACK scale down to see in view to skip orbit tool limits
+  // TODO This is a workaround pending upstream Ignition orbit tool improvements
+  // Scale down in order to see in view
   // 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;
 
-  // TODO TEMPORARY HACK Skip depths below this z, so have memory to
+  // TODO This is a workaround pending upstream Marker performance improvements.
+  // Performance trick. Skip depths below this z, so have memory to
   // visualize higher layers at higher resolution.
   // This is only for visualization, so that MAX_PTS_VIS can calculate close
   // to the actual number of points visualized.
@@ -421,7 +432,7 @@ void ScienceSensorsSystemPrivate::ReadData()
       // Check validity of spatial coordinates
       if (!std::isnan(latitude) && !std::isnan(longitude) && !std::isnan(depth))
       {
-        // TODO TEMPORARY HACK skip points below a certain depth
+        // Performance trick. Skip points below a certain depth
         if (-depth < this->SKIP_Z_BELOW)
         {
           continue;
@@ -434,10 +445,10 @@ void ScienceSensorsSystemPrivate::ReadData()
         // Shift to be relative to world origin spherical coordinates
         cart -= this->worldOriginEarthCartesianCoords;
 
-        // TODO TEMPORARY HACK scale down to see in view
+        // Performance trick. Scale down to see in view
         cart *= this->MINIATURE_SCALE;
 
-        // TODO TEMPORARY HACK skip points beyond some distance from origin
+        // Performance trick. Skip points beyond some distance from origin
         if (abs(cart.X()) > 1000 || abs(cart.Y()) > 1000)
         {
           continue;
@@ -483,8 +494,8 @@ void ScienceSensorsSystemPrivate::ReadData()
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::GetWorldOriginSphericalCoords()
 {
-  // TODO TEMPORARY HACK hard code lat long.
-  // When have time, init default value from SDF <spherical_coordinates> tag
+  // TODO don't hard code lat long. Initialize default value from SDF
+  // <spherical_coordinates> tag
   this->worldOriginSphericalCoords = ignition::math::Vector3d(
     36.8024781413352, -121.829647676843, 0);
   // Convert spherical coordinates to Cartesian
@@ -657,7 +668,7 @@ void ScienceSensorsSystem::Configure(
   this->dataPtr->node.Advertise("/science_data_srv",
       &ScienceSensorsSystemPrivate::ScienceDataService, this->dataPtr.get());
 
-  // TODO TEMPORARY HACK publish float array instead of point cloud fields
+  // Advertise science data topics
   this->dataPtr->tempPub = this->dataPtr->node.Advertise<
       ignition::msgs::Float_V>("/temperature");
   this->dataPtr->chlorPub = this->dataPtr->node.Advertise<
@@ -721,8 +732,8 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
       }
     }
 
-    // TODO TEMPORARY DEBUG publishing more frequently to help debugging.
     // Publish every n iters so that VisualizePointCloud plugin gets it.
+    // Otherwise the initial publication in Configure() is not enough.
     if (this->dataPtr->repeatPubTimes % 10000 == 0)
     {
       this->dataPtr->PublishData();
@@ -742,7 +753,7 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
       // TODO convert to Cartesian
       auto sensorLatLon = ignition::gazebo::sphericalCoordinates(entity, _ecm);
       /*
-      // TODO TEMP DEBUG what is the sensor attached to?? World? Not robot?
+      // TODO DEBUG what is the sensor attached to?? World? Not robot?
       ignerr << "sensor lat long: "
              << sensorLatLon.value().X() << ", " << sensorLatLon.value().Y()
              << std::endl;
@@ -921,36 +932,10 @@ ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg()
   msg.set_row_step(pclPC2.row_step);
   msg.set_is_dense(pclPC2.is_dense);
 
-  // FIXME: Include more than position data
   msg.mutable_data()->resize(pclPC2.data.size());
-  //  + temperatureArr[this->timeIdx].size());
   memcpy(msg.mutable_data()->data(), pclPC2.data.data(), pclPC2.data.size());
 
-  /*
-  // Interleaf fields to each point
-  for (int c = 0; c < pclPC2.width; ++c)
-  {
-    for (int r = 0; r < pclPC2.height; ++r)
-    {
-      memcpy(msg.mutable_data()->data() + ,
-        pclPC2.data.data() + ,
-        pclPC2.data.size());
-
-
-    }
-  }
-  */
-
-  // Wrong layout
-  // Temperature field, offset from beginning by previous data's size
-  //memcpy(msg.mutable_data()->data() + pclPC2.data.size(),
-  //  temperatureArr[this->timeIdx].data(), temperatureArr[this->timeIdx].size());
-
-  // TODO TEMPORARY HACK publish float arrays instead of point cloud fields
-  //this->tempMsg.mutable_data()->Resize(temperatureArr[this->timeIdx].size(), 0.0f);
-  //memcpy(this->tempMsg.mutable_data(),
-  //  temperatureArr[this->timeIdx].data(), temperatureArr[this->timeIdx].size());
-  // TODO did memory leak start after I started publishing float array?
+  // Populate float arrays for actual science data
   *this->tempMsg.mutable_data() = {temperatureArr[this->timeIdx].begin(),
     temperatureArr[this->timeIdx].end()};
   *this->chlorMsg.mutable_data() = {chlorophyllArr[this->timeIdx].begin(),
diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
index 8cf216ed..bdedea49 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
@@ -69,40 +69,52 @@ namespace tethys
     /// \brief Point cloud message containing location of data
     public: ignition::msgs::PointCloudPacked pcMsg;
 
-    /// \brief Temperature mssage to visualize
-    // TODO TEMPORARY HACK 1D array instead of point cloud fields
+    /// \brief Temperature data to visualize
     public: ignition::msgs::Float_V tempMsg;
+
+    /// \brief Chlorophyll data to visualize
     public: ignition::msgs::Float_V chlorMsg;
+
+    /// \brief Salinity data to visualize
     public: ignition::msgs::Float_V salMsg;
-    //public: ignition::msgs::Float_V valMsg;
 
-    // Cap of how many points to visualize, to save memory
+    /// \brief Performance trick. Cap number of points to visualize, to save
+    /// memory.
     public: const int MAX_PTS_VIS = 1000;
 
-    // TODO TEMPORARY HACK
-    // Render only every other n, to save performance. Increase to render fewer
-    // markers (faster performance).
+    /// \brief Performance trick. Render only every other n. Increase to render
+    /// fewer markers (faster performance).
     public: int renderEvery = 20;
 
-    // TODO TEMPORARY HACK Skip depths below this z, so have memory to
-    // visualize higher layers at higher resolution.
+    /// \brief Performance trick. Skip depths below this z, so have memory to
+    /// visualize higher layers at higher resolution.
+    /// For less confusion, match the parameter in ScienceSensorsSystem.cc.
     public: const float SKIP_Z_BELOW = -40;
 
-    // TODO TEMPORARY HACK scale down to see in view to skip orbit tool limits
+    /// \brief Scale down to see in view to skip orbit tool limits
+    /// For less confusion, match the parameter in ScienceSensorsSystem.cc.
+    // TODO This is a workaround pending upstream orbit tool improvements
     // 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;
+
+    /// \brief Performance trick. Factor to multiply in calculating Marker sizes
     public: float dimFactor = 0.03;
 
-    // TODO TEMPORARY HACK hardcode resolution to make markers resemble voxels
+    /// \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;
+
+    /// \brief Parameter to calculate Marker size in y.
     public: const float RES_Y = 22;
+
+    /// \brief Parameter to calculate Marker size in z.
     public: const float RES_Z = 10;
   };
 }
@@ -368,8 +380,7 @@ void VisualizePointCloud::PublishMarkers()
     maxVal = 34.5f;
   }
 
-  // TODO TEMPORARY DEBUG
-  ignerr << "First point in cloud (size "
+  igndbg << "First point in cloud (size "
          << this->dataPtr->pcMsg.height() * this->dataPtr->pcMsg.width()
          << "): " << *iterX << ", " << *iterY << ", " << *iterZ << std::endl;
 
@@ -381,7 +392,7 @@ void VisualizePointCloud::PublishMarkers()
          iterY != iterY.end() &&
          iterZ != iterZ.end())
   {
-    // TODO TEMPORARY Only publish every nth. Skip z below some depth
+    // Performance trick. Only publish every nth. Skip z below some depth
     if (this->dataPtr->renderEvery != 0 &&
         ptIdx % this->dataPtr->renderEvery == 0 &&
         *iterZ > this->dataPtr->SKIP_Z_BELOW)
@@ -443,7 +454,7 @@ void VisualizePointCloud::PublishMarkers()
         msg->set_visibility(ignition::msgs::Marker::GUI);
         //ignition::msgs::Set(msg->mutable_scale(),
         //                ignition::math::Vector3d::One);
-        // TODO TEMPORARY HACK make boxes exact dimension of x and y gaps to
+        // Performance trick. Make boxes exact dimension of x and y gaps to
         // resemble "voxels". Then scale up by renderEvery to cover the space
         // where all the points are skipped.
         float dimX = this->dataPtr->RES_X * this->dataPtr->MINIATURE_SCALE
@@ -458,22 +469,21 @@ void VisualizePointCloud::PublishMarkers()
         ignition::msgs::Set(msg->mutable_scale(),
           ignition::math::Vector3d(dimX, dimY, dimZ));
 
-        // TODO TEMPORARY HACK Center the hack-scaled markers by shifting by
-        // half of box size
         ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
-          *iterX,// + (0.5 * dimX),
-          *iterY,// + (0.5 * dimY),
-          *iterZ,// + (0.5 * dimZ),
+          *iterX,
+          *iterY,
+          *iterZ,
           0, 0, 0));
 
         /*
         // Use POINTS type and array for better performance, pending per-point
         // color.
         // One marker per point cloud, many points.
-        // TODO Implement in ign-gazebo per-point color like RViz point arrays, so
-        // can have just 1 marker, many points in it, each with a specified color,
-        // to improve performance. Color is the limiting factor that requires us
-        // to use many markers here, 1 point per marker.
+        // TODO Implement in ign-gazebo per-point color like RViz point arrays,
+        // so can have just 1 marker, many points in it, each with a specified
+        // color, to improve performance. Color is the limiting factor that
+        // requires us to use many markers here, 1 point per marker.
+        // https://github.com/osrf/lrauv/issues/52
         ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
           0, 0, 0, 0, 0, 0));
         auto pt = msg->add_point();
@@ -482,10 +492,9 @@ void VisualizePointCloud::PublishMarkers()
         pt->set_z(*iterZ);
         */
 
-        // TODO TEMPORARY DEBUG
         if (nPtsViz < 10)
         {
-          ignerr << "Added point " << nPtsViz << " at "
+          igndbg << "Added point " << nPtsViz << " at "
                  << msg->pose().position().x() << ", "
                  << msg->pose().position().y() << ", "
                  << msg->pose().position().z() << ", "
diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
index 67de445f..9cb7d35d 100644
--- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
+++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
@@ -13,18 +13,6 @@
       <background>0.0 0.7 0.8</background>
     </scene>
 
-    <!--
-      Set the coordinates for the world origin to 0 / 0
-    -->
-    <spherical_coordinates>
-      <surface_model>EARTH_WGS84</surface_model>
-      <world_frame_orientation>ENU</world_frame_orientation>
-      <latitude_deg>0</latitude_deg>
-      <longitude_deg>0</longitude_deg>
-      <elevation>0</elevation>
-      <heading_deg>0</heading_deg>
-    </spherical_coordinates>
-
     <physics name="1ms" type="dart">
       <max_step_size>0.02</max_step_size>
       <real_time_factor>0</real_time_factor>
@@ -69,9 +57,10 @@
       name="tethys::TimeAnalysisPlugin">
     </plugin-->
 
-    <!-- Spawn at default in a location with science data in csv -->
+    <!-- Spawn by default in a location with science data in csv -->
     <spherical_coordinates>
       <surface_model>EARTH_WGS84</surface_model>
+      <world_frame_orientation>ENU</world_frame_orientation>
 
       <!-- For 2003080103_mb_l3_las.csv -->
       <latitude_deg>35.5999984741211</latitude_deg>

From 3dc76c72259c849741c99080b4ac2e2d7c1cc4a4 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Wed, 10 Nov 2021 02:27:06 -0500
Subject: [PATCH 04/12] cleanup

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 lrauv_ignition_plugins/src/ScienceSensorsSystem.cc |  8 ++++++--
 lrauv_ignition_plugins/src/VisualizePointCloud.cc  | 10 +---------
 lrauv_ignition_plugins/worlds/buoyant_tethys.sdf   |  2 +-
 3 files changed, 8 insertions(+), 12 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index 46f6d475..aee35d7f 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -102,8 +102,8 @@ class tethys::ScienceSensorsSystemPrivate
 
   /// \brief Service name for getting the spherical coordinates associated with
   /// the world origin
-  public: std::string getWorldOriginSphericalService
-    {"/world_origin_spherical"};
+  // public: std::string getWorldOriginSphericalService
+  //   {"/world_origin_spherical"};
 
   /// \brief Input data file name, relative to a path Ignition can find in its
   /// environment variables.
@@ -503,6 +503,10 @@ void ScienceSensorsSystemPrivate::GetWorldOriginSphericalCoords()
   this->worldOriginEarthCartesianCoords = sc.LocalFromSphericalPosition(
     this->worldOriginSphericalCoords);
 
+  // TODO: Need a good way (a service?) to get vehicle spawn lat long from
+  // WorldCommPlugin.cc. Cannot wait until vehicle is spawned, because could
+  // wait forever. After that, then must shift all the science coordinates AFTER
+  // ReadData(), to have Cartesian coordinates wrt new origin lat/long.
   /*
   ignition::msgs::Vector3d res;
   bool success;
diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
index bdedea49..aef7921f 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
@@ -399,12 +399,6 @@ void VisualizePointCloud::PublishMarkers()
     {
       // Science data value
       float dataVal = std::numeric_limits<float>::quiet_NaN();
-      /*
-      if (this->dataPtr->valMsg.data().size() > ptIdx)
-      {
-        dataVal = this->dataPtr->valMsg.data(ptIdx);
-      }
-      */
       // Sanity check array size
       if (dataType == "/temperature")
       {
@@ -452,8 +446,6 @@ void VisualizePointCloud::PublishMarkers()
         // TODO: Use POINTS or LINE_LIST, but need per-vertex color
         msg->set_type(ignition::msgs::Marker::BOX);
         msg->set_visibility(ignition::msgs::Marker::GUI);
-        //ignition::msgs::Set(msg->mutable_scale(),
-        //                ignition::math::Vector3d::One);
         // Performance trick. Make boxes exact dimension of x and y gaps to
         // resemble "voxels". Then scale up by renderEvery to cover the space
         // where all the points are skipped.
@@ -513,7 +505,7 @@ void VisualizePointCloud::PublishMarkers()
     ++ptIdx;
   }
 
-  ignerr << "Visualizing " << markers.marker().size() << " markers"
+  igndbg << "Visualizing " << markers.marker().size() << " markers"
     << std::endl;
 
   ignition::msgs::Boolean res;
diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
index 9cb7d35d..7232b12b 100644
--- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
+++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
@@ -253,7 +253,7 @@
       <plugin filename="VisualizePointCloud" name="Visualize science data">
         <ignition-gui>
           <title>Visualize science data</title>
-          <property type="string" key="state">floating</property>
+          <property type="string" key="state">floating_collapsed</property>
           <property key="x" type="double">800</property>
         </ignition-gui>
       </plugin>

From 64eca3203e0747afaabb789fbaf328ce93c44c6a Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Mon, 15 Nov 2021 21:46:52 -0500
Subject: [PATCH 05/12] add mutex for worldOriginSpherical*

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 .../src/ScienceSensorsSystem.cc                   | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index 2156ee88..103aed14 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -20,6 +20,8 @@
  * Research Institute (MBARI) and the David and Lucile Packard Foundation
  */
 
+#include <mutex>
+
 #include <ignition/msgs/pointcloud_packed.pb.h>
 
 #include <ignition/common/SystemPaths.hh>
@@ -118,6 +120,9 @@ class tethys::ScienceSensorsSystemPrivate
   /// 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;
 
@@ -284,6 +289,10 @@ ScienceSensorsSystem::ScienceSensorsSystem()
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::ReadData()
 {
+  // Lock modifications to world origin spherical association until finish
+  // reading and transforming data
+  std::lock_guard<std::mutex> lock(mtx);
+
   std::fstream fs;
   fs.open(this->dataPath, std::ios::in);
 
@@ -505,6 +514,9 @@ void ScienceSensorsSystemPrivate::ReadData()
 void ScienceSensorsSystemPrivate::UpdateWorldSphericalOrigin(
   ignition::gazebo::EntityComponentManager &_ecm)
 {
+  // Lock for changes to worldOriginSpherical* until update complete
+  std::lock_guard<std::mutex> lock(mtx);
+
   if (!this->worldSphericalCoordsInitialized)
   {
     auto latLon = this->world.SphericalCoordinates(_ecm);
@@ -542,6 +554,9 @@ void ScienceSensorsSystemPrivate::UpdateWorldSphericalOrigin(
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::ShiftDataToNewSphericalOrigin()
 {
+  // Lock modifications to world origin spherical association until finish
+  // transforming data
+  std::lock_guard<std::mutex> lock(mtx);
 }
 
 /////////////////////////////////////////////////

From dae526f700e7cc7e0f6b01d33dd92ce99de19710 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Tue, 16 Nov 2021 02:29:45 -0500
Subject: [PATCH 06/12] add BUILD_TESTING to README

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 README.md | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/README.md b/README.md
index 60d6a1a0..ecefa95c 100644
--- a/README.md
+++ b/README.md
@@ -52,6 +52,11 @@ Clone this repository then run
 colcon build
 ```
 
+Developers may want to build tests. Note that this would take longer:
+```
+colcon build --cmake-args "-DBUILD_TESTING=ON"
+```
+
 ## To test simulation in Ignition standalone (without MBARI integration)
 
 This package comes with an empty example world. To run this example world simply

From 17a165cfaae41a466a4768f19ca1a08c1dd714a2 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Tue, 16 Nov 2021 02:30:02 -0500
Subject: [PATCH 07/12] disable science sensors in empty_environment.sdf

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 lrauv_ignition_plugins/worlds/empty_environment.sdf | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/lrauv_ignition_plugins/worlds/empty_environment.sdf b/lrauv_ignition_plugins/worlds/empty_environment.sdf
index 682b7b73..c50029ba 100644
--- a/lrauv_ignition_plugins/worlds/empty_environment.sdf
+++ b/lrauv_ignition_plugins/worlds/empty_environment.sdf
@@ -54,13 +54,13 @@
       name="tethys::TimeAnalysisPlugin">
     </plugin-->
 
-    <plugin
+    <!-- Dynamic vehicle spawn not supported by science sensors yet - need to
+      shift all science data positions to new lat/long of world origin. -->
+    <!--plugin
       filename="ScienceSensorsSystem"
       name="tethys::ScienceSensorsSystem">
-      <!--data_path>2003080103_mb_l3_las.csv</data_path-->
-      <data_path>2003080103_mb_l3_las_1x1km.csv</data_path>
-      <!--data_path>dummy.csv</data_path-->
-    </plugin>
+      <data_path>2003080103_mb_l3_las.csv</data_path>
+    </plugin-->
 
     <!-- Interface with LRAUV Main Vehicle Application for the world -->
     <plugin

From 6aa70766867ce471f5eb17127fdcd28fc6cfd37b Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Tue, 16 Nov 2021 04:38:01 -0500
Subject: [PATCH 08/12] manual cherry pick 643c97d from
 mabelzhang/interpolate_sci_raw_mat

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 .../src/ScienceSensorsSystem.cc               | 50 ++++++++++++-------
 1 file changed, 33 insertions(+), 17 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index 103aed14..0721d660 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -46,6 +46,12 @@ class tethys::ScienceSensorsSystemPrivate
   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 &_latlon,
+    ignition::math::Vector3d &_cart);
+
   /// \brief Shift point cloud with respect to the world origin in spherical
   /// coordinates, if available.
   public: void ShiftDataToNewSphericalOrigin();
@@ -126,13 +132,17 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Spherical coordinates of world origin. Can change at any time.
   public: ignition::math::SphericalCoordinates worldOriginSphericalCoords;
 
-  /// World origin in spherical position (latitude, longitude, elevation),
-  /// angles in degrees
+  /// \brief World origin in spherical position (latitude, longitude,
+  /// elevation), angles in degrees
   public: ignition::math::Vector3d worldOriginSphericalPos = {0, 0, 0};
 
-  /// World origin in Cartesian coordinates converted from spherical coordinates
+  /// \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};
 
@@ -206,14 +216,14 @@ class tethys::ScienceSensorsSystemPrivate
   public: int repeatPubTimes = 1;
 
   // TODO This is a workaround pending upstream Ignition orbit tool improvements
-  // Scale down in order to see in view
+  // \brief Scale down in order to see in view
   // 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;
 
   // TODO This is a workaround pending upstream Marker performance improvements.
-  // Performance trick. Skip depths below this z, so have memory to
+  // \brief Performance trick. Skip depths below this z, so have memory to
   // visualize higher layers at higher resolution.
   // This is only for visualization, so that MAX_PTS_VIS can calculate close
   // to the actual number of points visualized.
@@ -304,10 +314,6 @@ void ScienceSensorsSystemPrivate::ReadData()
 
   std::stringstream ss(line);
 
-  // Spherical coordinates object for Cartesian conversions
-  ignition::math::SphericalCoordinates sphCoord(
-    this->worldOriginSphericalCoords.Surface());
-
   // Tokenize header line into columns
   while (std::getline(ss, word, ','))
   {
@@ -458,11 +464,8 @@ void ScienceSensorsSystemPrivate::ReadData()
         }
 
         // Convert spherical coordinates to Cartesian
-        ignition::math::Vector3d cart = sphCoord.LocalFromSphericalPosition(
-          {latitude, longitude, 0});
-
-        // Shift to be relative to world origin spherical coordinates
-        cart -= this->worldOriginCartesianCoords;
+        ignition::math::Vector3d cart;
+        this->ConvertLatLonToCart({latitude, longitude, 0}, cart);
 
         // Performance trick. Scale down to see in view
         cart *= this->MINIATURE_SCALE;
@@ -534,10 +537,11 @@ void ScienceSensorsSystemPrivate::UpdateWorldSphericalOrigin(
         this->worldOriginSphericalCoords.LongitudeReference().Degree(),
         0);
       // Convert spherical coordinates to Cartesian
-      ignition::math::SphericalCoordinates sphCoord(
+      this->sphCoord = ignition::math::SphericalCoordinates(
         this->worldOriginSphericalCoords.Surface());
-      this->worldOriginCartesianCoords = sphCoord.LocalFromSphericalPosition(
-        this->worldOriginSphericalPos);
+      this->worldOriginCartesianCoords =
+        this->sphCoord.LocalFromSphericalPosition(
+          this->worldOriginSphericalPos);
 
       this->worldSphericalCoordsInitialized = true;
 
@@ -551,6 +555,18 @@ void ScienceSensorsSystemPrivate::UpdateWorldSphericalOrigin(
   }
 }
 
+/////////////////////////////////////////////////
+void ScienceSensorsSystemPrivate::ConvertLatLonToCart(
+  const ignition::math::Vector3d &_latlon,
+  ignition::math::Vector3d &_cart)
+{
+  // Convert spherical coordinates to Cartesian
+  _cart = this->sphCoord.LocalFromSphericalPosition(_latlon);
+
+  // Shift to be relative to world origin spherical coordinates
+  _cart -= this->worldOriginCartesianCoords;
+}
+
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::ShiftDataToNewSphericalOrigin()
 {

From 58af8e80de7eba1ba2d0879186d02c1e5b3afb8f Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Tue, 16 Nov 2021 20:27:13 -0500
Subject: [PATCH 09/12] remove WorldOriginSphericalService from WorldCommPlugin

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 lrauv_ignition_plugins/src/WorldCommPlugin.cc | 27 -------------------
 lrauv_ignition_plugins/src/WorldCommPlugin.hh | 12 ---------
 2 files changed, 39 deletions(-)

diff --git a/lrauv_ignition_plugins/src/WorldCommPlugin.cc b/lrauv_ignition_plugins/src/WorldCommPlugin.cc
index d7bec3a2..3caf206e 100644
--- a/lrauv_ignition_plugins/src/WorldCommPlugin.cc
+++ b/lrauv_ignition_plugins/src/WorldCommPlugin.cc
@@ -105,11 +105,6 @@ void WorldCommPlugin::Configure(
   this->createService = "/world/" + topicWorldName + "/create";
   this->setSphericalCoordsService = "/world/" + topicWorldName
     + "/set_spherical_coordinates";
-
-  // Advertise service for world origin in spherical coordinates
-  this->getWorldOriginSphericalService = "/world_origin_spherical";
-  this->node.Advertise(this->getWorldOriginSphericalService,
-    &WorldCommPlugin::WorldOriginSphericalService, this);
 }
 
 /////////////////////////////////////////////////
@@ -151,9 +146,6 @@ void WorldCommPlugin::SpawnCallback(
     scReq.set_longitude_deg(lon);
     scReq.set_elevation(ele);
 
-    // Save world origin spherical coordinates for future service calls
-    this->worldOriginSphericalCoords = {lat, lon, ele};
-
     // Use zero heading so world is always aligned with lat / lon,
     // rotate vehicle instead.
     scReq.set_heading_deg(0.0);
@@ -242,25 +234,6 @@ std::string WorldCommPlugin::TethysSdfString(const std::string &_id)
   return sdfStr;
 }
 
-/////////////////////////////////////////////////
-bool WorldCommPlugin::WorldOriginSphericalService(
-  ignition::msgs::Vector3d &_res)
-{
-  if (this->spawnCount > 1)
-  {
-    _res.set_x(this->worldOriginSphericalCoords.X());
-    _res.set_y(this->worldOriginSphericalCoords.Y());
-    _res.set_z(this->worldOriginSphericalCoords.Z());
-    return true;
-  }
-  else
-  {
-    ignwarn << "No vehicles spawned yet. Request for world origin in "
-      << "spherical coordinates cannot be fulfilled." << std::endl;
-    return false;
-  }
-}
-
 IGNITION_ADD_PLUGIN(
   tethys::WorldCommPlugin,
   ignition::gazebo::System,
diff --git a/lrauv_ignition_plugins/src/WorldCommPlugin.hh b/lrauv_ignition_plugins/src/WorldCommPlugin.hh
index 06d6b46f..7a839797 100644
--- a/lrauv_ignition_plugins/src/WorldCommPlugin.hh
+++ b/lrauv_ignition_plugins/src/WorldCommPlugin.hh
@@ -65,11 +65,6 @@ namespace tethys
     private: void ServiceResponse(const ignition::msgs::Boolean &_rep,
         const bool _result);
 
-    /// Service callback for world origin in spherical coordinates. Service is
-    /// only valid after a first vehicle has been spawned.
-    /// \param[out] _res World origin in spherical coordinates
-    private: bool WorldOriginSphericalService(ignition::msgs::Vector3d &_res);
-
     /// Topic used to spawn robots
     private: std::string spawnTopic{"lrauv/init"};
 
@@ -82,15 +77,8 @@ namespace tethys
     /// Service to create entities
     private: std::string createService;
 
-    /// Service to get world origin spherical coordinates, after a first
-    /// vehicle has been spawned.
-    private: std::string getWorldOriginSphericalService;
-
     /// Count how many vehicles have been spawned
     private: int spawnCount{0};
-
-    /// World origin in spherical coordinates (latitude, longitude, elevation)
-    private: ignition::math::Vector3d worldOriginSphericalCoords;
   };
 }
 

From b6b52454b5bebf44a6fcc9d713e6bbcd2a4b0449 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Tue, 16 Nov 2021 20:57:18 -0500
Subject: [PATCH 10/12] add depth to ConvertLatLonToCart for convenience

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 .../src/ScienceSensorsSystem.cc                  | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index 0721d660..d94e18bc 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -49,7 +49,7 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Convert (lat, lon, 0) to Cartesian XYZ, including shifting by
   /// world origin
   public: void ConvertLatLonToCart(
-    const ignition::math::Vector3d &_latlon,
+    const ignition::math::Vector3d &_latLonEle,
     ignition::math::Vector3d &_cart);
 
   /// \brief Shift point cloud with respect to the world origin in spherical
@@ -465,10 +465,12 @@ void ScienceSensorsSystemPrivate::ReadData()
 
         // Convert spherical coordinates to Cartesian
         ignition::math::Vector3d cart;
-        this->ConvertLatLonToCart({latitude, longitude, 0}, cart);
+        this->ConvertLatLonToCart({latitude, longitude, -depth}, cart);
 
         // Performance trick. Scale down to see in view
         cart *= this->MINIATURE_SCALE;
+        // Revert Z to the unscaled depth
+        cart.Z() = -depth;
 
         // Performance trick. Skip points beyond some distance from origin
         if (abs(cart.X()) > 1000 || abs(cart.Y()) > 1000)
@@ -480,7 +482,7 @@ void ScienceSensorsSystemPrivate::ReadData()
         // 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(), -depth));
+          pcl::PointXYZ(cart.X(), cart.Y(), cart.Z()));
 
         // Populate science data
         this->temperatureArr[lineTimeIdx].push_back(temp);
@@ -557,14 +559,18 @@ void ScienceSensorsSystemPrivate::UpdateWorldSphericalOrigin(
 
 /////////////////////////////////////////////////
 void ScienceSensorsSystemPrivate::ConvertLatLonToCart(
-  const ignition::math::Vector3d &_latlon,
+  const ignition::math::Vector3d &_latLonEle,
   ignition::math::Vector3d &_cart)
 {
   // Convert spherical coordinates to Cartesian
-  _cart = this->sphCoord.LocalFromSphericalPosition(_latlon);
+  _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();
 }
 
 /////////////////////////////////////////////////

From 6a5b426ea6b6815069e35955cf8bfac0159e2675 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Fri, 19 Nov 2021 10:06:51 -0800
Subject: [PATCH 11/12] Suggestions to #70 (#85)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 .../src/VisualizePointCloud.cc                | 530 ++++++++++--------
 .../src/VisualizePointCloud.hh                | 147 ++++-
 .../src/VisualizePointCloud.qml               | 197 +++++--
 3 files changed, 568 insertions(+), 306 deletions(-)

diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
index aef7921f..ff785331 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc
@@ -33,9 +33,12 @@
 
 #include <ignition/plugin/Register.hh>
 
+#include <ignition/math/Color.hh>
 #include <ignition/math/Vector3.hh>
 #include <ignition/math/Pose3.hh>
 
+#include <ignition/msgs/Utility.hh>
+
 #include <ignition/transport/Node.hh>
 
 #include <ignition/gui/Application.hh>
@@ -51,32 +54,26 @@ namespace tethys
     /// \brief Transport node
     public: ignition::transport::Node node;
 
-    /// \brief Science data type-specific topic name to subscribe
-    public: std::string topicName{""};
+    /// \brief Name of topic for PointCloudPacked
+    public: std::string pointCloudTopic{""};
 
-    /// \brief List of science data topics
-    public: QStringList topicList;
+    /// \brief Name of topic for FloatV
+    public: std::string floatVTopic{""};
 
-    /// \brief Protect variables changed from transport and the user
-    public: std::recursive_mutex mutex;
+    /// \brief List of topics publishing PointCloudPacked.
+    public: QStringList pointCloudTopicList;
 
-    /// \brief Generic point cloud topic name
-    public: std::string pcTopic = {"/science_data"};
+    /// \brief List of topics publishing FloatV.
+    public: QStringList floatVTopicList;
 
-    /// \brief Generic point cloud service name
-    public: std::string pcSrv = {"/science_data_srv"};
+    /// \brief Protect variables changed from transport and the user
+    public: std::recursive_mutex mutex;
 
     /// \brief Point cloud message containing location of data
-    public: ignition::msgs::PointCloudPacked pcMsg;
-
-    /// \brief Temperature data to visualize
-    public: ignition::msgs::Float_V tempMsg;
+    public: ignition::msgs::PointCloudPacked pointCloudMsg;
 
-    /// \brief Chlorophyll data to visualize
-    public: ignition::msgs::Float_V chlorMsg;
-
-    /// \brief Salinity data to visualize
-    public: ignition::msgs::Float_V salMsg;
+    /// \brief Message holding a float vector.
+    public: ignition::msgs::Float_V floatVMsg;
 
     /// \brief Performance trick. Cap number of points to visualize, to save
     /// memory.
@@ -116,6 +113,21 @@ namespace tethys
 
     /// \brief Parameter to calculate Marker size in z.
     public: const float RES_Z = 10;
+
+    /// \brief Minimum value in latest float vector
+    public: float minFloatV{std::numeric_limits<float>::max()};
+
+    /// \brief Maximum value in latest float vector
+    public: float maxFloatV{-std::numeric_limits<float>::max()};
+
+    /// \brief Color for minimum value
+    public: ignition::math::Color minColor{255, 0, 0, 255};
+
+    /// \brief Color for maximum value
+    public: ignition::math::Color maxColor{0, 255, 0, 255};
+
+    /// \brief True if showing
+    public: bool showing{true};
   };
 }
 
@@ -140,82 +152,78 @@ void VisualizePointCloud::LoadConfig(const tinyxml2::XMLElement *)
   if (this->title.empty())
     this->title = "Visualize point cloud";
 
-  if (!this->dataPtr->node.Subscribe("/temperature",
-                            &VisualizePointCloud::OnTemperature, this))
-  {
-    ignerr << "Unable to subscribe to topic ["
-           << "/temperature" << "]\n";
-    return;
-  }
-  if (!this->dataPtr->node.Subscribe("/chlorophyll",
-                            &VisualizePointCloud::OnChlorophyll, this))
-  {
-    ignerr << "Unable to subscribe to topic ["
-           << "/chlorophyll" << "]\n";
-    return;
-  }
-  if (!this->dataPtr->node.Subscribe("/salinity",
-                            &VisualizePointCloud::OnSalinity, this))
-  {
-    ignerr << "Unable to subscribe to topic ["
-           << "/salinity" << "]\n";
-    return;
-  }
-
   ignition::gui::App()->findChild<
     ignition::gui::MainWindow *>()->installEventFilter(this);
 }
 
 //////////////////////////////////////////////////
-void VisualizePointCloud::OnTopic(const QString &_topicName)
+void VisualizePointCloud::OnPointCloudTopic(const QString &_pointCloudTopic)
 {
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
   // Unsubscribe from previous choice
-  /*
-  if (!this->dataPtr->topicName.empty() &&
-      !this->dataPtr->node.Unsubscribe(this->dataPtr->topicName))
+  if (!this->dataPtr->pointCloudTopic.empty() &&
+      !this->dataPtr->node.Unsubscribe(this->dataPtr->pointCloudTopic))
   {
     ignerr << "Unable to unsubscribe from topic ["
-           << this->dataPtr->topicName <<"]" <<std::endl;
+           << this->dataPtr->pointCloudTopic <<"]" <<std::endl;
   }
-  */
-  this->dataPtr->topicName = _topicName.toStdString();
+
+  // Clear visualization
+  this->ClearMarkers();
+
+  this->dataPtr->pointCloudTopic = _pointCloudTopic.toStdString();
 
   // Request service
-  this->dataPtr->node.Request(this->dataPtr->pcSrv,
-      &VisualizePointCloud::OnService, this);
+  this->dataPtr->node.Request(this->dataPtr->pointCloudTopic,
+      &VisualizePointCloud::OnPointCloudService, this);
 
   // Create new subscription
-  if (!this->dataPtr->node.Subscribe(this->dataPtr->pcTopic,
-                            &VisualizePointCloud::OnCloud, this))
+  if (!this->dataPtr->node.Subscribe(this->dataPtr->pointCloudTopic,
+                            &VisualizePointCloud::OnPointCloud, this))
   {
     ignerr << "Unable to subscribe to topic ["
-           << this->dataPtr->pcTopic << "]\n";
+           << this->dataPtr->pointCloudTopic << "]\n";
     return;
   }
-  ignmsg << "Subscribed to " << this->dataPtr->pcTopic << std::endl;
-
-  // This doesn't work correctly. Values do not correspond to the right type
-  // of data. Maybe doesn't have time to subscribe befores markers go out.
-  // Better to subscribe individually - worked more reliably.
-  /*
-  if (!this->dataPtr->node.Subscribe(this->dataPtr->topicName,
-                            &VisualizePointCloud::OnFloatData, this))
+  ignmsg << "Subscribed to " << this->dataPtr->pointCloudTopic << std::endl;
+}
+
+//////////////////////////////////////////////////
+void VisualizePointCloud::OnFloatVTopic(const QString &_floatVTopic)
+{
+  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
+  // Unsubscribe from previous choice
+  if (!this->dataPtr->floatVTopic.empty() &&
+      !this->dataPtr->node.Unsubscribe(this->dataPtr->floatVTopic))
   {
-    ignerr << "Unable to subscribe to topic ["
-           << this->dataPtr->topicName << "]\n";
-    return;
+    ignerr << "Unable to unsubscribe from topic ["
+           << this->dataPtr->floatVTopic <<"]" <<std::endl;
   }
-  ignmsg << "Subscribed to " << this->dataPtr->topicName << std::endl;
-  */
 
   // Clear visualization
   this->ClearMarkers();
+
+  this->dataPtr->floatVTopic = _floatVTopic.toStdString();
+
+  // Request service
+  this->dataPtr->node.Request(this->dataPtr->floatVTopic,
+      &VisualizePointCloud::OnPointCloudService, this);
+
+  // Create new subscription
+  if (!this->dataPtr->node.Subscribe(this->dataPtr->floatVTopic,
+                            &VisualizePointCloud::OnFloatV, this))
+  {
+    ignerr << "Unable to subscribe to topic ["
+           << this->dataPtr->floatVTopic << "]\n";
+    return;
+  }
+  ignmsg << "Subscribed to " << this->dataPtr->floatVTopic << std::endl;
 }
 
 //////////////////////////////////////////////////
 void VisualizePointCloud::Show(bool _show)
 {
+  this->dataPtr->showing = _show;
   if (_show)
   {
     this->PublishMarkers();
@@ -233,7 +241,8 @@ void VisualizePointCloud::OnRefresh()
   ignmsg << "Refreshing topic list for point cloud messages." << std::endl;
 
   // Clear
-  this->dataPtr->topicList.clear();
+  this->dataPtr->pointCloudTopicList.clear();
+  this->dataPtr->floatVTopicList.clear();
 
   bool gotCloud = false;
 
@@ -246,263 +255,234 @@ void VisualizePointCloud::OnRefresh()
     this->dataPtr->node.TopicInfo(topic, publishers);
     for (auto pub : publishers)
     {
-      // Have a fixed topic for point cloud locations. Let user choose
-      // which science data type to visualize
       if (pub.MsgTypeName() == "ignition.msgs.PointCloudPacked")
       {
-        //this->dataPtr->topicList.push_back(QString::fromStdString(topic));
-        //break;
-        gotCloud = true;
+        this->dataPtr->pointCloudTopicList.push_back(
+            QString::fromStdString(topic));
       }
       else if (pub.MsgTypeName() == "ignition.msgs.Float_V")
       {
-        this->dataPtr->topicList.push_back(QString::fromStdString(topic));
+        this->dataPtr->floatVTopicList.push_back(QString::fromStdString(topic));
       }
     }
   }
-  if (gotCloud && this->dataPtr->topicList.size() > 0)
+  if (this->dataPtr->pointCloudTopicList.size() > 0)
+  {
+    this->OnPointCloudTopic(this->dataPtr->pointCloudTopicList.at(0));
+  }
+  if (this->dataPtr->floatVTopicList.size() > 0)
   {
-    this->OnTopic(this->dataPtr->topicList.at(0));
+    this->OnFloatVTopic(this->dataPtr->floatVTopicList.at(0));
   }
 
-  this->TopicListChanged();
+  this->PointCloudTopicListChanged();
+  this->FloatVTopicListChanged();
 }
 
 /////////////////////////////////////////////////
-QStringList VisualizePointCloud::TopicList() const
+QStringList VisualizePointCloud::PointCloudTopicList() const
 {
-  return this->dataPtr->topicList;
+  return this->dataPtr->pointCloudTopicList;
 }
 
 /////////////////////////////////////////////////
-void VisualizePointCloud::SetTopicList(const QStringList &_topicList)
+void VisualizePointCloud::SetPointCloudTopicList(
+    const QStringList &_pointCloudTopicList)
 {
-  this->dataPtr->topicList = _topicList;
-  this->TopicListChanged();
+  this->dataPtr->pointCloudTopicList = _pointCloudTopicList;
+  this->PointCloudTopicListChanged();
 }
 
-//////////////////////////////////////////////////
-void VisualizePointCloud::OnCloud(const ignition::msgs::PointCloudPacked &_msg)
+/////////////////////////////////////////////////
+QStringList VisualizePointCloud::FloatVTopicList() const
 {
-  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
-  this->dataPtr->pcMsg = _msg;
-  this->PublishMarkers();
+  return this->dataPtr->floatVTopicList;
 }
 
-/*
-//////////////////////////////////////////////////
-void VisualizePointCloud::OnFloatData(const ignition::msgs::Float_V &_msg)
+/////////////////////////////////////////////////
+void VisualizePointCloud::SetFloatVTopicList(
+    const QStringList &_floatVTopicList)
 {
-  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
-  this->dataPtr->valMsg = _msg;
+  this->dataPtr->floatVTopicList = _floatVTopicList;
+  this->FloatVTopicListChanged();
 }
-*/
 
 //////////////////////////////////////////////////
-void VisualizePointCloud::OnTemperature(const ignition::msgs::Float_V &_msg)
+void VisualizePointCloud::OnPointCloud(
+    const ignition::msgs::PointCloudPacked &_msg)
 {
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
-  this->dataPtr->tempMsg = _msg;
+  this->dataPtr->pointCloudMsg = _msg;
+  this->PublishMarkers();
 }
 
 //////////////////////////////////////////////////
-void VisualizePointCloud::OnChlorophyll(const ignition::msgs::Float_V &_msg)
+void VisualizePointCloud::OnFloatV(const ignition::msgs::Float_V &_msg)
 {
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
-  this->dataPtr->chlorMsg = _msg;
+  this->dataPtr->floatVMsg = _msg;
+
+  this->dataPtr->minFloatV = std::numeric_limits<float>::max();
+  this->dataPtr->maxFloatV = -std::numeric_limits<float>::max();
+
+  for (auto i = 0; i < _msg.data_size(); ++i)
+  {
+    auto data = _msg.data(i);
+    if (data < this->dataPtr->minFloatV)
+      this->SetMinFloatV(data);
+    if (data > this->dataPtr->maxFloatV)
+      this->SetMaxFloatV(data);
+  }
+
+  this->PublishMarkers();
 }
 
 //////////////////////////////////////////////////
-void VisualizePointCloud::OnSalinity(const ignition::msgs::Float_V &_msg)
+void VisualizePointCloud::OnPointCloudService(
+    const ignition::msgs::PointCloudPacked &_msg, bool _result)
 {
-  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
-  this->dataPtr->salMsg = _msg;
+  if (!_result)
+  {
+    ignerr << "Service request failed." << std::endl;
+    return;
+  }
+  this->OnPointCloud(_msg);
 }
 
 //////////////////////////////////////////////////
-void VisualizePointCloud::OnService(const ignition::msgs::PointCloudPacked &_msg,
-    bool _result)
+void VisualizePointCloud::OnFloatVService(
+    const ignition::msgs::Float_V &_msg, bool _result)
 {
   if (!_result)
   {
     ignerr << "Service request failed." << std::endl;
     return;
   }
-
-  std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
-  this->dataPtr->pcMsg = _msg;
-  this->PublishMarkers();
+  this->OnFloatV(_msg);
 }
 
 //////////////////////////////////////////////////
 void VisualizePointCloud::PublishMarkers()
 {
+  if (!this->dataPtr->showing)
+    return;
+
   // If point cloud empty, do nothing. (PointCloudPackedIteratorBase errors on
   // empty cloud.)
-  if (this->dataPtr->pcMsg.height() == 0 && this->dataPtr->pcMsg.width() == 0)
+  if (this->dataPtr->pointCloudMsg.height() == 0 &&
+      this->dataPtr->pointCloudMsg.width() == 0)
   {
     return;
   }
 
   // Used to calculate cap of number of points to visualize, to save memory
-  int nPts = this->dataPtr->pcMsg.height() * this->dataPtr->pcMsg.width();
+  int nPts = this->dataPtr->pointCloudMsg.height() *
+      this->dataPtr->pointCloudMsg.width();
   this->dataPtr->renderEvery = (int) round(
     nPts / (double) this->dataPtr->MAX_PTS_VIS);
 
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
   ignition::msgs::Marker_V markers;
 
-  PointCloudPackedIterator<float> iterX(this->dataPtr->pcMsg, "x");
-  PointCloudPackedIterator<float> iterY(this->dataPtr->pcMsg, "y");
-  PointCloudPackedIterator<float> iterZ(this->dataPtr->pcMsg, "z");
-  // FIXME: publish point cloud fields instead of float arrays
-  //PointCloudPackedIterator<float> iterTemp(this->dataPtr->pcMsg, "temperature");
-
-  // Type of data to visualize
-  std::string dataType = this->dataPtr->topicName;
-  // Ranges to scale marker colors
-  float minVal = 0.0f;
-  float maxVal = 10000.0f;
-  if (dataType == "/temperature")
-  {
-    minVal = 6.0f;
-    maxVal = 20.0f;
-  }
-  else if (dataType == "/chlorophyll")
-  {
-    //minVal = -6.0f;
-    minVal = 0.0f;
-    maxVal = 6.5f;
-  }
-  else if (dataType == "/salinity")
-  {
-    minVal = 32.0f;
-    maxVal = 34.5f;
-  }
-
-  igndbg << "First point in cloud (size "
-         << this->dataPtr->pcMsg.height() * this->dataPtr->pcMsg.width()
-         << "): " << *iterX << ", " << *iterY << ", " << *iterZ << std::endl;
+  PointCloudPackedIterator<float> iterX(this->dataPtr->pointCloudMsg, "x");
+  PointCloudPackedIterator<float> iterY(this->dataPtr->pointCloudMsg, "y");
+  PointCloudPackedIterator<float> iterZ(this->dataPtr->pointCloudMsg, "z");
 
   // Index of point in point cloud, visualized or not
   int ptIdx{0};
   // Number of points actually visualized
   int nPtsViz{0};
-  while (iterX != iterX.end() &&
-         iterY != iterY.end() &&
-         iterZ != iterZ.end())
+  for (;iterX != iterX.end() &&
+        iterY != iterY.end() &&
+        iterZ != iterZ.end(); ++iterX, ++iterY, ++iterZ, ++ptIdx)
   {
     // Performance trick. Only publish every nth. Skip z below some depth
-    if (this->dataPtr->renderEvery != 0 &&
-        ptIdx % this->dataPtr->renderEvery == 0 &&
-        *iterZ > this->dataPtr->SKIP_Z_BELOW)
+    if (this->dataPtr->renderEvery == 0 ||
+        ptIdx % this->dataPtr->renderEvery != 0 ||
+        *iterZ < this->dataPtr->SKIP_Z_BELOW)
     {
-      // Science data value
-      float dataVal = std::numeric_limits<float>::quiet_NaN();
-      // Sanity check array size
-      if (dataType == "/temperature")
-      {
-        if (this->dataPtr->tempMsg.data().size() > ptIdx)
-        {
-          dataVal = this->dataPtr->tempMsg.data(ptIdx);
-        }
-      }
-      else if (dataType == "/chlorophyll")
-      {
-        if (this->dataPtr->chlorMsg.data().size() > ptIdx)
-        {
-          dataVal = this->dataPtr->chlorMsg.data(ptIdx);
-        }
-      }
-      else if (dataType == "/salinity")
-      {
-        if (this->dataPtr->salMsg.data().size() > ptIdx)
-        {
-          dataVal = this->dataPtr->salMsg.data(ptIdx);
-        }
-      }
+      continue;
+    }
 
-      // Don't visualize NaN
-      if (!std::isnan(dataVal))
-      {
-        auto msg = markers.add_marker();
-
-        msg->set_ns(this->dataPtr->pcTopic);
-        msg->set_id(nPtsViz + 1);
-
-        msg->mutable_material()->mutable_ambient()->set_r(
-          (dataVal - minVal) / (maxVal - minVal));
-        msg->mutable_material()->mutable_ambient()->set_g(
-          1 - (dataVal - minVal) / (maxVal - minVal));
-        msg->mutable_material()->mutable_ambient()->set_a(0.5);
-
-        msg->mutable_material()->mutable_diffuse()->set_r(
-          (dataVal - minVal) / (maxVal - minVal));
-        msg->mutable_material()->mutable_diffuse()->set_g(
-          1 - (dataVal - minVal) / (maxVal - minVal));
-        msg->mutable_material()->mutable_diffuse()->set_a(0.5);
-        msg->set_action(ignition::msgs::Marker::ADD_MODIFY);
-
-        // TODO: Use POINTS or LINE_LIST, but need per-vertex color
-        msg->set_type(ignition::msgs::Marker::BOX);
-        msg->set_visibility(ignition::msgs::Marker::GUI);
-        // Performance trick. Make boxes exact dimension of x and y gaps to
-        // resemble "voxels". Then scale up by renderEvery to cover the space
-        // where all the points are skipped.
-        float dimX = this->dataPtr->RES_X * this->dataPtr->MINIATURE_SCALE
-          * this->dataPtr->renderEvery * this->dataPtr->renderEvery
-          * this->dataPtr->dimFactor;
-        float dimY = this->dataPtr->RES_Y * this->dataPtr->MINIATURE_SCALE
-          * this->dataPtr->renderEvery * this->dataPtr->renderEvery
-          * this->dataPtr->dimFactor;
-        float dimZ = this->dataPtr->RES_Z * this->dataPtr->MINIATURE_SCALE
-          * this->dataPtr->renderEvery * this->dataPtr->renderEvery
-          * this->dataPtr->dimFactor;
-        ignition::msgs::Set(msg->mutable_scale(),
-          ignition::math::Vector3d(dimX, dimY, dimZ));
-
-        ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
-          *iterX,
-          *iterY,
-          *iterZ,
-          0, 0, 0));
-
-        /*
-        // Use POINTS type and array for better performance, pending per-point
-        // color.
-        // One marker per point cloud, many points.
-        // TODO Implement in ign-gazebo per-point color like RViz point arrays,
-        // so can have just 1 marker, many points in it, each with a specified
-        // color, to improve performance. Color is the limiting factor that
-        // requires us to use many markers here, 1 point per marker.
-        // https://github.com/osrf/lrauv/issues/52
-        ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
-          0, 0, 0, 0, 0, 0));
-        auto pt = msg->add_point();
-        pt->set_x(*iterX);
-        pt->set_y(*iterY);
-        pt->set_z(*iterZ);
-        */
-
-        if (nPtsViz < 10)
-        {
-          igndbg << "Added point " << nPtsViz << " at "
-                 << msg->pose().position().x() << ", "
-                 << msg->pose().position().y() << ", "
-                 << msg->pose().position().z() << ", "
-                 << "value " << dataVal << ", "
-                 << "type " << dataType << ", "
-                 << "dimX " << dimX
-                 << std::endl;
-        }
-        ++nPtsViz;
-      }
+    // Value from float vector
+    float dataVal = std::numeric_limits<float>::quiet_NaN();
+    if (this->dataPtr->floatVMsg.data().size() > ptIdx)
+    {
+      dataVal = this->dataPtr->floatVMsg.data(ptIdx);
     }
 
-    ++iterX;
-    ++iterY;
-    ++iterZ;
-    ++ptIdx;
+    // Don't visualize NaN
+    if (std::isnan(dataVal))
+      continue;
+
+    auto msg = markers.add_marker();
+
+    msg->set_ns(this->dataPtr->pointCloudTopic + "-" +
+        this->dataPtr->floatVTopic);
+    msg->set_id(nPtsViz + 1);
+
+    auto ratio = (dataVal - this->dataPtr->minFloatV) /
+        (this->dataPtr->maxFloatV - this->dataPtr->minFloatV);
+    auto color = this->dataPtr->minColor +
+        (this->dataPtr->maxColor - this->dataPtr->minColor) * ratio;
+
+    ignition::msgs::Set(msg->mutable_material()->mutable_ambient(), color);
+    ignition::msgs::Set(msg->mutable_material()->mutable_diffuse(), color);
+    msg->mutable_material()->mutable_diffuse()->set_a(0.5);
+    msg->set_action(ignition::msgs::Marker::ADD_MODIFY);
+
+    // TODO: Use POINTS or LINE_LIST, but need per-vertex color
+    msg->set_type(ignition::msgs::Marker::BOX);
+    msg->set_visibility(ignition::msgs::Marker::GUI);
+    // Performance trick. Make boxes exact dimension of x and y gaps to
+    // resemble "voxels". Then scale up by renderEvery to cover the space
+    // where all the points are skipped.
+    float dimX = this->dataPtr->RES_X * this->dataPtr->MINIATURE_SCALE
+      * this->dataPtr->renderEvery * this->dataPtr->renderEvery
+      * this->dataPtr->dimFactor;
+    float dimY = this->dataPtr->RES_Y * this->dataPtr->MINIATURE_SCALE
+      * this->dataPtr->renderEvery * this->dataPtr->renderEvery
+      * this->dataPtr->dimFactor;
+    float dimZ = this->dataPtr->RES_Z * this->dataPtr->MINIATURE_SCALE
+      * this->dataPtr->renderEvery * this->dataPtr->renderEvery
+      * this->dataPtr->dimFactor;
+    ignition::msgs::Set(msg->mutable_scale(),
+      ignition::math::Vector3d(dimX, dimY, dimZ));
+
+    ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
+      *iterX,
+      *iterY,
+      *iterZ,
+      0, 0, 0));
+
+    /*
+    // Use POINTS type and array for better performance, pending per-point
+    // color.
+    // One marker per point cloud, many points.
+    // TODO Implement in ign-gazebo per-point color like RViz point arrays,
+    // so can have just 1 marker, many points in it, each with a specified
+    // color, to improve performance. Color is the limiting factor that
+    // requires us to use many markers here, 1 point per marker.
+    // https://github.com/osrf/lrauv/issues/52
+    ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
+      0, 0, 0, 0, 0, 0));
+    auto pt = msg->add_point();
+    pt->set_x(*iterX);
+    pt->set_y(*iterY);
+    pt->set_z(*iterZ);
+    */
+
+    if (nPtsViz < 10)
+    {
+      igndbg << "Added point " << nPtsViz << " at "
+             << msg->pose().position().x() << ", "
+             << msg->pose().position().y() << ", "
+             << msg->pose().position().z() << ", "
+             << "value " << dataVal << ", "
+             << "dimX " << dimX
+             << std::endl;
+    }
+    ++nPtsViz;
   }
 
   igndbg << "Visualizing " << markers.marker().size() << " markers"
@@ -524,13 +504,67 @@ void VisualizePointCloud::ClearMarkers()
 {
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
   ignition::msgs::Marker msg;
-  msg.set_ns(this->dataPtr->pcTopic);
+  msg.set_ns(this->dataPtr->pointCloudTopic + "-" + this->dataPtr->floatVTopic);
   msg.set_id(0);
   msg.set_action(ignition::msgs::Marker::DELETE_ALL);
 
   this->dataPtr->node.Request("/marker", msg);
 }
 
+/////////////////////////////////////////////////
+QColor VisualizePointCloud::MinColor() const
+{
+  return ignition::gui::convert(this->dataPtr->minColor);
+}
+
+/////////////////////////////////////////////////
+void VisualizePointCloud::SetMinColor(const QColor &_minColor)
+{
+  this->dataPtr->minColor = ignition::gui::convert(_minColor);
+  this->MinColorChanged();
+  this->PublishMarkers();
+}
+
+/////////////////////////////////////////////////
+QColor VisualizePointCloud::MaxColor() const
+{
+  return ignition::gui::convert(this->dataPtr->maxColor);
+}
+
+/////////////////////////////////////////////////
+void VisualizePointCloud::SetMaxColor(const QColor &_maxColor)
+{
+  this->dataPtr->maxColor = ignition::gui::convert(_maxColor);
+  this->MaxColorChanged();
+  this->PublishMarkers();
+}
+
+/////////////////////////////////////////////////
+float VisualizePointCloud::MinFloatV() const
+{
+  return this->dataPtr->minFloatV;
+}
+
+/////////////////////////////////////////////////
+void VisualizePointCloud::SetMinFloatV(float _minFloatV)
+{
+  this->dataPtr->minFloatV = _minFloatV;
+  this->MinFloatVChanged();
+}
+
+/////////////////////////////////////////////////
+float VisualizePointCloud::MaxFloatV() const
+{
+  return this->dataPtr->maxFloatV;
+}
+
+/////////////////////////////////////////////////
+void VisualizePointCloud::SetMaxFloatV(float _maxFloatV)
+{
+  this->dataPtr->maxFloatV = _maxFloatV;
+  this->MaxFloatVChanged();
+}
+
 // Register this plugin
 IGNITION_ADD_PLUGIN(tethys::VisualizePointCloud,
                     ignition::gui::Plugin)
diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.hh b/lrauv_ignition_plugins/src/VisualizePointCloud.hh
index 5989ff86..772ac9f8 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.hh
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.hh
@@ -41,10 +41,50 @@ namespace tethys
 
     /// \brief List of topics publishing PointCloudPacked messages
     Q_PROPERTY(
-      QStringList topicList
-      READ TopicList
-      WRITE SetTopicList
-      NOTIFY TopicListChanged
+      QStringList pointCloudTopicList
+      READ PointCloudTopicList
+      WRITE SetPointCloudTopicList
+      NOTIFY PointCloudTopicListChanged
+    )
+
+    /// \brief List of topics publishing FloatV messages
+    Q_PROPERTY(
+      QStringList floatVTopicList
+      READ FloatVTopicList
+      WRITE SetFloatVTopicList
+      NOTIFY FloatVTopicListChanged
+    )
+
+    /// \brief Color for minimum value
+    Q_PROPERTY(
+      QColor minColor
+      READ MinColor
+      WRITE SetMinColor
+      NOTIFY MinColorChanged
+    )
+
+    /// \brief Color for maximum value
+    Q_PROPERTY(
+      QColor maxColor
+      READ MaxColor
+      WRITE SetMaxColor
+      NOTIFY MaxColorChanged
+    )
+
+    /// \brief Minimum value
+    Q_PROPERTY(
+      float minFloatV
+      READ MinFloatV
+      WRITE SetMinFloatV
+      NOTIFY MinFloatVChanged
+    )
+
+    /// \brief Maximum value
+    Q_PROPERTY(
+      float maxFloatV
+      READ MaxFloatV
+      WRITE SetMaxFloatV
+      NOTIFY MaxFloatVChanged
     )
 
     /// \brief Constructor
@@ -56,36 +96,101 @@ namespace tethys
     // Documentation inherited
     public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override;
 
-    /// \brief Callback function to get data from the message
+    /// \brief Callback function for point cloud topic.
     /// \param[in]_msg Point cloud message
-    public: void OnCloud(const ignition::msgs::PointCloudPacked &_msg);
+    public: void OnPointCloud(const ignition::msgs::PointCloudPacked &_msg);
 
-    // TODO TEMPORARY HACK float arrays instead of point cloud fields
-    public: void OnTemperature(const ignition::msgs::Float_V &_msg);
-    public: void OnChlorophyll(const ignition::msgs::Float_V &_msg);
-    public: void OnSalinity(const ignition::msgs::Float_V &_msg);
-    public: void OnFloatData(const ignition::msgs::Float_V &_msg);
-
-    /// \brief Callback function to get data from the message
+    /// \brief Callback function for point cloud service
     /// \param[in]_msg Point cloud message
     /// \param[out]_result True on success.
-    public: void OnService(const ignition::msgs::PointCloudPacked &_msg,
-        bool _result);
+    public: void OnPointCloudService(
+        const ignition::msgs::PointCloudPacked &_msg, bool _result);
+
+    /// \brief Get the topic list
+    /// \return List of topics
+    public: Q_INVOKABLE QStringList PointCloudTopicList() const;
+
+    /// \brief Set the topic list from a string
+    /// \param[in] _pointCloudTopicList List of topics.
+    public: Q_INVOKABLE void SetPointCloudTopicList(
+        const QStringList &_pointCloudTopicList);
+
+    /// \brief Notify that topic list has changed
+    signals: void PointCloudTopicListChanged();
+
+    /// \brief Set topic to subscribe to for point cloud.
+    /// \param[in] _topicName Name of selected topic
+    public: Q_INVOKABLE void OnPointCloudTopic(const QString &_topicName);
+
+    /// \brief Callback function for float vector topic.
+    /// \param[in]_msg Float vector message
+    public: void OnFloatV(const ignition::msgs::Float_V &_msg);
+
+    /// \brief Callback function for point cloud service
+    /// \param[in]_msg Float vector message
+    /// \param[out]_result True on success.
+    public: void OnFloatVService(
+        const ignition::msgs::Float_V &_msg, bool _result);
 
     /// \brief Get the topic list
     /// \return List of topics
-    public: Q_INVOKABLE QStringList TopicList() const;
+    public: Q_INVOKABLE QStringList FloatVTopicList() const;
 
     /// \brief Set the topic list from a string
-    /// \param[in] _topicList List of topics.
-    public: Q_INVOKABLE void SetTopicList(const QStringList &_topicList);
+    /// \param[in] _floatVTopicList List of topics.
+    public: Q_INVOKABLE void SetFloatVTopicList(
+        const QStringList &_floatVTopicList);
 
     /// \brief Notify that topic list has changed
-    signals: void TopicListChanged();
+    signals: void FloatVTopicListChanged();
 
-    /// \brief Set topic to subscribe to.
+    /// \brief Set topic to subscribe to for float vectors.
     /// \param[in] _topicName Name of selected topic
-    public: Q_INVOKABLE void OnTopic(const QString &_topicName);
+    public: Q_INVOKABLE void OnFloatVTopic(const QString &_topicName);
+
+    /// \brief Get the minimum color
+    /// \return Minimum color
+    public: Q_INVOKABLE QColor MinColor() const;
+
+    /// \brief Set the minimum color
+    /// \param[in] _minColor Minimum color.
+    public: Q_INVOKABLE void SetMinColor(const QColor &_minColor);
+
+    /// \brief Notify that minimum color has changed
+    signals: void MinColorChanged();
+
+    /// \brief Get the maximum color
+    /// \return Maximum color
+    public: Q_INVOKABLE QColor MaxColor() const;
+
+    /// \brief Set the maximum color
+    /// \param[ax] _maxColor Maximum color.
+    public: Q_INVOKABLE void SetMaxColor(const QColor &_maxColor);
+
+    /// \brief Notify that maximum color has changed
+    signals: void MaxColorChanged();
+
+    /// \brief Get the minimum value
+    /// \return Minimum value
+    public: Q_INVOKABLE float MinFloatV() const;
+
+    /// \brief Set the minimum value
+    /// \param[in] _minFloatV Minimum value.
+    public: Q_INVOKABLE void SetMinFloatV(float _minFloatV);
+
+    /// \brief Notify that minimum value has changed
+    signals: void MinFloatVChanged();
+
+    /// \brief Get the maximum value
+    /// \return Maximum value
+    public: Q_INVOKABLE float MaxFloatV() const;
+
+    /// \brief Set the maximum value
+    /// \param[ax] _maxFloatV Maximum value.
+    public: Q_INVOKABLE void SetMaxFloatV(float _maxFloatV);
+
+    /// \brief Notify that maximum value has changed
+    signals: void MaxFloatVChanged();
 
     /// \brief Set whether to show the point cloud.
     /// \param[in] _show Boolean value for displaying the points.
diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.qml b/lrauv_ignition_plugins/src/VisualizePointCloud.qml
index f5542260..6bf90183 100644
--- a/lrauv_ignition_plugins/src/VisualizePointCloud.qml
+++ b/lrauv_ignition_plugins/src/VisualizePointCloud.qml
@@ -27,57 +27,180 @@ import QtQuick.Controls.Material 2.1
 import QtQuick.Layouts 1.3
 import "qrc:/qml"
 
-GridLayout {
-  columns: 6
-  columnSpacing: 10
+ColumnLayout {
+  spacing: 10
   Layout.minimumWidth: 350
-  Layout.minimumHeight: 200
+  Layout.minimumHeight: 300
   anchors.fill: parent
   anchors.leftMargin: 10
   anchors.rightMargin: 10
 
-  // TODO Use switch
-  CheckBox {
-    Layout.alignment: Qt.AlignHCenter
-    id: displayVisual
-    Layout.columnSpan: 6
+  RowLayout {
+    spacing: 10
     Layout.fillWidth: true
-    text: qsTr("Show")
-    checked: true
-    onClicked: {
-      VisualizePointCloud.Show(checked)
+
+    Switch {
+      Layout.alignment: Qt.AlignHCenter
+      id: displayVisual
+      Layout.columnSpan: 5
+      Layout.fillWidth: true
+      text: qsTr("Show")
+      checked: true
+      onToggled: {
+        VisualizePointCloud.Show(checked)
+      }
+    }
+
+    RoundButton {
+      Layout.columnSpan: 1
+      text: "\u21bb"
+      Material.background: Material.primary
+      onClicked: {
+        VisualizePointCloud.OnRefresh();
+        pcCombo.currentIndex = 0
+        floatCombo.currentIndex = 0
+      }
+      ToolTip.visible: hovered
+      ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
+      ToolTip.text: qsTr("Refresh list of topics publishing point clouds and float vectors")
     }
   }
 
-  RoundButton {
-    Layout.columnSpan: 1
-    text: "\u21bb"
-    Material.background: Material.primary
-    onClicked: {
-      combo.currentIndex = 0
-      VisualizePointCloud.OnRefresh();
+  GridLayout {
+    columns: 3
+    columnSpacing: 10
+    Layout.fillWidth: true
+
+    Label {
+      Layout.columnSpan: 1
+      text: "Point cloud"
+    }
+
+    ComboBox {
+      Layout.columnSpan: 2
+      id: pcCombo
+      Layout.fillWidth: true
+      model: VisualizePointCloud.pointCloudTopicList
+      currentIndex: 0
+      onCurrentIndexChanged: {
+        if (currentIndex < 0)
+          return;
+        VisualizePointCloud.OnPointCloudTopic(textAt(currentIndex));
+      }
+      ToolTip.visible: hovered
+      ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
+      ToolTip.text: qsTr("Ignition transport topics publishing PointCloudPacked messages")
+    }
+
+    Label {
+      Layout.columnSpan: 1
+      text: "Float vector"
+    }
+
+    ComboBox {
+      Layout.columnSpan: 2
+      id: floatCombo
+      Layout.fillWidth: true
+      model: VisualizePointCloud.floatVTopicList
+      currentIndex: 0
+      onCurrentIndexChanged: {
+        if (currentIndex < 0)
+          return;
+        VisualizePointCloud.OnFloatVTopic(textAt(currentIndex));
+      }
+      ToolTip.visible: hovered
+      ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
+      ToolTip.text: qsTr("Ignition transport topics publishing FloatV messages")
     }
-    ToolTip.visible: hovered
-    ToolTip.delay: tooltipDelay
-    ToolTip.timeout: tooltipTimeout
-    ToolTip.text: qsTr("Refresh list of topics publishing point clouds")
   }
 
-  ComboBox {
-    Layout.columnSpan: 5
-    id: combo
+  RowLayout {
+    spacing: 10
     Layout.fillWidth: true
-    model: VisualizePointCloud.topicList
-    currentIndex: 0
-    onCurrentIndexChanged: {
-      if (currentIndex < 0)
-        return;
-      VisualizePointCloud.OnTopic(textAt(currentIndex));
+
+    Label {
+      Layout.columnSpan: 1
+      text: "Min"
+    }
+
+    Label {
+      Layout.columnSpan: 1
+      Layout.maximumWidth: 50
+      text: VisualizePointCloud.minFloatV.toFixed(2)
+      elide: Text.ElideRight
+    }
+
+    Button {
+      Layout.columnSpan: 1
+      id: minColorButton
+      Layout.fillWidth: true
+      ToolTip.visible: hovered
+      ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
+      ToolTip.text: qsTr("Color for minimum value")
+      onClicked: minColorDialog.open()
+      background: Rectangle {
+        implicitWidth: 40
+        implicitHeight: 40
+        radius: 5
+        border.color: VisualizePointCloud.minColor
+        border.width: 2
+        color: VisualizePointCloud.minColor
+      }
+      ColorDialog {
+        id: minColorDialog
+        title: "Choose a color for the minimum value"
+        visible: false
+        onAccepted: {
+          VisualizePointCloud.SetMinColor(minColorDialog.color)
+          minColorDialog.close()
+        }
+        onRejected: {
+          minColorDialog.close()
+        }
+      }
+    }
+
+    Button {
+      Layout.columnSpan: 1
+      id: maxColorButton
+      Layout.fillWidth: true
+      ToolTip.visible: hovered
+      ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
+      ToolTip.text: qsTr("Color for maximum value")
+      onClicked: maxColorDialog.open()
+      background: Rectangle {
+        implicitWidth: 40
+        implicitHeight: 40
+        radius: 5
+        border.color: VisualizePointCloud.maxColor
+        border.width: 2
+        color: VisualizePointCloud.maxColor
+      }
+      ColorDialog {
+        id: maxColorDialog
+        title: "Choose a color for the maximum value"
+        visible: false
+        onAccepted: {
+          VisualizePointCloud.SetMaxColor(maxColorDialog.color)
+          maxColorDialog.close()
+        }
+        onRejected: {
+          maxColorDialog.close()
+        }
+      }
+    }
+
+    Label {
+      Layout.columnSpan: 1
+      Layout.maximumWidth: 50
+      text: VisualizePointCloud.maxFloatV.toFixed(2)
+      elide: Text.ElideRight
+    }
+
+    Label {
+      Layout.columnSpan: 1
+      text: "Max"
     }
-    ToolTip.visible: hovered
-    ToolTip.delay: tooltipDelay
-    ToolTip.timeout: tooltipTimeout
-    ToolTip.text: qsTr("Ignition transport topics publishing PointCloudPacked messages")
   }
 
   Item {

From c2871690e93b3e1e25c1c35f38cf74ba3e862987 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Tue, 23 Nov 2021 23:04:23 -0500
Subject: [PATCH 12/12] PR comments

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 .../src/ScienceSensorsSystem.cc               | 45 ++++++++++++++-----
 1 file changed, 35 insertions(+), 10 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index d94e18bc..917043e6 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -194,6 +194,9 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Publisher for point clouds representing positions for science data
   public: ignition::transport::Node::Publisher cloudPub;
 
+  /// \brief Name used for both the point cloud topic and service
+  public: std::string cloudTopic {"/science_data"};
+
   /// \brief Publisher for temperature
   public: ignition::transport::Node::Publisher tempPub;
 
@@ -522,6 +525,7 @@ void ScienceSensorsSystemPrivate::UpdateWorldSphericalOrigin(
   // 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);
@@ -537,24 +541,37 @@ void ScienceSensorsSystemPrivate::UpdateWorldSphericalOrigin(
       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);
 
-      this->worldSphericalCoordsInitialized = true;
+      igndbg << "Data will be transformed wrt world origin in Cartesian ("
+             << this->worldOriginCartesianCoords.X() << ", "
+             << this->worldOriginCartesianCoords.Y() << ", "
+             << this->worldOriginCartesianCoords.Z() << ")" << std::endl;
 
-      //TODO(chapulina) Shift science data to new coordinates
-      // If data had been loaded before origin was updated, need to shift data
-      if (this->initialized)
-      {
-        this->ShiftDataToNewSphericalOrigin();
-      }
+      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();
+  }
 }
 
 /////////////////////////////////////////////////
@@ -571,6 +588,13 @@ void ScienceSensorsSystemPrivate::ConvertLatLonToCart(
 
   // Set depth
   _cart.Z() = _latLonEle.Z();
+
+  /*
+  igndbg << "Data point at Cartesian ("
+         << _cart.X() << ", "
+         << _cart.Y() << ", "
+         << _cart.Z() << ")" << std::endl;
+  */
 }
 
 /////////////////////////////////////////////////
@@ -680,9 +704,9 @@ void ScienceSensorsSystem::Configure(
   }
 
   this->dataPtr->cloudPub = this->dataPtr->node.Advertise<
-      ignition::msgs::PointCloudPacked>("/science_data");
+      ignition::msgs::PointCloudPacked>(this->dataPtr->cloudTopic);
 
-  this->dataPtr->node.Advertise("/science_data_srv",
+  this->dataPtr->node.Advertise(this->dataPtr->cloudTopic,
       &ScienceSensorsSystemPrivate::ScienceDataService, this->dataPtr.get());
 
   // Advertise science data topics
@@ -693,7 +717,7 @@ void ScienceSensorsSystem::Configure(
   this->dataPtr->salPub = this->dataPtr->node.Advertise<
       ignition::msgs::Float_V>("/salinity");
 
-  // Initialize world origin in spherical coordinates, to data is loaded to the
+  // Initialize world origin in spherical coordinates, so data is loaded to the
   // correct Cartesian positions.
   this->dataPtr->UpdateWorldSphericalOrigin(_ecm);
 
@@ -709,6 +733,7 @@ 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);