Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add more visualization options for BoundingBox and BoundingBoxArray #844

Closed
wants to merge 5 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
88 changes: 88 additions & 0 deletions jsk_rviz_plugins/src/bounding_box_array_display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,12 @@ namespace jsk_rviz_plugins
coloring_property_->addOption("Label", 2);
coloring_property_->addOption("Value", 3);

alpha_method_property_ = new rviz::EnumProperty(
"alpha method", "flat", "alpha method",
this, SLOT(updateAlphaMethod()));
alpha_method_property_->addOption("flat", 0);
alpha_method_property_->addOption("value", 1);

color_property_ = new rviz::ColorProperty(
"color", QColor(25, 255, 0),
"color to draw the bounding boxes",
Expand All @@ -57,6 +63,14 @@ namespace jsk_rviz_plugins
"alpha", 0.8,
"alpha value to draw the bounding boxes",
this, SLOT(updateAlpha()));
alpha_min_property_ = new rviz::FloatProperty(
"alpha min", 0.0,
"alpha value corresponding to value = 0",
this, SLOT(updateAlphaMin()));
alpha_max_property_ = new rviz::FloatProperty(
"alpha max", 1.0,
"alpha value corresponding to value = 1",
this, SLOT(updateAlphaMax()));
only_edge_property_ = new rviz::BoolProperty(
"only edge", false,
"show only the edges of the boxes",
Expand All @@ -69,15 +83,23 @@ namespace jsk_rviz_plugins
"show coords", false,
"show coordinate of bounding box",
this, SLOT(updateShowCoords()));
value_threshold_property_ = new rviz::FloatProperty(
"value threshold", 0.0,
"filter all boxes with value < threshold",
this, SLOT(updateValueThreshold()));
}

BoundingBoxArrayDisplay::~BoundingBoxArrayDisplay()
{
delete color_property_;
delete alpha_property_;
delete alpha_min_property_;
delete alpha_max_property_;
delete only_edge_property_;
delete coloring_property_;
delete alpha_method_property_;
delete show_coords_property_;
delete value_threshold_property_;
}

void BoundingBoxArrayDisplay::onInitialize()
Expand All @@ -87,10 +109,14 @@ namespace jsk_rviz_plugins

updateColor();
updateAlpha();
updateAlphaMin();
updateAlphaMax();
updateOnlyEdge();
updateColoring();
updateAlphaMethod();
updateLineWidth();
updateShowCoords();
updateValueThreshold();
}

void BoundingBoxArrayDisplay::updateLineWidth()
Expand All @@ -117,6 +143,34 @@ namespace jsk_rviz_plugins
}
}

void BoundingBoxArrayDisplay::updateAlphaMin()
{
if (alpha_min_property_->getFloat() > alpha_max_)
{
ROS_WARN("alpha_min must be <= alpha_max");
alpha_min_property_->setFloat(alpha_min_);
return;
}
alpha_min_ = alpha_min_property_->getFloat();
if (latest_msg_) {
processMessage(latest_msg_);
}
}

void BoundingBoxArrayDisplay::updateAlphaMax()
{
if (alpha_max_property_->getFloat() < alpha_min_)
{
ROS_WARN("alpha_min must be <= alpha_max");
alpha_max_property_->setFloat(alpha_max_);
return;
}
alpha_max_ = alpha_max_property_->getFloat();
if (latest_msg_) {
processMessage(latest_msg_);
}
}

void BoundingBoxArrayDisplay::updateOnlyEdge()
{
only_edge_ = only_edge_property_->getBool();
Expand Down Expand Up @@ -161,6 +215,26 @@ namespace jsk_rviz_plugins
}
}

void BoundingBoxArrayDisplay::updateAlphaMethod()
{
if (alpha_method_property_->getOptionInt() == 0) {
alpha_method_ = "flat";
alpha_property_->show();
alpha_min_property_->hide();
alpha_max_property_->hide();
}
else if (alpha_method_property_->getOptionInt() == 1) {
alpha_method_ = "value";
alpha_property_->hide();
alpha_min_property_->show();
alpha_max_property_->show();
}

if (latest_msg_) {
processMessage(latest_msg_);
}
}

void BoundingBoxArrayDisplay::updateShowCoords()
{
show_coords_ = show_coords_property_->getBool();
Expand Down Expand Up @@ -204,6 +278,20 @@ namespace jsk_rviz_plugins
}
}

void BoundingBoxArrayDisplay::updateValueThreshold()
{
if (value_threshold_property_->getFloat() < 0.0 || value_threshold_property_->getFloat() > 1.0)
{
ROS_WARN("value threshold must be in [0,1]");
value_threshold_property_->setFloat(value_threshold_);
return;
}
value_threshold_ = value_threshold_property_->getFloat();
if (latest_msg_) {
processMessage(latest_msg_);
}
}

} // namespace jsk_rviz_plugins

#include <pluginlib/class_list_macros.h>
Expand Down
8 changes: 8 additions & 0 deletions jsk_rviz_plugins/src/bounding_box_array_display.h
Original file line number Diff line number Diff line change
Expand Up @@ -68,19 +68,27 @@ namespace jsk_rviz_plugins
// Properties
rviz::EnumProperty* coloring_property_;
rviz::ColorProperty* color_property_;
rviz::EnumProperty* alpha_method_property_;
rviz::FloatProperty* alpha_property_;
rviz::FloatProperty* alpha_min_property_;
rviz::FloatProperty* alpha_max_property_;
rviz::BoolProperty* only_edge_property_;
rviz::FloatProperty* line_width_property_;
rviz::BoolProperty* show_coords_property_;
rviz::FloatProperty* value_threshold_property_;

jsk_recognition_msgs::BoundingBoxArray::ConstPtr latest_msg_;
protected Q_SLOTS:
void updateColor();
void updateAlpha();
void updateAlphaMin();
void updateAlphaMax();
void updateOnlyEdge();
void updateColoring();
void updateAlphaMethod();
void updateLineWidth();
void updateShowCoords();
void updateValueThreshold();
private:
void processMessage(
const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg);
Expand Down
88 changes: 88 additions & 0 deletions jsk_rviz_plugins/src/bounding_box_display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,12 @@ namespace jsk_rviz_plugins
coloring_property_->addOption("Label", 1);
coloring_property_->addOption("Value", 2);

alpha_method_property_ = new rviz::EnumProperty(
"alpha_method", "flat", "alpha method",
this, SLOT(updateAlphaMethod()));
alpha_method_property_->addOption("flat", 0);
alpha_method_property_->addOption("value", 1);

color_property_ = new rviz::ColorProperty(
"color", QColor(25, 255, 0),
"color to draw the bounding boxes",
Expand All @@ -57,6 +63,14 @@ namespace jsk_rviz_plugins
"alpha", 0.8,
"alpha value to draw the bounding boxes",
this, SLOT(updateAlpha()));
alpha_min_property_ = new rviz::FloatProperty(
"alpha min", 0.0,
"alpha value corresponding to value = 0",
this, SLOT(updateAlphaMin()));
alpha_max_property_ = new rviz::FloatProperty(
"alpha max", 1.0,
"alpha value corresponding to value = 1",
this, SLOT(updateAlphaMax()));
only_edge_property_ = new rviz::BoolProperty(
"only edge", false,
"show only the edges of the boxes",
Expand All @@ -69,15 +83,23 @@ namespace jsk_rviz_plugins
"show coords", false,
"show coordinate of bounding box",
this, SLOT(updateShowCoords()));
value_threshold_property_ = new rviz::FloatProperty(
"value threshold", 0.0,
"filter all boxes with value < threshold",
this, SLOT(updateValueThreshold()));
}

BoundingBoxDisplay::~BoundingBoxDisplay()
{
delete color_property_;
delete alpha_property_;
delete alpha_min_property_;
delete alpha_max_property_;
delete only_edge_property_;
delete coloring_property_;
delete alpha_method_property_;
delete show_coords_property_;
delete value_threshold_property_;
}

void BoundingBoxDisplay::onInitialize()
Expand All @@ -87,10 +109,14 @@ namespace jsk_rviz_plugins

updateColor();
updateAlpha();
updateAlphaMin();
updateAlphaMax();
updateOnlyEdge();
updateColoring();
updateAlphaMethod();
updateLineWidth();
updateShowCoords();
updateValueThreshold();
}

void BoundingBoxDisplay::updateLineWidth()
Expand All @@ -117,6 +143,34 @@ namespace jsk_rviz_plugins
}
}

void BoundingBoxDisplay::updateAlphaMin()
{
if (alpha_min_property_->getFloat() > alpha_max_)
{
ROS_WARN("alpha_min must be <= alpha_max");
alpha_min_property_->setFloat(alpha_min_);
return;
}
alpha_min_ = alpha_min_property_->getFloat();
if (latest_msg_) {
processMessage(latest_msg_);
}
}

void BoundingBoxDisplay::updateAlphaMax()
{
if (alpha_max_property_->getFloat() < alpha_min_)
{
ROS_WARN("alpha_min must be <= alpha_max");
alpha_max_property_->setFloat(alpha_max_);
return;
}
alpha_max_ = alpha_max_property_->getFloat();
if (latest_msg_) {
processMessage(latest_msg_);
}
}

void BoundingBoxDisplay::updateOnlyEdge()
{
only_edge_ = only_edge_property_->getBool();
Expand Down Expand Up @@ -151,6 +205,26 @@ namespace jsk_rviz_plugins
}
}

void BoundingBoxDisplay::updateAlphaMethod()
{
if (alpha_method_property_->getOptionInt() == 0) {
alpha_method_ = "flat";
alpha_property_->show();
alpha_min_property_->hide();
alpha_max_property_->hide();
}
else if (alpha_method_property_->getOptionInt() == 1) {
alpha_method_ = "value";
alpha_property_->hide();
alpha_min_property_->show();
alpha_max_property_->show();
}

if (latest_msg_) {
processMessage(latest_msg_);
}
}

void BoundingBoxDisplay::updateShowCoords()
{
show_coords_ = show_coords_property_->getBool();
Expand Down Expand Up @@ -201,6 +275,20 @@ namespace jsk_rviz_plugins
}
}

void BoundingBoxDisplay::updateValueThreshold()
{
if (value_threshold_property_->getFloat() < 0.0 || value_threshold_property_->getFloat() > 1.0)
{
ROS_WARN("value threshold must be in [0,1]");
value_threshold_property_->setFloat(value_threshold_);
return;
}
value_threshold_ = value_threshold_property_->getFloat();
if (latest_msg_) {
processMessage(latest_msg_);
}
}

} // namespace jsk_rviz_plugins

#include <pluginlib/class_list_macros.h>
Expand Down
8 changes: 8 additions & 0 deletions jsk_rviz_plugins/src/bounding_box_display.h
Original file line number Diff line number Diff line change
Expand Up @@ -68,19 +68,27 @@ namespace jsk_rviz_plugins
// Properties
rviz::EnumProperty* coloring_property_;
rviz::ColorProperty* color_property_;
rviz::EnumProperty* alpha_method_property_;
rviz::FloatProperty* alpha_property_;
rviz::FloatProperty* alpha_min_property_;
rviz::FloatProperty* alpha_max_property_;
rviz::BoolProperty* only_edge_property_;
rviz::FloatProperty* line_width_property_;
rviz::BoolProperty* show_coords_property_;
rviz::FloatProperty* value_threshold_property_;

jsk_recognition_msgs::BoundingBox::ConstPtr latest_msg_;
protected Q_SLOTS:
void updateColor();
void updateAlpha();
void updateAlphaMin();
void updateAlphaMax();
void updateOnlyEdge();
void updateColoring();
void updateAlphaMethod();
void updateLineWidth();
void updateShowCoords();
void updateValueThreshold();
private:
void processMessage(
const jsk_recognition_msgs::BoundingBox::ConstPtr& msg);
Expand Down
Loading