Skip to content

Commit

Permalink
Add option to set individual frame visibility
Browse files Browse the repository at this point in the history 
* Add checkboxes to tree view frame list

* Update documentation

Signed-off-by: Sarathkrishnan Ramesh <[email protected]>
  • Loading branch information
@Sarath18
Sarath18 committed Jul 6, 2020
1 parent 19def75 commit 8bcce84
Show file tree
Hide file tree
Showing 5 changed files with 86 additions and 29 deletions.
4 changes: 2 additions & 2 deletions ign_rviz_common/include/ignition/rviz/common/frame_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,15 +60,15 @@ class FrameManager : public QObject
* @param[out] _pose: Frame pose
* @return Pose validity (true if pose is valid, else false)
*/
bool getFramePose(std::string & _frame, ignition::math::Pose3d & _pose);
bool getFramePose(const std::string & _frame, ignition::math::Pose3d & _pose);

/**
* @brief Get parent frame pose (position and orientation)
* @param[in] _child: Child frame name
* @param[out] _pose: Parent frame pose
* @return Pose validity (true if pose is valid, else false)
*/
bool getParentPose(std::string & _child, ignition::math::Pose3d & _pose);
bool getParentPose(const std::string & _child, ignition::math::Pose3d & _pose);

/**
* @brief Get available tf frames
Expand Down
4 changes: 2 additions & 2 deletions ign_rviz_common/src/rviz/common/frame_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ void FrameManager::tf_callback(tf2_msgs::msg::TFMessage::SharedPtr _msg)
}

////////////////////////////////////////////////////////////////////////////////
bool FrameManager::getFramePose(std::string & _frame, ignition::math::Pose3d & _pose)
bool FrameManager::getFramePose(const std::string & _frame, ignition::math::Pose3d & _pose)
{
std::lock_guard<std::mutex>(this->tf_mutex_);

Expand All @@ -153,7 +153,7 @@ bool FrameManager::getFramePose(std::string & _frame, ignition::math::Pose3d & _
}

////////////////////////////////////////////////////////////////////////////////
bool FrameManager::getParentPose(std::string & _child, ignition::math::Pose3d & _pose)
bool FrameManager::getParentPose(const std::string & _child, ignition::math::Pose3d & _pose)
{
std::lock_guard<std::mutex>(this->tf_mutex_);

Expand Down
36 changes: 28 additions & 8 deletions ign_rviz_plugins/include/ignition/rviz/plugins/TFDisplay.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,10 @@

#include <QStandardItem>

#include <string>
#include <mutex>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>

#include "ignition/rviz/plugins/message_display_base.hpp"
Expand All @@ -39,12 +40,14 @@ class FrameModel : public QStandardItemModel
Q_OBJECT

public:
// Roles for tree view frames
enum FrameRoles
{
NameRole = Qt::UserRole + 1
};

explicit FrameModel(QObject * parent = 0);
// Constructor
explicit FrameModel(QObject * _parent = 0);

Q_INVOKABLE void addFrame(const QString & _name, QStandardItem * _parentItem);
Q_INVOKABLE QStandardItem * addParentRow(const QString & _name);
Expand Down Expand Up @@ -88,33 +91,45 @@ class TFDisplay : public MessageDisplay<tf2_msgs::msg::TFMessage>
bool eventFilter(QObject *, QEvent *);

// Documentation inherited
void setFrameManager(std::shared_ptr<common::FrameManager>);
void setFrameManager(std::shared_ptr<common::FrameManager> _frameManager);

/**
* @brief Set axis visibility
* @param[in] _visible Axes visibility
*/
Q_INVOKABLE void showAxes(const bool & _visible);

/**
* @brief Set arrow visibility
* @param[in] _visible Arrow visibility
*/
Q_INVOKABLE void showArrows(const bool & _visible);

/**
* @brief Set frame name visibility
* @param[in] _visible Frame name visibility
*/
Q_INVOKABLE void showNames(const bool & _visible);

/**
* @brief Set axes arrow head visibility
* @param[in] _visible Axes arrow head visibility
*/
Q_INVOKABLE void showAxesHead(const bool & _visible);

/**
* @brief Set marker scale
* @param _scale TF visual marker scale
*/
Q_INVOKABLE void setMarkerScale(const float & _scale);

/**
* @brief Set frame visibility
* @param[in] _frame Frame name
* @param[in] _visible Frame visibility
*/
Q_INVOKABLE void setFrameVisibility(const QString & _frame, const bool & _visible);

protected:
/**
* @brief Create custom arrow visual for visualizing tf links
Expand All @@ -134,23 +149,28 @@ class TFDisplay : public MessageDisplay<tf2_msgs::msg::TFMessage>
*/
rendering::VisualPtr createVisualFrame();

/**
* @brief Update tree view and local frame list
*/
void refresh();

public:
// Tree view frame model
FrameModel * model;

private:
ignition::rendering::AxisVisualPtr axis;
ignition::rendering::RenderEngine * engine;
ignition::rendering::ScenePtr scene;
std::mutex lock;
ignition::rendering::VisualPtr tfRootVisual;
std::mutex lock;
bool axesVisible;
bool arrowsVisible;
bool namesVisible;
bool axesHeadVisible;
float markerScale;
QStandardItem * parentRow;

public:
FrameModel * model;
std::map<std::string, bool> frameInfo;
};

} // namespace plugins
Expand Down
8 changes: 8 additions & 0 deletions ign_rviz_plugins/res/qml/TFDisplay.qml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,6 +142,14 @@ Item {

TableViewColumn {
role: "name"
delegate: CheckDelegate {
text: model.name
checked: model.checked
onClicked: {
model.checked = checked;
TFDisplay.setFrameVisibility(model.name, model.checked);
}
}
}

// Delegates
Expand Down
63 changes: 46 additions & 17 deletions ign_rviz_plugins/src/rviz/plugins/TFDisplay.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,15 +36,16 @@ namespace plugins
{
#define MIN_FRAME_DISTANCE 0.25
////////////////////////////////////////////////////////////////////////////////
FrameModel::FrameModel(QObject * parent)
: QStandardItemModel(parent)
FrameModel::FrameModel(QObject * _parent)
: QStandardItemModel(_parent)
{}

////////////////////////////////////////////////////////////////////////////////
void FrameModel::addFrame(const QString & _name, QStandardItem * _parentItem)
{
QStandardItem * frameRow = new QStandardItem();
frameRow->setData(_name, NameRole);
frameRow->setCheckState(Qt::CheckState::Checked);
_parentItem->appendRow(frameRow);
}

Expand All @@ -53,6 +54,7 @@ QStandardItem * FrameModel::addParentRow(const QString & _name)
{
QStandardItem * entry = new QStandardItem();
entry->setData(_name, NameRole);
entry->setCheckState(Qt::CheckState::Checked);
appendRow(entry);
return entry;
}
Expand All @@ -66,6 +68,10 @@ QVariant FrameModel::data(const QModelIndex & _index, int _role) const
return myItem->data(NameRole);
}

if (_role == Qt::CheckStateRole) {
return myItem->data(Qt::CheckStateRole);
}

return QVariant();
}

Expand All @@ -74,6 +80,7 @@ QHash<int, QByteArray> FrameModel::roleNames() const
{
QHash<int, QByteArray> roles;
roles[NameRole] = "name";
roles[Qt::CheckStateRole] = "checked";

return roles;
}
Expand Down Expand Up @@ -110,7 +117,6 @@ TFDisplay::TFDisplay()
this->scene->RootVisual()->AddChild(tfRootVisual);

this->model = new FrameModel();

parentRow = this->model->addParentRow(QString::fromStdString("Frames"));

ignition::gui::App()->Engine()->rootContext()->setContextProperty("FrameModel", this->model);
Expand Down Expand Up @@ -217,34 +223,40 @@ void TFDisplay::updateTF()
{
std::lock_guard<std::mutex>(this->lock);

// Get available tf frames
std::vector<std::string> frameIds;
frameManager->getFrames(frameIds);

// Create tf visual frames
for (int i = tfRootVisual->ChildCount(); i < static_cast<int>(frameIds.size()); ++i) {
for (int i = tfRootVisual->ChildCount(); i < static_cast<int>(frameInfo.size()); ++i) {
rendering::VisualPtr visualFrame = this->createVisualFrame();
this->tfRootVisual->AddChild(visualFrame);
}

int i = -1;
// Update tf visual frames
for (int i = 0; i < static_cast<int>(frameIds.size()); ++i) {
for (auto frame : frameInfo) {
i++;
ignition::math::Pose3d pose, parentPose;

rendering::VisualPtr visualFrame = std::dynamic_pointer_cast<rendering::Visual>(
this->tfRootVisual->ChildByIndex(i));

bool result = this->frameManager->getFramePose(frameIds[i], pose);
// Set frame visibility
visualFrame->SetVisible(frame.second);

// Skip processing if frame not visible
if (!frame.second) {
continue;
}

// Set frame text
rendering::TextPtr frameName = std::dynamic_pointer_cast<rendering::Text>(
visualFrame->GeometryByIndex(0));
frameName->SetTextString(frameIds[i]);
frameName->SetTextString(frame.first);

visualFrame->SetVisible(this->namesVisible);

// Set frame position
visualFrame->SetLocalPosition(pose.Pos());
if (this->frameManager->getFramePose(frame.first, pose)) {
visualFrame->SetLocalPosition(pose.Pos());
}

// Set axis orientation
rendering::AxisVisualPtr axis = std::dynamic_pointer_cast<rendering::AxisVisual>(
Expand All @@ -258,7 +270,7 @@ void TFDisplay::updateTF()
}

// Get parent pose for tf links
result = this->frameManager->getParentPose(frameIds[i], parentPose);
bool result = this->frameManager->getParentPose(frame.first, parentPose);
rendering::ArrowVisualPtr arrow = std::dynamic_pointer_cast<rendering::ArrowVisual>(
visualFrame->ChildByIndex(0));
if (result) {
Expand Down Expand Up @@ -302,13 +314,15 @@ void TFDisplay::refresh()
this->frameManager->getFrames(frames);

if (frames.size() > 0) {
std::sort(frames.begin(), frames.end());
for (const auto frame : frames) {
this->frameInfo.insert({frame, true});
}

// Clear rows
parentRow->removeRows(0, parentRow->rowCount());

// Add frames to tree
for (const auto frame : frames) {
this->model->addFrame(QString::fromStdString(frame), parentRow);
for (auto frame : frameInfo) {
this->model->addFrame(QString::fromStdString(frame.first), parentRow);
}
}
}
Expand All @@ -324,33 +338,48 @@ void TFDisplay::LoadConfig(const tinyxml2::XMLElement * /*_pluginElem*/)
////////////////////////////////////////////////////////////////////////////////
void TFDisplay::showAxes(const bool & _visible)
{
std::lock_guard<std::mutex>(this->lock);
this->axesVisible = _visible;
}

////////////////////////////////////////////////////////////////////////////////
void TFDisplay::showNames(const bool & _visible)
{
std::lock_guard<std::mutex>(this->lock);
this->namesVisible = _visible;
}

////////////////////////////////////////////////////////////////////////////////
void TFDisplay::showArrows(const bool & _visible)
{
std::lock_guard<std::mutex>(this->lock);
this->arrowsVisible = _visible;
}

////////////////////////////////////////////////////////////////////////////////
void TFDisplay::showAxesHead(const bool & _visible)
{
std::lock_guard<std::mutex>(this->lock);
this->axesHeadVisible = _visible;
}

////////////////////////////////////////////////////////////////////////////////
void TFDisplay::setMarkerScale(const float & _scale)
{
std::lock_guard<std::mutex>(this->lock);
this->markerScale = _scale * 0.4;
}

////////////////////////////////////////////////////////////////////////////////
void TFDisplay::setFrameVisibility(const QString & _frame, const bool & _visible)
{
std::lock_guard<std::mutex>(this->lock);
auto it = frameInfo.find(_frame.toStdString());
if (it != frameInfo.end()) {
it->second = _visible;
}
}

} // namespace plugins
} // namespace rviz
} // namespace ignition
Expand Down

0 comments on commit 8bcce84

Please sign in to comment.