From c26260b829ec55c4adcf7de78ac2942516b441f9 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 29 Nov 2021 15:56:04 -0500 Subject: [PATCH] Science data visualization only, no lat/long to Cartesian conversion (#93) Signed-off-by: Mabel Zhang Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- README.md | 5 + .../src/ScienceSensorsSystem.cc | 284 ++++++++---- .../src/VisualizePointCloud.cc | 437 +++++++++++++++--- .../src/VisualizePointCloud.hh | 144 +++++- .../src/VisualizePointCloud.qml | 198 ++++++-- lrauv_ignition_plugins/src/WorldCommPlugin.cc | 6 +- .../worlds/buoyant_tethys.sdf | 37 +- .../worlds/empty_environment.sdf | 9 +- 8 files changed, 893 insertions(+), 227 deletions(-) diff --git a/README.md b/README.md index 504ea184..6501cdb4 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" +``` + > You can pass `--cmake-args ' -DENABLE_PROFILER=1'` to use the profiler. > See more on [this tutorial](https://ignitionrobotics.org/api/common/4.4/profiler.html) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 0b6ca76a..f30e2f91 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -20,11 +20,14 @@ * Research Institute (MBARI) and the David and Lucile Packard Foundation */ +#include + #include #include #include #include +#include #include #include #include @@ -49,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 @@ -100,13 +105,16 @@ class tethys::ScienceSensorsSystemPrivate public: std::string dataPath {"2003080103_mb_l3_las.csv"}; /// \brief Indicates whether data has been loaded - public: bool initialized = false; + public: bool initialized {false}; + + /// \brief Mutex for writing to world origin association to lat/long + public: std::mutex mtx; /// \brief Whether using more than one time slices of data - public: bool multipleTimeSlices = false; + public: bool multipleTimeSlices {false}; /// \brief Index of the latest time slice - public: int timeIdx = 0; + public: int timeIdx {0}; /// \brief Timestamps to index slices of data public: std::vector timestamps; @@ -142,13 +150,7 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Octree for data search based on spatial location of sensor. One /// octree per point cloud in timeSpaceCoords. 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}; + spatialOctrees; /// \brief World object to access world properties. public: ignition::gazebo::World world; @@ -156,8 +158,49 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Node for communication 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; + + /// \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; + + /// \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; + + /// \brief Publish a few more times for visualization plugin to get them + public: int repeatPubTimes = 1; + + // TODO This is a workaround pending upstream Ignition orbit tool improvements + // \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; + // For simple_test.csv + public: const float MINIATURE_SCALE = 1000.0; + + // TODO This is a workaround pending upstream Marker performance improvements. + // \brief Performance trick. Skip depths below this z, so have memory to + // 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; }; ///////////////////////////////////////////////// @@ -230,6 +273,10 @@ void ScienceSensorsSystemPrivate::ReadData() { IGN_PROFILE("ScienceSensorsSystemPrivate::ReadData"); + // Lock modifications to world origin spherical association until finish + // reading and transforming data + std::lock_guard lock(mtx); + std::fstream fs; fs.open(this->dataPath, std::ios::in); @@ -384,10 +431,30 @@ void ScienceSensorsSystemPrivate::ReadData() // Check validity of spatial coordinates if (!std::isnan(latitude) && !std::isnan(longitude) && !std::isnan(depth)) { + // Performance trick. Skip points below a certain depth + if (-depth < this->SKIP_Z_BELOW) + { + continue; + } + + // TODO: Convert spherical coordinates to Cartesian + ignition::math::Vector3d cart = {latitude, longitude, -depth}; + + // 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) + { + continue; + } + // Gather spatial coordinates, 3 fields in the line, into point cloud // for indexing this time slice of data. this->timeSpaceCoords[lineTimeIdx]->push_back( - pcl::PointXYZ(latitude, longitude, depth)); + pcl::PointXYZ(cart.X(), cart.Y(), cart.Z())); // Populate science data this->temperatureArr[lineTimeIdx].push_back(temp); @@ -440,6 +507,9 @@ void ScienceSensorsSystemPrivate::PublishData() { IGN_PROFILE("ScienceSensorsSystemPrivate::PublishData"); this->cloudPub.Publish(this->PointCloudMsg()); + this->tempPub.Publish(this->tempMsg); + this->chlorPub.Publish(this->chlorMsg); + this->salPub.Publish(this->salMsg); } ///////////////////////////////////////////////// @@ -519,13 +589,23 @@ 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", + this->dataPtr->node.Advertise(this->dataPtr->cloudTopic, &ScienceSensorsSystemPrivate::ScienceDataService, this->dataPtr.get()); + // Advertise science data topics + 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"); + this->dataPtr->ReadData(); this->dataPtr->GenerateOctrees(); + + // Publish science data at the initial timestamp this->dataPtr->PublishData(); } @@ -534,17 +614,6 @@ void ScienceSensorsSystem::PreUpdate( const ignition::gazebo::UpdateInfo &, ignition::gazebo::EntityComponentManager &_ecm) { - if (!this->dataPtr->sphericalCoordinatesInitialized) - { - auto latLon = this->dataPtr->world.SphericalCoordinates(_ecm); - if (latLon) - { - ignwarn << "New spherical coordinates detected." << std::endl; - //TODO(chapulina) Shift science data to new coordinates - this->dataPtr->sphericalCoordinatesInitialized = true; - } - } - _ecm.EachNew( [&](const ignition::gazebo::Entity &_entity, @@ -582,16 +651,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(); } } + // 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 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) { static std::unordered_set warnedEntities; @@ -618,6 +709,8 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, std::vector spatialSqrDist; // Search in octree to find spatial index of science data + if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].getLeafCount() + > 0) { IGN_PROFILE("ScienceSensorsSystem::PostUpdate nearestKSearch"); if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx].nearestKSearch( @@ -627,70 +720,70 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, << 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++) + // Debug output + /* + else { - // 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 (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(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); + // 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; + 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); } @@ -749,12 +842,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; @@ -767,10 +862,17 @@ 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()); memcpy(msg.mutable_data()->data(), pclPC2.data.data(), pclPC2.data.size()); + // 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(), + 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 e47fadd6..fb14e2c9 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc @@ -34,9 +34,12 @@ #include +#include #include #include +#include + #include #include @@ -52,17 +55,95 @@ namespace tethys /// \brief Transport node public: ignition::transport::Node node; - /// \brief Topic name to subscribe - public: std::string topicName{""}; + /// \brief Name of topic for PointCloudPacked + public: std::string pointCloudTopic{""}; + + /// \brief Name of topic for FloatV + public: std::string floatVTopic{""}; - /// \brief List of topics publishing LaserScan messages. - public: QStringList topicList; + /// \brief List of topics publishing PointCloudPacked. + public: QStringList pointCloudTopicList; + + /// \brief List of topics publishing FloatV. + public: QStringList floatVTopicList; /// \brief Protect variables changed from transport and the user public: std::recursive_mutex mutex; - /// \brief Latest point cloud message - public: ignition::msgs::PointCloudPacked pcMsg; + /// \brief Point cloud message containing location of data + public: ignition::msgs::PointCloudPacked pointCloudMsg; + + /// \brief Message holding a float vector. + public: ignition::msgs::Float_V floatVMsg; + + /// \brief Performance trick. Cap number of points to visualize, to save + /// memory. + public: const int MAX_PTS_VIS = 1000; + + /// \brief Performance trick. Render only every other n. Increase to render + /// 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()}; + + /// \brief Maximum value in latest float vector + public: float maxFloatV{-std::numeric_limits::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}; }; } @@ -92,39 +173,73 @@ void VisualizePointCloud::LoadConfig(const tinyxml2::XMLElement *) } ////////////////////////////////////////////////// -void VisualizePointCloud::OnTopic(const QString &_topicName) +void VisualizePointCloud::OnPointCloudTopic(const QString &_pointCloudTopic) { std::lock_guard(this->dataPtr->mutex); - if (!this->dataPtr->topicName.empty() && - !this->dataPtr->node.Unsubscribe(this->dataPtr->topicName)) + // Unsubscribe from previous choice + if (!this->dataPtr->pointCloudTopic.empty() && + !this->dataPtr->node.Unsubscribe(this->dataPtr->pointCloudTopic)) { ignerr << "Unable to unsubscribe from topic [" - << this->dataPtr->topicName <<"]" <dataPtr->pointCloudTopic <<"]" <ClearMarkers(); - this->dataPtr->topicName = _topicName.toStdString(); + this->dataPtr->pointCloudTopic = _pointCloudTopic.toStdString(); // Request service - this->dataPtr->node.Request(this->dataPtr->topicName, - &VisualizePointCloud::OnService, this); + this->dataPtr->node.Request(this->dataPtr->pointCloudTopic, + &VisualizePointCloud::OnPointCloudService, this); // Create new subscription - if (!this->dataPtr->node.Subscribe(this->dataPtr->topicName, - &VisualizePointCloud::OnMessage, this)) + if (!this->dataPtr->node.Subscribe(this->dataPtr->pointCloudTopic, + &VisualizePointCloud::OnPointCloud, this)) { ignerr << "Unable to subscribe to topic [" - << this->dataPtr->topicName << "]\n"; + << this->dataPtr->pointCloudTopic << "]\n"; return; } - ignmsg << "Subscribed to " << this->dataPtr->topicName << std::endl; + ignmsg << "Subscribed to " << this->dataPtr->pointCloudTopic << std::endl; +} + +////////////////////////////////////////////////// +void VisualizePointCloud::OnFloatVTopic(const QString &_floatVTopic) +{ + std::lock_guard(this->dataPtr->mutex); + // Unsubscribe from previous choice + if (!this->dataPtr->floatVTopic.empty() && + !this->dataPtr->node.Unsubscribe(this->dataPtr->floatVTopic)) + { + ignerr << "Unable to unsubscribe from topic [" + << this->dataPtr->floatVTopic <<"]" <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(); @@ -142,7 +257,10 @@ 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; // Get updated list std::vector allTopics; @@ -155,53 +273,108 @@ void VisualizePointCloud::OnRefresh() { if (pub.MsgTypeName() == "ignition.msgs.PointCloudPacked") { - this->dataPtr->topicList.push_back(QString::fromStdString(topic)); - break; + this->dataPtr->pointCloudTopicList.push_back( + QString::fromStdString(topic)); + } + else if (pub.MsgTypeName() == "ignition.msgs.Float_V") + { + this->dataPtr->floatVTopicList.push_back(QString::fromStdString(topic)); } } } - if (this->dataPtr->topicList.size() > 0) + if (this->dataPtr->pointCloudTopicList.size() > 0) { - this->OnTopic(this->dataPtr->topicList.at(0)); + this->OnPointCloudTopic(this->dataPtr->pointCloudTopicList.at(0)); + } + if (this->dataPtr->floatVTopicList.size() > 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(); +} + +///////////////////////////////////////////////// +QStringList VisualizePointCloud::FloatVTopicList() const +{ + return this->dataPtr->floatVTopicList; +} + +///////////////////////////////////////////////// +void VisualizePointCloud::SetFloatVTopicList( + const QStringList &_floatVTopicList) +{ + this->dataPtr->floatVTopicList = _floatVTopicList; + this->FloatVTopicListChanged(); } ////////////////////////////////////////////////// -void VisualizePointCloud::OnMessage(const ignition::msgs::PointCloudPacked &_msg) +void VisualizePointCloud::OnPointCloud( + const ignition::msgs::PointCloudPacked &_msg) { std::lock_guard(this->dataPtr->mutex); - this->dataPtr->pcMsg = _msg; + this->dataPtr->pointCloudMsg = _msg; + this->PublishMarkers(); +} + +////////////////////////////////////////////////// +void VisualizePointCloud::OnFloatV(const ignition::msgs::Float_V &_msg) +{ + std::lock_guard(this->dataPtr->mutex); + this->dataPtr->floatVMsg = _msg; + + this->dataPtr->minFloatV = std::numeric_limits::max(); + this->dataPtr->maxFloatV = -std::numeric_limits::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::OnService(const ignition::msgs::PointCloudPacked &_res, - bool _result) +void VisualizePointCloud::OnPointCloudService( + const ignition::msgs::PointCloudPacked &_msg, bool _result) { if (!_result) { ignerr << "Service request failed." << std::endl; return; } + this->OnPointCloud(_msg); +} - std::lock_guard(this->dataPtr->mutex); - this->dataPtr->pcMsg = _res; - this->PublishMarkers(); +////////////////////////////////////////////////// +void VisualizePointCloud::OnFloatVService( + const ignition::msgs::Float_V &_msg, bool _result) +{ + if (!_result) + { + ignerr << "Service request failed." << std::endl; + return; + } + this->OnFloatV(_msg); } ////////////////////////////////////////////////// @@ -209,55 +382,135 @@ void VisualizePointCloud::PublishMarkers() { IGN_PROFILE("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; } std::lock_guard(this->dataPtr->mutex); - ignition::msgs::Marker_V markers; - PointCloudPackedIterator iter_x(this->dataPtr->pcMsg, "x"); - PointCloudPackedIterator iter_y(this->dataPtr->pcMsg, "y"); - PointCloudPackedIterator iter_z(this->dataPtr->pcMsg, "z"); + // Used to calculate cap of number of points to visualize, to save memory + int nPts = this->dataPtr->pointCloudMsg.height() * + this->dataPtr->pointCloudMsg.width(); + // If there are more points than we can render, render every n + if (nPts > this->dataPtr->MAX_PTS_VIS) + { + this->dataPtr->renderEvery = (int) round( + nPts / (double) this->dataPtr->MAX_PTS_VIS); + } + + ignition::msgs::Marker_V markers; - int count{0}; - while (iter_x != iter_x.end() && - iter_y != iter_y.end() && - iter_z != iter_z.end()) + PointCloudPackedIterator iterX(this->dataPtr->pointCloudMsg, "x"); + PointCloudPackedIterator iterY(this->dataPtr->pointCloudMsg, "y"); + PointCloudPackedIterator 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}; + 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) + { + continue; + } + + // Value from float vector + float dataVal = std::numeric_limits::quiet_NaN(); + if (this->dataPtr->floatVMsg.data().size() > ptIdx) + { + dataVal = this->dataPtr->floatVMsg.data(ptIdx); + } + + // Don't visualize NaN + if (std::isnan(dataVal)) + continue; + 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_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::One); + ignition::math::Vector3d(dimX, dimY, dimZ)); ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d( - *iter_x, - *iter_y, - *iter_z, + *iterX, + *iterY, + *iterZ, 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; + /* + // 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" + << std::endl; + ignition::msgs::Boolean res; bool result; unsigned int timeout = 5000; @@ -274,13 +527,71 @@ void VisualizePointCloud::ClearMarkers() { std::lock_guard(this->dataPtr->mutex); ignition::msgs::Marker msg; - msg.set_ns(this->dataPtr->topicName); + msg.set_ns(this->dataPtr->pointCloudTopic + "-" + this->dataPtr->floatVTopic); msg.set_id(0); msg.set_action(ignition::msgs::Marker::DELETE_ALL); + igndbg << "Clearing markers on " + << this->dataPtr->pointCloudTopic + "-" + this->dataPtr->floatVTopic + << std::endl; + 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 3529ff75..772ac9f8 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.hh +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.hh @@ -25,6 +25,7 @@ #include +#include "ignition/msgs/float_v.pb.h" #include "ignition/msgs/pointcloud_packed.pb.h" #include @@ -40,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 @@ -55,30 +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 OnMessage(const ignition::msgs::PointCloudPacked &_msg); + public: void OnPointCloud(const ignition::msgs::PointCloudPacked &_msg); + + /// \brief Callback function for point cloud service + /// \param[in]_msg Point cloud message + /// \param[out]_result True on success. + 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 Callback function to get data from the message - /// \param[in]_res Point cloud message + /// \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 OnService(const ignition::msgs::PointCloudPacked &_res, - bool _result); + 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 146bd3e3..6bf90183 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.qml +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.qml @@ -27,59 +27,181 @@ 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: { - combo.currentIndex = 0 - VisualizePointCloud.OnRefresh(); + 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") } - 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 + GridLayout { + columns: 3 + columnSpacing: 10 Layout.fillWidth: true - model: VisualizePointCloud.topicList - currentIndex: 0 - onCurrentIndexChanged: { - if (currentIndex < 0) - return; - VisualizePointCloud.OnTopic(textAt(currentIndex)); + + 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("Ignition transport topics publishing PointCloudPacked messages") } + RowLayout { + spacing: 10 + Layout.fillWidth: true + + 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" + } + } Item { Layout.columnSpan: 6 diff --git a/lrauv_ignition_plugins/src/WorldCommPlugin.cc b/lrauv_ignition_plugins/src/WorldCommPlugin.cc index 5b43fd87..6a3b40c5 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 &_sdf, @@ -106,13 +107,15 @@ void WorldCommPlugin::Configure( + "/set_spherical_coordinates"; } +///////////////////////////////////////////////// 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) { @@ -174,6 +177,7 @@ void WorldCommPlugin::SpawnCallback( } } +///////////////////////////////////////////////// std::string WorldCommPlugin::TethysSdfString(const std::string &_id) { const std::string sdfStr = R"( diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 202555be..3f07b4ac 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -13,18 +13,6 @@ 0.0 0.7 0.8 - - - EARTH_WGS84 - ENU - 0 - 0 - 0 - 0 - - 0.02 0 @@ -69,11 +57,32 @@ name="tethys::TimeAnalysisPlugin"> + + + EARTH_WGS84 + ENU + + + 35.5999984741211 + -121.779998779297 + + + + + + 0 + 0 + + 0 + 0 + - 2003080103_mb_l3_las.csv - + + + simple_test.csv diff --git a/lrauv_ignition_plugins/worlds/empty_environment.sdf b/lrauv_ignition_plugins/worlds/empty_environment.sdf index ea514d4a..c50029ba 100644 --- a/lrauv_ignition_plugins/worlds/empty_environment.sdf +++ b/lrauv_ignition_plugins/worlds/empty_environment.sdf @@ -54,12 +54,13 @@ name="tethys::TimeAnalysisPlugin"> - + - dummy.csv - + 2003080103_mb_l3_las.csv +