Skip to content

Commit

Permalink
Visualize science data as points (#88)
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <louise@openrobotics.org>
  • Loading branch information
@chapulina
chapulina authored Dec 23, 2021
1 parent b25aeb7 commit d3d77b0
Show file tree
Hide file tree
Showing 4 changed files with 93 additions and 122 deletions.
20 changes: 0 additions & 20 deletions lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -295,14 +295,6 @@ class tethys::ScienceSensorsSystemPrivate

/// \brief Publish a few more times for visualization plugin to get them
public: int repeatPubTimes = 1;

// 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;
};

/////////////////////////////////////////////////
Expand Down Expand Up @@ -541,12 +533,6 @@ 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;
}

// Convert lat / lon / elevation to Cartesian ENU
auto cart = this->world.SphericalCoordinates(_ecm).value()
.PositionTransform({IGN_DTOR(latitude), IGN_DTOR(longitude), 0.0},
Expand All @@ -555,12 +541,6 @@ void ScienceSensorsSystemPrivate::ReadData(
// Flip sign of z, because positive depth is negative z.
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(
Expand Down
140 changes: 45 additions & 95 deletions lrauv_ignition_plugins/src/VisualizePointCloud.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,14 +76,6 @@ namespace tethys
/// \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 Minimum value in latest float vector
public: float minFloatV{std::numeric_limits<float>::max()};

Expand All @@ -96,6 +88,9 @@ namespace tethys
/// \brief Color for maximum value
public: ignition::math::Color maxColor{0, 255, 0, 255};

/// \brief Size of each point
public: float pointSize{20};

/// \brief True if showing
public: bool showing{true};
};
Expand Down Expand Up @@ -305,6 +300,10 @@ void VisualizePointCloud::OnFloatV(const ignition::msgs::Float_V &_msg)
this->SetMaxFloatV(data);
}

// TODO(chapulina) Publishing whenever we get a new point cloud and a new
// floatV is good in case these topics are out of sync. But here they're
// synchronized, so in practice we're publishing markers twice for each
// PC+float that we get.
this->PublishMarkers();
}

Expand Down Expand Up @@ -349,40 +348,29 @@ void VisualizePointCloud::PublishMarkers()
}

std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
ignition::msgs::Marker marker;
marker.set_ns(this->dataPtr->pointCloudTopic + this->dataPtr->floatVTopic);
marker.set_id(1);
marker.set_action(ignition::msgs::Marker::ADD_MODIFY);
marker.set_type(ignition::msgs::Marker::POINTS);
marker.set_visibility(ignition::msgs::Marker::GUI);

// 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);
ignwarn << "Only rendering one science data point for each "
<< this->dataPtr->renderEvery << std::endl;
}

ignition::msgs::Marker_V markers;
ignition::msgs::Set(marker.mutable_scale(),
ignition::math::Vector3d::One * this->dataPtr->pointSize);

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};
auto minC = this->dataPtr->minColor;
auto maxC = this->dataPtr->maxColor;
auto floatRange = this->dataPtr->maxFloatV - this->dataPtr->minFloatV;
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)
{
continue;
}

// Value from float vector, if available. Otherwise publish all data as
// zeroes.
float dataVal = 0.0;
Expand All @@ -391,82 +379,30 @@ void VisualizePointCloud::PublishMarkers()
dataVal = this->dataPtr->floatVMsg.data(ptIdx);
}

auto msg = markers.add_marker();

msg->set_ns(this->dataPtr->pointCloudTopic + this->dataPtr->floatVTopic);
msg->set_id(ptIdx + 1);

// Don't visualize NaN
if (std::isnan(dataVal))
{
msg->set_action(ignition::msgs::Marker::DELETE_MARKER);
continue;
}
msg->set_action(ignition::msgs::Marker::ADD_MODIFY);

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);
auto ratio = floatRange > 0 ?
(dataVal - this->dataPtr->minFloatV) / floatRange : 0.0f;
ignition:: math::Color color{
minC.R() + (maxC.R() - minC.R()) * ratio,
minC.G() + (maxC.G() - minC.G()) * ratio,
minC.B() + (maxC.B() - minC.B()) * ratio
};

// 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(0.2, 0.2, 0.2));
ignition::msgs::Set(marker.add_materials()->mutable_diffuse(), color);

ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
ignition::msgs::Set(marker.add_point(), ignition::math::Vector3d(
*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 << ", "
<< std::endl;
}
++nPtsViz;
*iterZ));
}

igndbg << "Received [" << nPts
<< "] markers, visualizing [" << nPtsViz << "]"
<< std::endl;

ignition::msgs::Boolean res;
bool result;
unsigned int timeout = 5000;
this->dataPtr->node.Request("/marker_array", markers, timeout, res, result);
igndbg << "Visualizing " << marker.point_size() << " points"
<< std::endl;

if (!result || !res.data())
{
ignerr << "Failed to create markers on /marker_array" << std::endl;
}
this->dataPtr->node.Request("/marker", marker);
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -542,6 +478,20 @@ void VisualizePointCloud::SetMaxFloatV(float _maxFloatV)
this->MaxFloatVChanged();
}

/////////////////////////////////////////////////
float VisualizePointCloud::PointSize() const
{
return this->dataPtr->pointSize;
}

/////////////////////////////////////////////////
void VisualizePointCloud::SetPointSize(float _pointSize)
{
this->dataPtr->pointSize = _pointSize;
this->PointSizeChanged();
this->PublishMarkers();
}

// Register this plugin
IGNITION_ADD_PLUGIN(tethys::VisualizePointCloud,
ignition::gui::Plugin)
19 changes: 19 additions & 0 deletions lrauv_ignition_plugins/src/VisualizePointCloud.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,14 @@ namespace tethys
NOTIFY MaxFloatVChanged
)

/// \brief Point size
Q_PROPERTY(
float pointSize
READ PointSize
WRITE SetPointSize
NOTIFY PointSizeChanged
)

/// \brief Constructor
public: VisualizePointCloud();

Expand Down Expand Up @@ -196,6 +204,17 @@ namespace tethys
/// \brief Notify that maximum value has changed
signals: void MaxFloatVChanged();

/// \brief Get the point size
/// \return Maximum value
public: Q_INVOKABLE float PointSize() const;

/// \brief Set the point size
/// \param[ax] _pointSize Maximum value.
public: Q_INVOKABLE void SetPointSize(float _pointSize);

/// \brief Notify that point size has changed
signals: void PointSizeChanged();

/// \brief Set whether to show the point cloud.
/// \param[in] _show Boolean value for displaying the points.
public: Q_INVOKABLE void Show(bool _show);
Expand Down
36 changes: 29 additions & 7 deletions lrauv_ignition_plugins/src/VisualizePointCloud.qml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,11 +30,15 @@ import "qrc:/qml"
ColumnLayout {
spacing: 10
Layout.minimumWidth: 350
Layout.minimumHeight: 300
Layout.minimumHeight: 350
anchors.fill: parent
anchors.leftMargin: 10
anchors.rightMargin: 10

function isUniform() {
return VisualizePointCloud.minFloatV >= VisualizePointCloud.maxFloatV
}

RowLayout {
spacing: 10
Layout.fillWidth: true
Expand Down Expand Up @@ -110,7 +114,23 @@ ColumnLayout {
}
ToolTip.visible: hovered
ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
ToolTip.text: qsTr("Ignition transport topics publishing FloatV messages")
ToolTip.text: qsTr("Ignition transport topics publishing FloatV messages, used to color each point on the cloud")
}

Label {
Layout.columnSpan: 1
text: "Point size"
}

IgnSpinBox {
id: pointSizeSpin
value: VisualizePointCloud.pointSize
minimumValue: 1
maximumValue: 1000
decimals: 0
onEditingFinished: {
VisualizePointCloud.SetPointSize(pointSizeSpin.value)
}
}
}

Expand All @@ -120,20 +140,20 @@ ColumnLayout {

Label {
Layout.columnSpan: 1
text: "Min"
text: isUniform() ? "Color" : "Min"
}

Label {
Layout.columnSpan: 1
Layout.maximumWidth: 50
text: VisualizePointCloud.minFloatV.toFixed(2)
text: VisualizePointCloud.minFloatV.toFixed(4)
elide: Text.ElideRight
visible: !isUniform()
}

Button {
Layout.columnSpan: 1
id: minColorButton
Layout.fillWidth: true
ToolTip.visible: hovered
ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
ToolTip.text: qsTr("Color for minimum value")
Expand Down Expand Up @@ -163,7 +183,7 @@ ColumnLayout {
Button {
Layout.columnSpan: 1
id: maxColorButton
Layout.fillWidth: true
visible: !isUniform()
ToolTip.visible: hovered
ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
ToolTip.text: qsTr("Color for maximum value")
Expand Down Expand Up @@ -193,13 +213,15 @@ ColumnLayout {
Label {
Layout.columnSpan: 1
Layout.maximumWidth: 50
text: VisualizePointCloud.maxFloatV.toFixed(2)
text: VisualizePointCloud.maxFloatV.toFixed(4)
elide: Text.ElideRight
visible: !isUniform()
}

Label {
Layout.columnSpan: 1
text: "Max"
visible: !isUniform()
}
}

Expand Down

0 comments on commit d3d77b0

Please sign in to comment.