Skip to content

Commit

Permalink
Merge updates from melodic-devel
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhaschke
rhaschke committed Mar 30, 2023
2 parents baf08e4 + 3ae7f6d commit 0f22a8b
Show file tree
Hide file tree
Showing 5 changed files with 162 additions and 70 deletions.
209 changes: 144 additions & 65 deletions src/rviz/default_plugin/tf_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,9 +28,13 @@
*/

#include <boost/bind/bind.hpp>
#include <regex>

#include <OgreSceneNode.h>
#include <OgreSceneManager.h>
#include <QValidator>
#include <QLineEdit>
#include <QToolTip>

#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
Expand Down Expand Up @@ -156,6 +160,75 @@ void FrameSelectionHandler::setOrientation(const Ogre::Quaternion& orientation)
}
}

class RegexValidator : public QValidator
{
QLineEdit* editor_;

public:
RegexValidator(QLineEdit* editor) : QValidator(editor), editor_(editor)
{
}
QValidator::State validate(QString& input, int& /*pos*/) const override
{
try
{
std::regex(input.toLocal8Bit().constData());
editor_->setStyleSheet(QString());
QToolTip::hideText();
return QValidator::Acceptable;
}
catch (const std::regex_error& e)
{
editor_->setStyleSheet("background: #ffe4e4");
QToolTip::showText(editor_->mapToGlobal(QPoint(0, 5)), tr(e.what()), editor_, QRect(), 5000);
return QValidator::Intermediate;
}
}
};

class RegexFilterProperty : public StringProperty
{
std::regex default_;
std::regex regex_;

void onValueChanged()
{
const auto& value = getString();
if (value.isEmpty())
regex_ = default_;
else
{
try
{
regex_.assign(value.toLocal8Bit().constData(), std::regex_constants::optimize);
}
catch (const std::regex_error& e)
{
regex_ = default_;
}
}
}

public:
RegexFilterProperty(const QString& name, const std::regex regex, Property* parent)
: StringProperty(name, "", "regular expression", parent), default_(regex), regex_(regex)
{
QObject::connect(this, &RegexFilterProperty::changed, this, [this]() { onValueChanged(); });
}
const std::regex& regex() const
{
return regex_;
}

QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option) override
{
auto* editor = qobject_cast<QLineEdit*>(StringProperty::createEditor(parent, option));
if (editor)
editor->setValidator(new RegexValidator(editor));
return editor;
}
};

typedef std::set<FrameInfo*> S_FrameInfo;

TFDisplay::TFDisplay() : Display(), update_timer_(0.0f), changing_single_frame_enabled_state_(false)
Expand Down Expand Up @@ -193,6 +266,9 @@ TFDisplay::TFDisplay() : Display(), update_timer_(0.0f), changing_single_frame_e
this);
frame_timeout_property_->setMin(1);

filter_whitelist_property_ = new RegexFilterProperty("Filter (whitelist)", std::regex(""), this);
filter_blacklist_property_ = new RegexFilterProperty("Filter (blacklist)", std::regex(), this);

frames_category_ = new Property("Frames", QVariant(), "The list of all frames.", this);

all_enabled_property_ =
Expand Down Expand Up @@ -365,42 +441,40 @@ void TFDisplay::updateFrames()
typedef std::vector<std::string> V_string;
V_string frames;
context_->getTF2BufferPtr()->_getFrameStrings(frames);
std::sort(frames.begin(), frames.end());

S_FrameInfo current_frames;
// filter frames according to white- and black-list regular expressions
auto it = frames.begin(), end = frames.end();
while (it != end)
{
if (it->empty() || !std::regex_search(*it, filter_whitelist_property_->regex()) ||
std::regex_search(*it, filter_blacklist_property_->regex()))
std::swap(*it, *--end); // swap current to-be-dropped name with last one
else
++it;
}

S_FrameInfo current_frames;
for (it = frames.begin(); it != end; ++it)
{
for (const std::string& frame : frames)
FrameInfo* info = getFrameInfo(*it);
if (!info)
{
if (frame.empty())
{
continue;
}

FrameInfo* info = getFrameInfo(frame);
if (!info)
{
info = createFrame(frame);
}
else
{
updateFrame(info);
}

current_frames.insert(info);
info = createFrame(*it);
}
else
{
updateFrame(info);
}

current_frames.insert(info);
}

for (auto frame_it = frames_.begin(), frame_end = frames_.end(); frame_it != frame_end;)
{
M_FrameInfo::iterator frame_it = frames_.begin();
M_FrameInfo::iterator frame_end = frames_.end();
while (frame_it != frame_end)
{
if (current_frames.find(frame_it->second) == current_frames.end())
frame_it = deleteFrame(frame_it, true);
else
++frame_it;
}
if (current_frames.find(frame_it->second) == current_frames.end())
frame_it = deleteFrame(frame_it, true);
else
++frame_it;
}

context_->queueRender();
Expand Down Expand Up @@ -433,8 +507,9 @@ FrameInfo* TFDisplay::createFrame(const std::string& frame)
info->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR);

info->enabled_property_ = new BoolProperty(QString::fromStdString(info->name_), true,
"Enable or disable this individual frame.",
frames_category_, SLOT(updateVisibilityFromFrame()), info);
"Enable or disable this individual frame.", nullptr,
SLOT(updateVisibilityFromFrame()), info);
frames_category_->insertChildSorted(info->enabled_property_);

info->parent_property_ =
new StringProperty("Parent", "", "Parent of this frame. (Not editable)", info->enabled_property_);
Expand Down Expand Up @@ -544,6 +619,9 @@ void TFDisplay::updateFrame(FrameInfo* frame)

setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK");

float scale = scale_property_->getFloat();
bool frame_enabled = frame->enabled_property_->getBool();

Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (!context_->getFrameManager()->getTransform(frame->name_, ros::Time(), position, orientation))
Expand All @@ -556,28 +634,25 @@ void TFDisplay::updateFrame(FrameInfo* frame)
frame->name_node_->setVisible(false);
frame->axes_->getSceneNode()->setVisible(false);
frame->parent_arrow_->getSceneNode()->setVisible(false);
return;
}
else
{
frame->selection_handler_->setPosition(position);
frame->selection_handler_->setOrientation(orientation);

frame->selection_handler_->setPosition(position);
frame->selection_handler_->setOrientation(orientation);

bool frame_enabled = frame->enabled_property_->getBool();

frame->axes_->setPosition(position);
frame->axes_->setOrientation(orientation);
frame->axes_->getSceneNode()->setVisible(show_axes_property_->getBool() && frame_enabled);
float scale = scale_property_->getFloat();
frame->axes_->setScale(Ogre::Vector3(scale, scale, scale));
float alpha = alpha_property_->getFloat();
frame->axes_->updateAlpha(alpha);
frame->axes_->setPosition(position);
frame->axes_->setOrientation(orientation);
frame->axes_->getSceneNode()->setVisible(show_axes_property_->getBool() && frame_enabled);
frame->axes_->setScale(Ogre::Vector3(scale, scale, scale));
frame->axes_->updateAlpha(alpha_property_->getFloat());

frame->name_node_->setPosition(position);
frame->name_node_->setVisible(show_names_property_->getBool() && frame_enabled);
frame->name_node_->setScale(scale, scale, scale);
frame->name_node_->setPosition(position);
frame->name_node_->setVisible(show_names_property_->getBool() && frame_enabled);
frame->name_node_->setScale(scale, scale, scale);

frame->position_property_->setVector(position);
frame->orientation_property_->setQuaternion(orientation);
frame->position_property_->setVector(position);
frame->orientation_property_->setQuaternion(orientation);
}

std::string old_parent = frame->parent_;
frame->parent_.clear();
Expand Down Expand Up @@ -651,31 +726,29 @@ void TFDisplay::updateFrame(FrameInfo* frame)
frame->parent_arrow_->getSceneNode()->setVisible(false);
}

// If this frame has no tree property or the parent has changed,
if (!frame->tree_property_ || old_parent != frame->parent_)
// If this frame has no tree property yet or the parent has changed,
if (!frame->tree_property_ || old_parent != frame->parent_ ||
// or its actual parent was not yet created
(has_parent && frame->tree_property_->getParent() == tree_category_))
{
// Look up the new parent.
FrameInfo* parent = has_parent ? getFrameInfo(frame->parent_) : nullptr;
// Retrieve tree property to add the new child at
rviz::Property* parent_tree_property = has_parent ? nullptr : tree_category_;
if (parent && parent->tree_property_)
rviz::Property* parent_tree_property;
if (parent && parent->tree_property_) // parent already created
parent_tree_property = parent->tree_property_;
else if (has_parent) // otherwise reset parent_ to retry if the parent property was created
frame->parent_ = old_parent;
else // create (temporarily) at root
parent_tree_property = tree_category_;

// If the parent has a tree property, make a new tree property for this frame.
if (!parent_tree_property)
; // nothing more to do
else if (!frame->tree_property_) // create new property
{
frame->tree_property_ =
new Property(QString::fromStdString(frame->name_), QVariant(), "", parent_tree_property);
if (!frame->tree_property_)
{ // create new tree node
frame->tree_property_ = new Property(QString::fromStdString(frame->name_));
parent_tree_property->insertChildSorted(frame->tree_property_);
}
else // update property
{
// re-parent the tree property
else if (frame->tree_property_->getParent() != parent_tree_property)
{ // re-parent existing tree property
frame->tree_property_->getParent()->takeChild(frame->tree_property_);
parent_tree_property->addChild(frame->tree_property_);
parent_tree_property->insertChildSorted(frame->tree_property_);
}
}

Expand All @@ -696,6 +769,12 @@ TFDisplay::M_FrameInfo::iterator TFDisplay::deleteFrame(M_FrameInfo::iterator it
if (delete_properties)
{
delete frame->enabled_property_;
// re-parent all children to root tree node
for (int i = frame->tree_property_->numChildren() - 1; i >= 0; --i)
{
auto* child = frame->tree_property_->takeChild(frame->tree_property_->childAtUnchecked(i));
tree_category_->insertChildSorted(child);
}
delete frame->tree_property_;
}
delete frame;
Expand Down
3 changes: 3 additions & 0 deletions src/rviz/default_plugin/tf_display.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ class VectorProperty;
class FrameInfo;
class FrameSelectionHandler;
typedef boost::shared_ptr<FrameSelectionHandler> FrameSelectionHandlerPtr;
class RegexFilterProperty;

/** @brief Displays a visual representation of the TF hierarchy. */
class TFDisplay : public Display
Expand Down Expand Up @@ -121,6 +122,8 @@ private Q_SLOTS:
FloatProperty* scale_property_;
FloatProperty* alpha_property_;

RegexFilterProperty* filter_whitelist_property_;
RegexFilterProperty* filter_blacklist_property_;
Property* frames_category_;
Property* tree_category_;

Expand Down
9 changes: 9 additions & 0 deletions src/rviz/properties/property.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -382,6 +382,15 @@ void Property::addChild(Property* child, int index)
Q_EMIT childListChanged(this);
}

void Property::insertChildSorted(Property* child)
{
auto it = std::lower_bound(children_.cbegin(), children_.cend(), child,
[](Property* element, Property* child) {
return element->getName() < child->getName();
});
addChild(child, it - children_.cbegin());
}

void Property::setModel(PropertyTreeModel* model)
{
model_ = model;
Expand Down
3 changes: 3 additions & 0 deletions src/rviz/properties/property.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -346,6 +346,9 @@ class RVIZ_EXPORT Property : public QObject
* child will be added at the end. */
virtual void addChild(Property* child, int index = -1);

/** @brief Insert a child property, sorted by name */
void insertChildSorted(Property* child);

/** @brief Set the model managing this Property and all its child properties, recursively. */
void setModel(PropertyTreeModel* model);

Expand Down
8 changes: 3 additions & 5 deletions src/rviz/robot/robot_joint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -387,10 +387,9 @@ void RobotJoint::updateAxis()
axis_->getSceneNode()->setVisible(getEnabled());

axis_->setPosition(position_property_->getVector());
axis_->setOrientation(orientation_property_->getQuaternion());
axis_->setDirection(orientation_property_->getQuaternion() * axis_property_->getVector());

// TODO(lucasw) store an Ogre::ColorValue and set it according to
// joint type.
// TODO(lucasw) store an Ogre::ColorValue and set it according to joint type.
axis_->setColor(0.0, 0.8, 0.0, 1.0);
}
}
Expand Down Expand Up @@ -421,8 +420,7 @@ void RobotJoint::setTransforms(const Ogre::Vector3& parent_link_position,
if (axis_)
{
axis_->setPosition(position);
axis_->setOrientation(orientation);
axis_->setDirection(parent_link_orientation * axis_property_->getVector());
axis_->setDirection(orientation * axis_property_->getVector());
}
}

Expand Down

0 comments on commit 0f22a8b

Please sign in to comment.