diff --git a/plugin_description.xml b/plugin_description.xml
index fb0152d531..465bb454d9 100644
--- a/plugin_description.xml
+++ b/plugin_description.xml
@@ -188,6 +188,11 @@
Sets the color of each point based on its position along one of the X, Y, or Z axes.
+
+
+ Transforms the color of each point based on its "label" value.
+
+
Displays from sensor_msgs/Temperature message
diff --git a/src/rviz/CMakeLists.txt b/src/rviz/CMakeLists.txt
index db5203421b..9f78781991 100644
--- a/src/rviz/CMakeLists.txt
+++ b/src/rviz/CMakeLists.txt
@@ -83,6 +83,7 @@ add_library(${PROJECT_NAME}
properties/float_edit.cpp
properties/float_property.cpp
properties/int_property.cpp
+ properties/label_property.cpp
properties/line_edit_with_button.cpp
properties/parse_color.cpp
properties/property.cpp
diff --git a/src/rviz/default_plugin/point_cloud_transformers.cpp b/src/rviz/default_plugin/point_cloud_transformers.cpp
index 3fc31d285d..42c69ec1c1 100644
--- a/src/rviz/default_plugin/point_cloud_transformers.cpp
+++ b/src/rviz/default_plugin/point_cloud_transformers.cpp
@@ -36,6 +36,9 @@
#include "rviz/properties/editable_enum_property.h"
#include "rviz/properties/enum_property.h"
#include "rviz/properties/float_property.h"
+#include "rviz/properties/int_property.h"
+#include "rviz/properties/label_property.h"
+#include "rviz/properties/string_property.h"
#include "rviz/validate_floats.h"
#include "point_cloud_transformers.h"
@@ -620,12 +623,206 @@ void AxisColorPCTransformer::updateAutoComputeBounds()
Q_EMIT needRetransform();
}
+LabelPCTransformer::LabelPCTransformer() :
+ use_rainbow_(NULL),
+ default_color_(NULL),
+ labels_(NULL)
+{
+}
+
+uint8_t LabelPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
+{
+ int32_t i = findChannelIndex(cloud, "label");
+ if (i == -1)
+ {
+ // no channel
+ return Support_None;
+ }
+
+ if (cloud->fields[i].datatype != sensor_msgs::PointField::UINT8 &&
+ cloud->fields[i].datatype != sensor_msgs::PointField::UINT16 &&
+ cloud->fields[i].datatype != sensor_msgs::PointField::UINT32 &&
+ cloud->fields[i].datatype != sensor_msgs::PointField::INT8 &&
+ cloud->fields[i].datatype != sensor_msgs::PointField::INT16 &&
+ cloud->fields[i].datatype != sensor_msgs::PointField::INT32)
+ {
+ // not integer data
+ return Support_None;
+ }
+
+ return Support_Color;
+}
+
+bool LabelPCTransformer::transform(
+ const sensor_msgs::PointCloud2ConstPtr& cloud,
+ uint32_t mask,
+ const Ogre::Matrix4& transform,
+ V_PointCloudPoint& points_out)
+{
+ if (!(mask & Support_Color))
+ {
+ return false;
+ }
+
+ const int32_t i = findChannelIndex(cloud, "label");
+ const uint8_t dtype = cloud->fields[i].datatype;
+ const uint32_t off = cloud->fields[i].offset;
+ const uint32_t point_step = cloud->point_step;
+ const uint32_t num_points = cloud->width * cloud->height;
+ uint8_t const* point = &cloud->data.front();
+
+ int label;
+ V_PointCloudPoint points(points_out.size());
+ uint32_t j = 0;
+ for (uint32_t i = 0; i < num_points; ++i, point += point_step) {
+ switch (dtype)
+ {
+ case sensor_msgs::PointField::UINT8:
+ {
+ label = *reinterpret_cast(point + off);
+ break;
+ }
+ case sensor_msgs::PointField::UINT16:
+ {
+ label = *reinterpret_cast(point + off);
+ break;
+ }
+ case sensor_msgs::PointField::UINT32:
+ {
+ label = *reinterpret_cast(point + off);
+ break;
+ }
+ case sensor_msgs::PointField::INT8:
+ {
+ label = *reinterpret_cast(point + off);
+ break;
+ }
+ case sensor_msgs::PointField::INT16:
+ {
+ label = *reinterpret_cast(point + off);
+ break;
+ }
+ case sensor_msgs::PointField::INT32:
+ {
+ label = *reinterpret_cast(point + off);
+ break;
+ }
+ default:
+ label = 0;
+ }
+ M_LabelInfos::const_iterator it = infos_.find(label);
+ if (it == infos_.end())
+ {
+ addLabel(label);
+ it = infos_.find(label);
+ }
+ const LabelInfo &info = it->second;
+ if (!info.enabled)
+ continue;
+ points[j].position = points_out[i].position;
+ points[j].color = info.color;
+ j += 1;
+ }
+ std::swap(points, points_out);
+ return true;
+}
+
+uint8_t LabelPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
+{
+ return 255;
+}
+
+void LabelPCTransformer::createProperties(Property* parent_property, uint32_t mask, QList& out_props)
+{
+ if (mask & Support_Color) {
+ use_rainbow_ = new BoolProperty(
+ "Label Use Rainbows",
+ true,
+ "Whether to assign a rainbow of colors to new labels.",
+ parent_property);
+ out_props.push_back(use_rainbow_);
+
+ default_color_ = new ColorProperty(
+ "Label Default Color",
+ Qt::white,
+ "Color to assign to new labels.",
+ parent_property);
+ out_props.push_back(default_color_);
+
+ labels_ = new LabelsProperty(
+ "Labels",
+ QVariant(),
+ "List of labels.",
+ parent_property,
+ SLOT(labelChanged()),
+ this);
+ connect(
+ labels_,
+ SIGNAL(childListChanged(Property*)),
+ this,
+ SLOT(labelListChanged()));
+ out_props.push_back(labels_);
+ }
+}
+
+void LabelPCTransformer::labelListChanged()
+{
+ updateInfos();
+ Q_EMIT needRetransform();
+}
+
+void LabelPCTransformer::labelChanged()
+{
+ updateInfos();
+ Q_EMIT needRetransform();
+}
+
+void LabelPCTransformer::updateInfos()
+{
+ M_LabelInfos infos;
+ const int num_children = labels_->numChildren();
+ for (int i = 0; i != num_children; i += 1)
+ {
+ LabelProperty *label = reinterpret_cast(labels_->childAt(i));
+ LabelInfo info;
+ info.value = label->getInt();
+ info.enabled = label->getBool();
+ info.color = qtToOgre(label->getColor());
+ infos.insert(std::make_pair(info.value, info));
+ }
+ std::swap(infos, infos_);
+}
+
+void LabelPCTransformer::addLabel(int value)
+{
+ bool prev = labels_->blockSignals(true);
+ QColor color;
+ if (use_rainbow_->getBool())
+ {
+ Ogre::ColourValue c;
+ getRainbowColor((value % 10)/10.0, c);
+ color = ogreToQt(c);
+ }
+ else
+ {
+ color = default_color_->getColor();
+ }
+ LabelProperty *label = labels_->add(value, color);
+ LabelInfo info;
+ info.value = label->getInt();
+ info.enabled = label->getBool();
+ info.color = qtToOgre(label->getColor());
+ infos_.insert(std::make_pair(info.value, info));
+ labels_->blockSignals(prev);
+}
+
} // end namespace rviz
#include
PLUGINLIB_EXPORT_CLASS( rviz::AxisColorPCTransformer, rviz::PointCloudTransformer )
PLUGINLIB_EXPORT_CLASS( rviz::FlatColorPCTransformer, rviz::PointCloudTransformer )
PLUGINLIB_EXPORT_CLASS( rviz::IntensityPCTransformer, rviz::PointCloudTransformer )
-PLUGINLIB_EXPORT_CLASS( rviz::RGB8PCTransformer, rviz::PointCloudTransformer )
-PLUGINLIB_EXPORT_CLASS( rviz::RGBF32PCTransformer, rviz::PointCloudTransformer )
-PLUGINLIB_EXPORT_CLASS( rviz::XYZPCTransformer, rviz::PointCloudTransformer )
+PLUGINLIB_EXPORT_CLASS( rviz::RGB8PCTransformer, rviz::PointCloudTransformer )
+PLUGINLIB_EXPORT_CLASS( rviz::RGBF32PCTransformer, rviz::PointCloudTransformer )
+PLUGINLIB_EXPORT_CLASS( rviz::XYZPCTransformer, rviz::PointCloudTransformer )
+PLUGINLIB_EXPORT_CLASS( rviz::LabelPCTransformer, rviz::PointCloudTransformer )
diff --git a/src/rviz/default_plugin/point_cloud_transformers.h b/src/rviz/default_plugin/point_cloud_transformers.h
index 36320654c8..dc6a0ba5a3 100644
--- a/src/rviz/default_plugin/point_cloud_transformers.h
+++ b/src/rviz/default_plugin/point_cloud_transformers.h
@@ -42,6 +42,7 @@ class ColorProperty;
class EditableEnumProperty;
class EnumProperty;
class FloatProperty;
+class LabelsProperty;
typedef std::vector V_string;
@@ -210,6 +211,47 @@ private Q_SLOTS:
BoolProperty* use_fixed_frame_property_;
};
+
+struct LabelInfo {
+ int value;
+ bool enabled;
+ Ogre::ColourValue color;
+};
+
+typedef std::map M_LabelInfos;
+
+class LabelPCTransformer : public PointCloudTransformer
+{
+Q_OBJECT
+public:
+ LabelPCTransformer();
+
+ virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
+ virtual bool transform(
+ const sensor_msgs::PointCloud2ConstPtr& cloud,
+ uint32_t mask,
+ const Ogre::Matrix4& transform,
+ V_PointCloudPoint& points_out);
+ virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
+ virtual void createProperties(
+ Property* parent_property,
+ uint32_t mask,
+ QList& out_props);
+
+public Q_SLOTS:
+ void labelListChanged();
+ void labelChanged();
+
+private:
+ void updateInfos();
+ void addLabel(int value);
+
+ BoolProperty* use_rainbow_;
+ ColorProperty* default_color_;
+ LabelsProperty* labels_;
+ M_LabelInfos infos_;
+};
+
}
#endif // POINT_CLOUD_TRANSFORMERS_H
diff --git a/src/rviz/properties/label_property.cpp b/src/rviz/properties/label_property.cpp
new file mode 100644
index 0000000000..56d1803e5c
--- /dev/null
+++ b/src/rviz/properties/label_property.cpp
@@ -0,0 +1,182 @@
+/*
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "rviz/properties/color_property.h"
+#include "rviz/properties/float_property.h"
+#include "rviz/properties/int_property.h"
+#include "rviz/properties/string_property.h"
+
+#include "label_property.h"
+
+namespace rviz
+{
+
+LabelsProperty::LabelsProperty(
+ const QString& name,
+ const QVariant& value,
+ const QString& description,
+ Property* parent,
+ const char *changed_slot,
+ QObject* receiver) :
+ Property(name, value, description, parent, changed_slot, receiver)
+{
+}
+
+void LabelsProperty::addChild(Property* child, int index)
+{
+ Property::addChild(child, index);
+ connect(child, SIGNAL(aboutToChange()), this, SLOT(emitAboutToChange()));
+ connect(child, SIGNAL(changed()), this, SLOT(updateFromChild()));
+}
+
+void LabelsProperty::load(const Config& config)
+{
+ if (config.getType() == Config::Map)
+ {
+ // HACK: prevent signals from partially constructed children
+ bool prev = blockSignals(true);
+
+ // HACK: must create children here or Property::load does not load
+ Config::MapIterator iter = config.mapIterator();
+ while (iter.isValid())
+ {
+ LabelProperty* label = new LabelProperty(iter.currentKey(), true, 0, Qt::white, this);
+ iter.advance();
+ }
+
+ blockSignals(prev);
+ }
+ Property::load(config);
+}
+
+LabelProperty* LabelsProperty::add(int value, const QColor &color)
+{
+ std::stringstream ss;
+ ss << value;
+ QString name = QString::fromStdString(ss.str());
+ LabelProperty *label = new LabelProperty(name, true, value, color, this);
+ return label;
+}
+
+bool LabelsProperty::remove(int value)
+{
+ const int num_children = numChildren();
+ for (int i = 0; i != num_children; i += 1)
+ {
+ LabelProperty *label = reinterpret_cast(childAt(i));
+ if (label->getInt() == value)
+ {
+ return true;
+ }
+ removeChildren(i);
+ }
+ return false;
+}
+
+void LabelsProperty::updateFromChild()
+{
+ Q_EMIT changed();
+}
+
+void LabelsProperty::emitAboutToChange()
+{
+ Q_EMIT aboutToChange();
+}
+
+LabelProperty::LabelProperty(
+ const QString& name,
+ bool enabled,
+ int value,
+ const QColor &color,
+ Property* parent,
+ const char *changed_slot,
+ QObject* receiver) :
+ BoolProperty(name, enabled, "", parent, changed_slot, receiver)
+{
+ alias_ = new StringProperty("Name", name, "Alias assigned to this label.", this);
+ value_ = new IntProperty("Label", value, "Value of this label.", this);
+ color_ = new ColorProperty("Color", color, "Color to assign to every point with this label.", this);
+ connect(alias_, SIGNAL(aboutToChange()), this, SLOT(emitAboutToChange()));
+ connect(alias_, SIGNAL(changed()), this, SLOT(updateFromChildren()));
+ connect(value_, SIGNAL(aboutToChange()), this, SLOT(emitAboutToChange()));
+ connect(value_, SIGNAL(changed()), this, SLOT(updateFromChildren()));
+ connect(color_, SIGNAL(changed()), this, SLOT(updateFromChildren()));
+ connect(color_, SIGNAL(aboutToChange()), this, SLOT(emitAboutToChange()));
+}
+
+bool LabelProperty::setInt(int value)
+{
+ value_->setInt(value);
+ Q_EMIT changed();
+ return true;
+}
+
+int LabelProperty::getInt() const
+{
+ return value_->getInt();
+}
+
+bool LabelProperty::setString(const QString& alias)
+{
+ alias_->setString(alias);
+ Q_EMIT changed();
+ return true;
+}
+
+QString LabelProperty::getString() const
+{
+ return alias_->getString();
+}
+
+bool LabelProperty::setColor(const QColor& color)
+{
+ color_->setColor(color);
+ Q_EMIT changed();
+ return true;
+}
+
+QColor LabelProperty::getColor() const
+{
+ return color_->getColor();
+}
+
+void LabelProperty::updateFromChildren()
+{
+ if (getString() != getName()) {
+ setName(getString());
+ }
+ Q_EMIT changed();
+}
+
+void LabelProperty::emitAboutToChange()
+{
+ Q_EMIT aboutToChange();
+}
+
+} // end namespace rviz
diff --git a/src/rviz/properties/label_property.h b/src/rviz/properties/label_property.h
new file mode 100644
index 0000000000..d4ed9a2f66
--- /dev/null
+++ b/src/rviz/properties/label_property.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef LABEL_PROPERTY_H
+#define LABEL_PROPERTY_H
+
+#include "rviz/properties/bool_property.h"
+
+namespace rviz
+{
+
+class ColorProperty;
+class IntProperty;
+class LabelProperty;
+class StringProperty;
+
+class LabelsProperty : public Property
+{
+Q_OBJECT
+
+public:
+ LabelsProperty(
+ const QString& name = QString(),
+ const QVariant& value = QVariant(),
+ const QString& description = QString(),
+ Property* parent = 0,
+ const char *changed_slot = 0,
+ QObject* receiver = 0);
+
+ virtual void addChild(Property* child, int index = -1);
+
+ virtual void load(const Config& config);
+
+ LabelProperty* add(int value, const QColor &color);
+ bool remove(int value);
+
+public Q_SLOTS:
+ void updateFromChild();
+ void emitAboutToChange();
+};
+
+class LabelProperty : public BoolProperty
+{
+Q_OBJECT
+public:
+ LabelProperty(
+ const QString& name,
+ bool enabled,
+ int value,
+ const QColor &color,
+ Property* parent = 0,
+ const char *changed_slot = 0,
+ QObject* receiver = 0);
+
+ virtual bool setInt(int value);
+ virtual int getInt() const;
+
+ virtual bool setString(const QString& name);
+ virtual QString getString() const;
+
+ virtual bool setColor(const QColor& color);
+ virtual QColor getColor() const;
+
+public Q_SLOTS:
+ void updateFromChildren();
+ void emitAboutToChange();
+
+private:
+ void updateName();
+
+ StringProperty* alias_;
+ IntProperty* value_;
+ ColorProperty* color_;
+};
+
+} // end namespace rviz
+
+#endif // LABEL_PROPERTY_H