DTOR

Signed-off-by: Louise Poubel <louise@openrobotics.org>

lrauv_ignition_plugins/src/ScienceSensorsSystem.cc

@@ -121,6 +121,7 @@ class tethys::ScienceSensorsSystemPrivate
   /// Vector size: number of time slices. Indices correspond to those of
   /// this->timestamps.
   /// Point cloud: spatial coordinates to index science data by location
+  /// in the ENU world frame.
   public: std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> timeSpaceCoords;
 
   /// \brief Science data.
@@ -429,9 +430,10 @@ void ScienceSensorsSystemPrivate::ReadData(
       {
         // Convert lat / lon / elevation to Cartesian ENU
         auto cart = this->world.SphericalCoordinates(_ecm).value()
-            .PositionTransform({latitude, longitude, -depth},
+            .PositionTransform({IGN_DTOR(latitude), IGN_DTOR(longitude), 0.0},
             ignition::math::SphericalCoordinates::SPHERICAL,
             ignition::math::SphericalCoordinates::LOCAL2);
+        cart.Z() = -depth;
 
         // Gather spatial coordinates, 3 fields in the line, into point cloud
         // for indexing this time slice of data.
@@ -819,11 +821,6 @@ ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg()
       {"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;

lrauv_ignition_plugins/src/VisualizePointCloud.cc

@@ -362,8 +362,6 @@ void VisualizePointCloud::PublishMarkers()
 
   // Index of point in point cloud, visualized or not
   int ptIdx{0};
-  // Number of points actually visualized
-  int nPtsViz{0};
   auto minC = this->dataPtr->minColor;
   auto maxC = this->dataPtr->maxColor;
   for (;iterX != iterX.end() &&
@@ -401,8 +399,6 @@ void VisualizePointCloud::PublishMarkers()
       *iterX,
       *iterY,
       *iterZ));
-
-    ++nPtsViz;
   }
 
   igndbg << "Visualizing " << marker.point_size() << " points"