From a2d0505de44853d908e4cad60eab9b02b3e755a9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 29 Nov 2021 16:27:52 -0800 Subject: [PATCH 1/4] Convert science data from lat/lon to ENU Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/data/simple_test.csv | 60 ++-- .../src/ScienceSensorsSystem.cc | 310 +++++++++--------- .../src/VisualizePointCloud.cc | 71 +--- .../worlds/buoyant_tethys.sdf | 8 +- 4 files changed, 200 insertions(+), 249 deletions(-) diff --git a/lrauv_ignition_plugins/data/simple_test.csv b/lrauv_ignition_plugins/data/simple_test.csv index 9d56e0b4..7449b802 100644 --- a/lrauv_ignition_plugins/data/simple_test.csv +++ b/lrauv_ignition_plugins/data/simple_test.csv @@ -1,31 +1,31 @@ elapsed_time_second,latitude_degree,longitude_degree,depth_meter,sea_water_temperature_degC,sea_water_salinity_psu,mass_concentration_of_chlorophyll_in_sea_water_ugram_per_liter,eastward_sea_water_velocity_meter_per_sec,northward_sea_water_velocity_meter_per_sec -0,0.001,0.000,0,-5.0,0.001,NaN,-1,0.5 -0,0.002,0.000,0,-5.1,0.002,NaN,-1,1.5 -0,0.003,0.000,0,-5.2,0.001,NaN,-1,0.5 -0,0.004,0.000,0,-5.3,0.002,0.1,-1,0.5 -0,0.005,0.000,0,-5.4,0.001,0.1,-1,0.5 -0,0.001,0.001,0,-5.5,0.002,0.1,-1,0.5 -0,0.002,0.001,0,-5.6,0.001,0.1,-1,0.5 -0,0.003,0.001,0,-5.7,0.002,0.1,-1,0.5 -0,0.004,0.001,0,-5.8,0.001,0.1,-1,1.5 -0,0.005,0.001,0,-5.9,0.002,0.1,-1,0.5 -0,0.001,0.002,0,-6.0,0.001,0.1,-1,0.5 -0,0.002,0.002,0,-6.1,0.002,NaN,-1,0.5 -0,0.003,0.002,0,-6.2,0.001,NaN,-1,0.5 -0,0.004,0.002,0,-6.3,0.002,NaN,-1,0.5 -0,0.005,0.002,0,-6.4,0.001,NaN,-1,1.5 -0,0.001,0.003,10,-6.5,0.002,NaN,-1,0.5 -0,0.002,0.003,10,-6.6,0.001,NaN,-1,0.5 -0,0.003,0.003,10,-6.7,0.002,NaN,-1,0.5 -0,0.004,0.003,10,-6.8,0.001,NaN,-1,0.5 -0,0.005,0.003,10,-6.9,0.002,NaN,-1,0.5 -0,0.001,0.004,10,-7.0,0.001,NaN,-1,0.5 -0,0.002,0.004,10,-7.1,0.002,0.1,-1,0.5 -0,0.003,0.004,10,-7.2,0.001,0.1,-1,1.5 -0,0.004,0.004,10,-7.3,0.002,0.1,-1,0.5 -0,0.005,0.004,10,-7.4,0.001,0.1,-1,0.5 -0,0.001,0.005,10,-7.5,0.002,0.1,-1,0.5 -0,0.002,0.005,10,-7.6,0.001,NaN,-1,0.5 -0,0.003,0.005,10,-7.7,0.002,NaN,-1,1.5 -0,0.004,0.005,10,-7.8,0.001,NaN,-1,0.5 -0,0.005,0.005,10,-7.9,0.002,NaN,-1,0.5 +0,0.0000001,0.0000000,0,-5.0,0.001,NaN,-1,0.5 +0,0.0000002,0.0000000,0,-5.1,0.002,NaN,-1,1.5 +0,0.0000003,0.0000000,0,-5.2,0.001,NaN,-1,0.5 +0,0.0000004,0.0000000,0,-5.3,0.002,0.1,-1,0.5 +0,0.0000005,0.0000000,0,-5.4,0.001,0.1,-1,0.5 +0,0.0000001,0.0000001,0,-5.5,0.002,0.1,-1,0.5 +0,0.0000002,0.0000001,0,-5.6,0.001,0.1,-1,0.5 +0,0.0000003,0.0000001,0,-5.7,0.002,0.1,-1,0.5 +0,0.0000004,0.0000001,0,-5.8,0.001,0.1,-1,1.5 +0,0.0000005,0.0000001,0,-5.9,0.002,0.1,-1,0.5 +0,0.0000001,0.0000002,0,-6.0,0.001,0.1,-1,0.5 +0,0.0000002,0.0000002,0,-6.1,0.002,NaN,-1,0.5 +0,0.0000003,0.0000002,0,-6.2,0.001,NaN,-1,0.5 +0,0.0000004,0.0000002,0,-6.3,0.002,NaN,-1,0.5 +0,0.0000005,0.0000002,0,-6.4,0.001,NaN,-1,1.5 +0,0.0000001,0.0000003,10,-6.5,0.002,NaN,-1,0.5 +0,0.0000002,0.0000003,10,-6.6,0.001,NaN,-1,0.5 +0,0.0000003,0.0000003,10,-6.7,0.002,NaN,-1,0.5 +0,0.0000004,0.0000003,10,-6.8,0.001,NaN,-1,0.5 +0,0.0000005,0.0000003,10,-6.9,0.002,NaN,-1,0.5 +0,0.0000001,0.0000004,10,-7.0,0.001,NaN,-1,0.5 +0,0.0000002,0.0000004,10,-7.1,0.002,0.1,-1,0.5 +0,0.0000003,0.0000004,10,-7.2,0.001,0.1,-1,1.5 +0,0.0000004,0.0000004,10,-7.3,0.002,0.1,-1,0.5 +0,0.0000005,0.0000004,10,-7.4,0.001,0.1,-1,0.5 +0,0.0000001,0.0000005,10,-7.5,0.002,0.1,-1,0.5 +0,0.0000002,0.0000005,10,-7.6,0.001,NaN,-1,0.5 +0,0.0000003,0.0000005,10,-7.7,0.002,NaN,-1,1.5 +0,0.0000004,0.0000005,10,-7.8,0.001,NaN,-1,0.5 +0,0.0000005,0.0000005,10,-7.9,0.002,NaN,-1,0.5 diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index f30e2f91..882972ca 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -44,7 +44,8 @@ using namespace tethys; class tethys::ScienceSensorsSystemPrivate { /// \brief Reads csv file and populate various data fields - public: void ReadData(); + /// \param[in] _ecm Immutable reference to the ECM + public: void ReadData(const ignition::gazebo::EntityComponentManager &_ecm); /// \brief Generate octree from spatial data, for searching public: void GenerateOctrees(); @@ -104,9 +105,6 @@ class tethys::ScienceSensorsSystemPrivate /// environment variables. public: std::string dataPath {"2003080103_mb_l3_las.csv"}; - /// \brief Indicates whether data has been loaded - public: bool initialized {false}; - /// \brief Mutex for writing to world origin association to lat/long public: std::mutex mtx; @@ -149,9 +147,16 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Octree for data search based on spatial location of sensor. One /// octree per point cloud in timeSpaceCoords. + /// Location stored in ENU world coordinates. public: std::vector> spatialOctrees; + /// \brief Set to true after the spherical coordinates have been initialized. + /// This may happen at startup if the SDF file has them hardcoded, or at + /// runtime when the first vehicle is spawned. Assume the coordinates are + /// only shifted once. + public: bool sphericalCoordinatesInitialized{false}; + /// \brief World object to access world properties. public: ignition::gazebo::World world; @@ -190,9 +195,9 @@ class tethys::ScienceSensorsSystemPrivate // For 2003080103_mb_l3_las_1x1km.csv //public: const float MINIATURE_SCALE = 0.01; // For 2003080103_mb_l3_las.csv - //public: const float MINIATURE_SCALE = 0.0001; + public: const float MINIATURE_SCALE = 0.000001; // For simple_test.csv - public: const float MINIATURE_SCALE = 1000.0; + //public: const float MINIATURE_SCALE = 1.0; // TODO This is a workaround pending upstream Marker performance improvements. // \brief Performance trick. Skip depths below this z, so have memory to @@ -269,10 +274,18 @@ ScienceSensorsSystem::ScienceSensorsSystem() } ///////////////////////////////////////////////// -void ScienceSensorsSystemPrivate::ReadData() +void ScienceSensorsSystemPrivate::ReadData( + const ignition::gazebo::EntityComponentManager &_ecm) { IGN_PROFILE("ScienceSensorsSystemPrivate::ReadData"); + if (!this->sphericalCoordinatesInitialized) + { + ignerr << "Trying to read data before spherical coordinates were " + << "initialized." << std::endl; + return; + } + // Lock modifications to world origin spherical association until finish // reading and transforming data std::lock_guard lock(mtx); @@ -437,8 +450,11 @@ void ScienceSensorsSystemPrivate::ReadData() continue; } - // TODO: Convert spherical coordinates to Cartesian - ignition::math::Vector3d cart = {latitude, longitude, -depth}; + // Convert lat / lon / elevation to Cartesian ENU + auto cart = this->world.SphericalCoordinates(_ecm).value() + .PositionTransform({latitude, longitude, -depth}, + ignition::math::SphericalCoordinates::SPHERICAL, + ignition::math::SphericalCoordinates::LOCAL2); // Performance trick. Scale down to see in view cart *= this->MINIATURE_SCALE; @@ -483,8 +499,6 @@ void ScienceSensorsSystemPrivate::ReadData() << this->timeSpaceCoords[i]->size() << " spatial coordinates." << std::endl; } - - this->initialized = true; } ///////////////////////////////////////////////// @@ -601,12 +615,6 @@ void ScienceSensorsSystem::Configure( ignition::msgs::Float_V>("/chlorophyll"); this->dataPtr->salPub = this->dataPtr->node.Advertise< ignition::msgs::Float_V>("/salinity"); - - this->dataPtr->ReadData(); - this->dataPtr->GenerateOctrees(); - - // Publish science data at the initial timestamp - this->dataPtr->PublishData(); } ///////////////////////////////////////////////// @@ -635,161 +643,161 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, IGN_PROFILE_THREAD_NAME("ScienceSensorsSystem PostUpdate"); IGN_PROFILE("ScienceSensorsSystem::PostUpdate"); - // Only update and publish if data has been loaded and simulation is not - // paused. - if (this->dataPtr->initialized && !_info.paused) - { - double simTimeSeconds = std::chrono::duration_cast( - _info.simTime).count(); + this->RemoveSensorEntities(_ecm); - // Update time index - if (this->dataPtr->multipleTimeSlices) + if (_info.paused) + return; + + // Delay reading data and generating octrees until spherical coordinates are + // received. + if (!this->dataPtr->sphericalCoordinatesInitialized) + { + if (this->dataPtr->world.SphericalCoordinates(_ecm)) { - // Only update if sim time exceeds the elapsed timestamp in data - if (!this->dataPtr->timestamps.empty() && - simTimeSeconds >= this->dataPtr->timestamps[this->dataPtr->timeIdx]) - { - // Increment for next point in time - this->dataPtr->timeIdx++; + this->dataPtr->sphericalCoordinatesInitialized = true; - // Publish science data at the next timestamp - this->dataPtr->PublishData(); - } + this->dataPtr->ReadData(_ecm); + this->dataPtr->GenerateOctrees(); } + } - // 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) + if (!this->dataPtr->sphericalCoordinatesInitialized) + { + ignwarn << "Science sensor data won't be published because spherical " + << "coordinates are unknown." << std::endl; + return; + } + + double simTimeSeconds = std::chrono::duration_cast( + _info.simTime).count(); + + // Update time index + if (this->dataPtr->multipleTimeSlices) + { + // Only update if sim time exceeds the elapsed timestamp in data + if (!this->dataPtr->timestamps.empty() && + simTimeSeconds >= this->dataPtr->timestamps[this->dataPtr->timeIdx]) { + // Increment for next point in time + this->dataPtr->timeIdx++; + + // Publish science data at the next timestamp 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) + // 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(); + // 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 ENU, used to search for data by spatial coordinates + auto sensorPosENU = ignition::gazebo::worldPose(entity, _ecm).Pos(); + pcl::PointXYZ searchPoint( + sensorPosENU.X(), + sensorPosENU.Y(), + sensorPosENU.Z()); + + // kNN search (alternatives are voxel search and radius search. kNN + // search is good for variable resolution when the distance to the next + // neighbor is unknown). + int k = 8; + + // Indices and distances of neighboring points in the search results + std::vector spatialIdx; + std::vector spatialSqrDist; + + // Search in octree to find spatial index of science data + if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].getLeafCount() + > 0) { - // 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 DEBUG what is the sensor attached to?? World? Not robot? - ignerr << "sensor lat long: " - << sensorLatLon.value().X() << ", " << sensorLatLon.value().Y() - << std::endl; - */ - if (!sensorLatLon) + IGN_PROFILE("ScienceSensorsSystem::PostUpdate nearestKSearch"); + if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].nearestKSearch( + searchPoint, k, spatialIdx, spatialSqrDist) <= 0) { - static std::unordered_set warnedEntities; - if (warnedEntities.find(entity) != warnedEntities.end()) - { - ignwarn << "Failed to get spherical coordinates for sensor entity [" - << entity << "]" << std::endl; - warnedEntities.insert(entity); - } + ignwarn << "No data found near sensor location " << sensorPosENU + << std::endl; continue; } - pcl::PointXYZ searchPoint( - sensorLatLon.value().X(), - sensorLatLon.value().Y(), - sensorLatLon.value().Z()); - - // kNN search (alternatives are voxel search and radius search. kNN - // search is good for variable resolution when the distance to the next - // neighbor is unknown). - int k = 8; - - // Indices and distances of neighboring points in the search results - std::vector spatialIdx; - std::vector spatialSqrDist; - - // Search in octree to find spatial index of science data - if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].getLeafCount() - > 0) + // Debug output + /* + else { - IGN_PROFILE("ScienceSensorsSystem::PostUpdate nearestKSearch"); - 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 - { - 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; - } - } - */ + igndbg << "kNN search for sensor pose (" << sensorPose.X() << ", " + << sensorPose.Y() << ", " << sensorPose.Z() << "):" + << std::endl; - // For the correct sensor, grab closest neighbors and interpolate - if (auto casted = std::dynamic_pointer_cast(sensor)) + for (std::size_t i = 0; i < spatialIdx.size(); i++) { - float sal = this->dataPtr->InterpolateData( - this->dataPtr->salinityArr[this->dataPtr->timeIdx], spatialIdx, - spatialSqrDist); - casted->SetData(sal); - } - else if (auto casted = std::dynamic_pointer_cast( - sensor)) - { - float temp = this->dataPtr->InterpolateData( - this->dataPtr->temperatureArr[this->dataPtr->timeIdx], spatialIdx, - spatialSqrDist); + // Index the point cloud at the current time slice + pcl::PointXYZ nbrPt = (*(this->dataPtr->timeSpaceCoords[ + this->dataPtr->timeIdx]))[spatialIdx[i]]; - ignition::math::Temperature tempC; - tempC.SetCelsius(temp); - casted->SetData(tempC); - } - else if (auto casted = std::dynamic_pointer_cast( - 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(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; + igndbg << "Neighbor at (" << nbrPt.x << ", " << nbrPt.y << ", " + << nbrPt.z << "), squared distance " << spatialSqrDist[i] + << " m" << std::endl; } } - sensor->Update(_info.simTime, false); + */ + + // For the correct sensor, grab closest neighbors and interpolate + if (auto casted = std::dynamic_pointer_cast(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( + 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( + 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(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); } - - this->RemoveSensorEntities(_ecm); } ////////////////////////////////////////////////// diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc index fb14e2c9..fad96d2f 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc @@ -84,52 +84,6 @@ namespace tethys /// fewer markers (faster performance). Recalulated in function. public: int renderEvery = 1; - /// \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; - - /// \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; - // For simple_test.csv - public: const float MINIATURE_SCALE = 1000.0; - - /// \brief Performance trick. Factor to multiply in calculating Marker sizes - // For 2003080103_mb_l3_las*.csv - //public: float dimFactor = 0.03; - // For simple_test.csv - public: float dimFactor = 0.003; - - /// \brief Parameter to calculate Marker size in x. - /// Performance trick. Hardcode resolution to make markers resemble voxels. - // For 2003080103_mb_l3_las_1x1km.csv - //public: const float RES_X = 15 * MINIATURE_SCALE; - // For 2003080103_mb_l3_las.csv - //public: const float RES_X = 15; - // For simple_test.csv - public: const float RES_X = 0.15; - - /// \brief Parameter to calculate Marker size in y. - // For 2003080103_mb_l3_las_1x1km.csv - //public: const float RES_Y = 22 * MINIATURE_SCALE; - // For 2003080103_mb_l3_las.csv - //public: const float RES_Y = 22; - // For simple_test.csv - public: const float RES_Y = 0.22; - - /// \brief Parameter to calculate Marker size in z. - // For 2003080103_mb_l3_las_1x1km.csv - //public: const float RES_Z = 5 * MINIATURE_SCALE; - // For 2003080103_mb_l3_las.csv - //public: const float RES_Z = 10; - // For simple_test.csv - public: const float RES_Z = 0.10; - /// \brief Minimum value in latest float vector public: float minFloatV{std::numeric_limits::max()}; @@ -403,6 +357,8 @@ void VisualizePointCloud::PublishMarkers() { this->dataPtr->renderEvery = (int) round( nPts / (double) this->dataPtr->MAX_PTS_VIS); + ignwarn << "Only rendering one science data point for each " + << this->dataPtr->renderEvery << std::endl; } ignition::msgs::Marker_V markers; @@ -421,8 +377,7 @@ void VisualizePointCloud::PublishMarkers() { // 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) + ptIdx % this->dataPtr->renderEvery != 0) { continue; } @@ -457,20 +412,8 @@ 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); - // 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::math::Vector3d(0.2, 0.2, 0.2)); ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d( *iterX, @@ -502,14 +445,14 @@ void VisualizePointCloud::PublishMarkers() << msg->pose().position().y() << ", " << msg->pose().position().z() << ", " << "value " << dataVal << ", " - << "dimX " << dimX << std::endl; } ++nPtsViz; } - igndbg << "Visualizing " << markers.marker().size() << " markers" - << std::endl; + igndbg << "Received [" << nPts + << "] markers, visualizing [" << markers.marker().size() << "]" + << std::endl; ignition::msgs::Boolean res; bool result; diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 3f07b4ac..7e73ea61 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -71,8 +71,8 @@ -121.829647676843 - 0 - 0 + 0 0 @@ -80,9 +80,9 @@ - + 2003080103_mb_l3_las.csv - simple_test.csv + From b7b859f0419f995458030a167b0473c8d1485ae4 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 29 Nov 2021 20:56:57 -0800 Subject: [PATCH 2/4] DTOR Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/src/ScienceSensorsSystem.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 882972ca..f81b831f 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/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::Ptr> timeSpaceCoords; /// \brief Science data. @@ -452,9 +453,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; // Performance trick. Scale down to see in view cart *= this->MINIATURE_SCALE; From 07bdce32f75a235e881068c69550d480a29b669c Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 29 Nov 2021 21:09:10 -0800 Subject: [PATCH 3/4] fix scale Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/data/simple_test.csv | 60 +++++++++---------- .../src/ScienceSensorsSystem.cc | 2 +- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/lrauv_ignition_plugins/data/simple_test.csv b/lrauv_ignition_plugins/data/simple_test.csv index 7449b802..5bd82da2 100644 --- a/lrauv_ignition_plugins/data/simple_test.csv +++ b/lrauv_ignition_plugins/data/simple_test.csv @@ -1,31 +1,31 @@ elapsed_time_second,latitude_degree,longitude_degree,depth_meter,sea_water_temperature_degC,sea_water_salinity_psu,mass_concentration_of_chlorophyll_in_sea_water_ugram_per_liter,eastward_sea_water_velocity_meter_per_sec,northward_sea_water_velocity_meter_per_sec -0,0.0000001,0.0000000,0,-5.0,0.001,NaN,-1,0.5 -0,0.0000002,0.0000000,0,-5.1,0.002,NaN,-1,1.5 -0,0.0000003,0.0000000,0,-5.2,0.001,NaN,-1,0.5 -0,0.0000004,0.0000000,0,-5.3,0.002,0.1,-1,0.5 -0,0.0000005,0.0000000,0,-5.4,0.001,0.1,-1,0.5 -0,0.0000001,0.0000001,0,-5.5,0.002,0.1,-1,0.5 -0,0.0000002,0.0000001,0,-5.6,0.001,0.1,-1,0.5 -0,0.0000003,0.0000001,0,-5.7,0.002,0.1,-1,0.5 -0,0.0000004,0.0000001,0,-5.8,0.001,0.1,-1,1.5 -0,0.0000005,0.0000001,0,-5.9,0.002,0.1,-1,0.5 -0,0.0000001,0.0000002,0,-6.0,0.001,0.1,-1,0.5 -0,0.0000002,0.0000002,0,-6.1,0.002,NaN,-1,0.5 -0,0.0000003,0.0000002,0,-6.2,0.001,NaN,-1,0.5 -0,0.0000004,0.0000002,0,-6.3,0.002,NaN,-1,0.5 -0,0.0000005,0.0000002,0,-6.4,0.001,NaN,-1,1.5 -0,0.0000001,0.0000003,10,-6.5,0.002,NaN,-1,0.5 -0,0.0000002,0.0000003,10,-6.6,0.001,NaN,-1,0.5 -0,0.0000003,0.0000003,10,-6.7,0.002,NaN,-1,0.5 -0,0.0000004,0.0000003,10,-6.8,0.001,NaN,-1,0.5 -0,0.0000005,0.0000003,10,-6.9,0.002,NaN,-1,0.5 -0,0.0000001,0.0000004,10,-7.0,0.001,NaN,-1,0.5 -0,0.0000002,0.0000004,10,-7.1,0.002,0.1,-1,0.5 -0,0.0000003,0.0000004,10,-7.2,0.001,0.1,-1,1.5 -0,0.0000004,0.0000004,10,-7.3,0.002,0.1,-1,0.5 -0,0.0000005,0.0000004,10,-7.4,0.001,0.1,-1,0.5 -0,0.0000001,0.0000005,10,-7.5,0.002,0.1,-1,0.5 -0,0.0000002,0.0000005,10,-7.6,0.001,NaN,-1,0.5 -0,0.0000003,0.0000005,10,-7.7,0.002,NaN,-1,1.5 -0,0.0000004,0.0000005,10,-7.8,0.001,NaN,-1,0.5 -0,0.0000005,0.0000005,10,-7.9,0.002,NaN,-1,0.5 +0,0.00001,0.00000,0,-5.0,0.001,NaN,-1,0.5 +0,0.00002,0.00000,0,-5.1,0.002,NaN,-1,1.5 +0,0.00003,0.00000,0,-5.2,0.001,NaN,-1,0.5 +0,0.00004,0.00000,0,-5.3,0.002,0.1,-1,0.5 +0,0.00005,0.00000,0,-5.4,0.001,0.1,-1,0.5 +0,0.00001,0.00001,0,-5.5,0.002,0.1,-1,0.5 +0,0.00002,0.00001,0,-5.6,0.001,0.1,-1,0.5 +0,0.00003,0.00001,0,-5.7,0.002,0.1,-1,0.5 +0,0.00004,0.00001,0,-5.8,0.001,0.1,-1,1.5 +0,0.00005,0.00001,0,-5.9,0.002,0.1,-1,0.5 +0,0.00001,0.00002,0,-6.0,0.001,0.1,-1,0.5 +0,0.00002,0.00002,0,-6.1,0.002,NaN,-1,0.5 +0,0.00003,0.00002,0,-6.2,0.001,NaN,-1,0.5 +0,0.00004,0.00002,0,-6.3,0.002,NaN,-1,0.5 +0,0.00005,0.00002,0,-6.4,0.001,NaN,-1,1.5 +0,0.00001,0.00003,10,-6.5,0.002,NaN,-1,0.5 +0,0.00002,0.00003,10,-6.6,0.001,NaN,-1,0.5 +0,0.00003,0.00003,10,-6.7,0.002,NaN,-1,0.5 +0,0.00004,0.00003,10,-6.8,0.001,NaN,-1,0.5 +0,0.00005,0.00003,10,-6.9,0.002,NaN,-1,0.5 +0,0.00001,0.00004,10,-7.0,0.001,NaN,-1,0.5 +0,0.00002,0.00004,10,-7.1,0.002,0.1,-1,0.5 +0,0.00003,0.00004,10,-7.2,0.001,0.1,-1,1.5 +0,0.00004,0.00004,10,-7.3,0.002,0.1,-1,0.5 +0,0.00005,0.00004,10,-7.4,0.001,0.1,-1,0.5 +0,0.00001,0.00005,10,-7.5,0.002,0.1,-1,0.5 +0,0.00002,0.00005,10,-7.6,0.001,NaN,-1,0.5 +0,0.00003,0.00005,10,-7.7,0.002,NaN,-1,1.5 +0,0.00004,0.00005,10,-7.8,0.001,NaN,-1,0.5 +0,0.00005,0.00005,10,-7.9,0.002,NaN,-1,0.5 diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index f81b831f..51e0233a 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -196,7 +196,7 @@ class tethys::ScienceSensorsSystemPrivate // For 2003080103_mb_l3_las_1x1km.csv //public: const float MINIATURE_SCALE = 0.01; // For 2003080103_mb_l3_las.csv - public: const float MINIATURE_SCALE = 0.000001; + public: const float MINIATURE_SCALE = 0.0001; // For simple_test.csv //public: const float MINIATURE_SCALE = 1.0; From 193447d8a42742c80e2e6074217f08baca714f00 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 3 Dec 2021 10:24:08 -0800 Subject: [PATCH 4/4] move if to else Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/src/ScienceSensorsSystem.cc | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 51e0233a..392f6e25 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -661,14 +661,15 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, this->dataPtr->ReadData(_ecm); this->dataPtr->GenerateOctrees(); } + else + { + // TODO(chapulina) Throttle if it becomes spammy + ignwarn << "Science sensor data won't be published because spherical " + << "coordinates are unknown." << std::endl; + return; + } } - if (!this->dataPtr->sphericalCoordinatesInitialized) - { - ignwarn << "Science sensor data won't be published because spherical " - << "coordinates are unknown." << std::endl; - return; - } double simTimeSeconds = std::chrono::duration_cast( _info.simTime).count();