mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge pull request #37 from ubi-agni/relative-markers
online update of relative markers
This commit is contained in:
commit
b15c5adbd7
@ -88,8 +88,6 @@ TaskDisplay::TaskDisplay() : Display()
|
||||
connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)),
|
||||
task_list_model_.get(), SLOT(highlightStage(size_t)));
|
||||
|
||||
marker_visual_ = new MarkerVisualizationProperty("Markers", this);
|
||||
|
||||
tasks_property_ =
|
||||
new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this);
|
||||
}
|
||||
@ -103,7 +101,6 @@ void TaskDisplay::onInitialize()
|
||||
{
|
||||
Display::onInitialize();
|
||||
trajectory_visual_->onInitialize(scene_node_, context_);
|
||||
marker_visual_->onInitialize(scene_node_, context_);
|
||||
|
||||
#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
|
||||
// displays are loaded before panels, hence wait a little bit for the panel to be loaded
|
||||
@ -246,16 +243,6 @@ void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionCon
|
||||
});
|
||||
}
|
||||
|
||||
void TaskDisplay::clearMarkers() {
|
||||
marker_visual_->clearMarkers();
|
||||
}
|
||||
|
||||
void TaskDisplay::addMarkers(const DisplaySolutionPtr &s) {
|
||||
if (!s) return;
|
||||
for (size_t i=0, end = s->numSubSolutions(); i != end; ++i) {
|
||||
marker_visual_->addMarkers(s->markersOfSubTrajectory(i));
|
||||
}
|
||||
}
|
||||
|
||||
void TaskDisplay::changedTaskSolutionTopic()
|
||||
{
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <rviz/display.h>
|
||||
#include <moveit/visualization_tools/task_solution_visualization.h>
|
||||
|
||||
#ifndef Q_MOC_RUN
|
||||
#include "job_queue.h"
|
||||
@ -63,8 +64,6 @@ namespace moveit_rviz_plugin
|
||||
{
|
||||
|
||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||
class TaskSolutionVisualization;
|
||||
class MarkerVisualizationProperty;
|
||||
class TaskListModel;
|
||||
|
||||
class TaskDisplay : public rviz::Display
|
||||
@ -87,8 +86,9 @@ public:
|
||||
|
||||
TaskListModel& getTaskListModel() { return *task_list_model_; }
|
||||
TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); }
|
||||
void clearMarkers();
|
||||
void addMarkers(const DisplaySolutionPtr &s);
|
||||
|
||||
inline void clearMarkers() { trajectory_visual_->clearMarkers(); }
|
||||
inline void addMarkers(const DisplaySolutionPtr &s) { trajectory_visual_->addMarkers(s); }
|
||||
|
||||
protected:
|
||||
void onInitialize() override;
|
||||
@ -132,7 +132,6 @@ protected:
|
||||
// Properties
|
||||
rviz::StringProperty* robot_description_property_;
|
||||
rviz::RosTopicProperty* task_solution_topic_property_;
|
||||
MarkerVisualizationProperty* marker_visual_;
|
||||
rviz::Property* tasks_property_;
|
||||
};
|
||||
|
||||
|
||||
@ -21,6 +21,9 @@ class MarkerBase;
|
||||
namespace planning_scene {
|
||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||
}
|
||||
namespace moveit { namespace core {
|
||||
MOVEIT_CLASS_FORWARD(RobotState)
|
||||
} }
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
@ -36,20 +39,48 @@ MOVEIT_CLASS_FORWARD(MarkerVisualization)
|
||||
class MarkerVisualization
|
||||
{
|
||||
// list of all markers, attached to scene nodes in namespaces_
|
||||
typedef std::pair<visualization_msgs::MarkerConstPtr, std::shared_ptr<rviz::MarkerBase>> MarkerData;
|
||||
struct MarkerData {
|
||||
visualization_msgs::MarkerPtr msg_;
|
||||
std::shared_ptr<rviz::MarkerBase> marker_;
|
||||
|
||||
MarkerData(const visualization_msgs::Marker& marker);
|
||||
};
|
||||
struct NamespaceData {
|
||||
Ogre::SceneNode* ns_node_ = nullptr;
|
||||
// markers grouped by frame
|
||||
std::map<std::string, Ogre::SceneNode*> frames_;
|
||||
};
|
||||
|
||||
// list of all markers
|
||||
std::deque<MarkerData> markers_;
|
||||
// markers grouped by their namespace
|
||||
std::map<QString, Ogre::SceneNode*> namespaces_;
|
||||
std::map<std::string, NamespaceData> namespaces_;
|
||||
|
||||
// planning_frame_ of scene
|
||||
std::string planning_frame_;
|
||||
// flag indicating that markers were created
|
||||
bool markers_created_ = false;
|
||||
|
||||
public:
|
||||
MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers,
|
||||
const planning_scene::PlanningScene& end_scene);
|
||||
~MarkerVisualization();
|
||||
|
||||
void createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node);
|
||||
const std::map<QString, Ogre::SceneNode*>& namespaces() const { return namespaces_; }
|
||||
/// did we successfully created all markers (and scene nodes)?
|
||||
bool created() const { return markers_created_; }
|
||||
/// create markers (placed at planning frame of scene)
|
||||
bool createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node);
|
||||
/// update marker position/orientation based on frames of given scene + robot_state
|
||||
void update(const planning_scene::PlanningScene &end_scene,
|
||||
const moveit::core::RobotState &robot_state);
|
||||
|
||||
const std::map<std::string, NamespaceData>& namespaces() const { return namespaces_; }
|
||||
void setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible);
|
||||
|
||||
private:
|
||||
void update(MarkerData &data,
|
||||
const planning_scene::PlanningScene &end_scene,
|
||||
const moveit::core::RobotState &robot_state) const;
|
||||
};
|
||||
|
||||
|
||||
@ -67,6 +98,7 @@ class MarkerVisualizationProperty: public rviz::BoolProperty
|
||||
Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to
|
||||
std::map<QString, rviz::BoolProperty*> namespaces_; // rviz properties for encountered namespaces
|
||||
std::list<MarkerVisualizationPtr> hosted_markers_; // list of hosted MarkerVisualization instances
|
||||
rviz::BoolProperty* all_markers_at_once_;
|
||||
|
||||
public:
|
||||
MarkerVisualizationProperty(const QString& name, Property* parent = nullptr);
|
||||
@ -76,12 +108,21 @@ public:
|
||||
|
||||
/// remove all hosted markers from display
|
||||
void clearMarkers();
|
||||
/// add all markers in MarkerVisualization for display
|
||||
/// add markers in MarkerVisualization for display
|
||||
void addMarkers(MarkerVisualizationPtr markers);
|
||||
/// update pose of all markers
|
||||
void update(const planning_scene::PlanningScene &scene,
|
||||
const moveit::core::RobotState &robot_state);
|
||||
|
||||
bool allAtOnce() const;
|
||||
|
||||
public Q_SLOTS:
|
||||
void onEnableChanged();
|
||||
void onNSEnableChanged();
|
||||
void onAllAtOnceChanged();
|
||||
|
||||
Q_SIGNALS:
|
||||
void allAtOnceChanged(bool);
|
||||
};
|
||||
|
||||
} // namespace moveit_rviz_plugin
|
||||
|
||||
@ -80,6 +80,7 @@ MOVEIT_CLASS_FORWARD(PlanningSceneRender)
|
||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||
|
||||
class TaskSolutionPanel;
|
||||
class MarkerVisualizationProperty;
|
||||
class TaskSolutionVisualization : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
@ -105,13 +106,18 @@ public:
|
||||
|
||||
planning_scene::PlanningSceneConstPtr getScene() const { return scene_; }
|
||||
void showTrajectory(const moveit_task_constructor_msgs::Solution& msg);
|
||||
void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s, bool lock);
|
||||
void showTrajectory(const moveit_rviz_plugin::DisplaySolutionPtr& s, bool lock);
|
||||
void unlock();
|
||||
|
||||
void clearMarkers();
|
||||
void addMarkers(const moveit_rviz_plugin::DisplaySolutionPtr &s);
|
||||
|
||||
public Q_SLOTS:
|
||||
void interruptCurrentDisplay();
|
||||
|
||||
private Q_SLOTS:
|
||||
void onAllAtOnceChanged(bool);
|
||||
|
||||
// trajectory property slots
|
||||
void changedRobotVisualEnabled();
|
||||
void changedRobotCollisionEnabled();
|
||||
@ -142,6 +148,8 @@ protected:
|
||||
PlanningSceneRenderPtr scene_render_;
|
||||
// render the robot
|
||||
RobotStateVisualizationPtr robot_render_;
|
||||
// render markers
|
||||
MarkerVisualizationProperty* marker_visual_;
|
||||
|
||||
// Handle colouring of robot
|
||||
void setRobotColor(rviz::Robot* robot, const QColor& color);
|
||||
|
||||
@ -13,6 +13,7 @@
|
||||
|
||||
#include <rviz/display_context.h>
|
||||
#include <rviz/frame_manager.h>
|
||||
#include <tf2_msgs/TF2Error.h>
|
||||
#include <ros/console.h>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
#include <OgreSceneManager.h>
|
||||
@ -56,51 +57,38 @@ rviz::MarkerBase* createMarker(int marker_type, rviz::DisplayContext* context, O
|
||||
}
|
||||
}
|
||||
|
||||
namespace {
|
||||
// express marker pose relative to planning frame (of end scene)
|
||||
bool toPlanningFrame(visualization_msgs::Marker &marker,
|
||||
const planning_scene::PlanningScene &scene)
|
||||
// create MarkerData with nil marker_ pointer, just with a copy of message
|
||||
MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::Marker& marker)
|
||||
{
|
||||
if (marker.header.frame_id == scene.getPlanningFrame())
|
||||
return true;
|
||||
|
||||
if (!scene.knowsFrameTransform(marker.header.frame_id)) {
|
||||
ROS_WARN_ONCE("unknown frame '%s' for solution marker in namespace '%s'",
|
||||
marker.header.frame_id.c_str(), marker.ns.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
Eigen::Affine3d pose;
|
||||
tf::poseMsgToEigen(marker.pose, pose);
|
||||
const Eigen::Affine3d tm = scene.getFrameTransform(marker.header.frame_id);
|
||||
tf::poseEigenToMsg(tm * pose, marker.pose);
|
||||
marker.header.frame_id = scene.getPlanningFrame();
|
||||
return true;
|
||||
}
|
||||
msg_.reset(new visualization_msgs::Marker(marker));
|
||||
msg_->header.stamp = ros::Time();
|
||||
msg_->frame_locked = false;
|
||||
}
|
||||
|
||||
MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::Marker> &markers,
|
||||
const planning_scene::PlanningScene &end_scene)
|
||||
{
|
||||
planning_frame_ = end_scene.getPlanningFrame();
|
||||
// remember marker message, postpone rviz::MarkerBase creation until later
|
||||
for (const auto& marker : markers) {
|
||||
// create MarkerData with nil Marker pointer
|
||||
MarkerData data;
|
||||
data.first.reset(new visualization_msgs::Marker(marker));
|
||||
// express marker pose relative to planning frame of end_scene
|
||||
if (!toPlanningFrame(const_cast<visualization_msgs::Marker&>(*data.first), end_scene))
|
||||
continue;
|
||||
markers_.push_back(std::move(data));
|
||||
if (!end_scene.knowsFrameTransform(marker.header.frame_id)) {
|
||||
ROS_WARN_ONCE("unknown frame '%s' for solution marker in namespace '%s'",
|
||||
marker.header.frame_id.c_str(), marker.ns.c_str());
|
||||
continue; // ignore markers with unknown frame
|
||||
}
|
||||
|
||||
// remember marker message
|
||||
markers_.emplace_back(marker);
|
||||
// remember namespace name
|
||||
namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr));
|
||||
namespaces_.insert(std::make_pair(marker.ns, NamespaceData()));
|
||||
}
|
||||
}
|
||||
|
||||
MarkerVisualization::~MarkerVisualization()
|
||||
{
|
||||
for (const auto& pair : namespaces_) {
|
||||
if (pair.second)
|
||||
pair.second->getCreator()->destroySceneNode(pair.second);
|
||||
if (pair.second.ns_node_)
|
||||
pair.second.ns_node_->getCreator()->destroySceneNode(pair.second.ns_node_);
|
||||
}
|
||||
}
|
||||
|
||||
@ -114,56 +102,124 @@ void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible)
|
||||
|
||||
void MarkerVisualization::setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible)
|
||||
{
|
||||
auto it = namespaces_.find(ns);
|
||||
auto it = namespaces_.find(ns.toStdString());
|
||||
if (it == namespaces_.end())
|
||||
return;
|
||||
setVisibility(it->second, parent_scene_node, visible);
|
||||
setVisibility(it->second.ns_node_, parent_scene_node, visible);
|
||||
}
|
||||
|
||||
void MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::SceneNode *parent_scene_node)
|
||||
bool MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::SceneNode *parent_scene_node)
|
||||
{
|
||||
std::string planning_frame;
|
||||
if (markers_created_)
|
||||
return true; // already called before
|
||||
|
||||
// fetch transform from planning_frame_ to rviz' fixed frame
|
||||
const std::string& fixed_frame = context->getFrameManager()->getFixedFrame();
|
||||
Ogre::Quaternion quat;
|
||||
Ogre::Vector3 pos;
|
||||
|
||||
for (MarkerData& data : markers_) {
|
||||
if (data.second) continue;
|
||||
|
||||
auto ns_it = namespaces_.find(QString::fromStdString(data.first->ns));
|
||||
Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before!
|
||||
if (ns_it->second == nullptr) // create scene node for this namespace
|
||||
ns_it->second = parent_scene_node->getCreator()->createSceneNode();
|
||||
|
||||
data.second.reset(createMarker(data.first->type, context, ns_it->second));
|
||||
if (data.second) {
|
||||
// rviz::MarkerBase::setMessage() initializes the marker
|
||||
data.second->setMessage(data.first);
|
||||
// ... and sets its position + orientation relative to rviz' fixed frame
|
||||
// however, we want the marker to be placed w.r.t. planning frame = msg.header.frame_id
|
||||
Q_ASSERT(!data.first->header.frame_id.empty());
|
||||
if (planning_frame.empty()) { // determine transform once
|
||||
planning_frame = data.first->header.frame_id;
|
||||
// transform from fixed frame to planning_frame
|
||||
tf::TransformListener* tf = context->getFrameManager()->getTFClient();
|
||||
tf::StampedTransform tm;
|
||||
tf->lookupTransform(context->getFrameManager()->getFixedFrame(), planning_frame, ros::Time(), tm);
|
||||
auto q = tm.getRotation();
|
||||
auto p = tm.getOrigin();
|
||||
quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z());
|
||||
pos = Ogre::Vector3(p.x(), p.y(), p.z());
|
||||
} else {
|
||||
Q_ASSERT(data.first->header.frame_id == planning_frame);
|
||||
}
|
||||
data.second->setOrientation(quat * data.second->getOrientation());
|
||||
data.second->setPosition(quat * (data.second->getPosition() - pos));
|
||||
try {
|
||||
tf::TransformListener* tf = context->getFrameManager()->getTFClient();
|
||||
std::string error_msg;
|
||||
if (!tf->canTransform(planning_frame_, fixed_frame, ros::Time(), &error_msg)) {
|
||||
ROS_WARN_STREAM_NAMED("MarkerVisualization", error_msg);
|
||||
return false; // frame transform not (yet) available
|
||||
}
|
||||
tf::StampedTransform tm;
|
||||
tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm);
|
||||
auto q = tm.getRotation();
|
||||
auto p = tm.getOrigin();
|
||||
quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z());
|
||||
pos = Ogre::Vector3(p.x(), p.y(), p.z());
|
||||
} catch (const tf2::TransformException& e) {
|
||||
ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
for (MarkerData& data : markers_) {
|
||||
if (data.marker_) continue;
|
||||
|
||||
auto ns_it = namespaces_.find(data.msg_->ns);
|
||||
Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before!
|
||||
if (ns_it->second.ns_node_ == nullptr) // create scene node for this namespace
|
||||
ns_it->second.ns_node_ = parent_scene_node->getCreator()->createSceneNode();
|
||||
Ogre::SceneNode* node = ns_it->second.ns_node_;
|
||||
|
||||
// create a scene node for all markers with given frame name
|
||||
auto frame_it = ns_it->second.frames_.insert(std::make_pair(data.msg_->header.frame_id, nullptr)).first;
|
||||
if (frame_it->second == nullptr)
|
||||
frame_it->second = node->createChildSceneNode();
|
||||
node = frame_it->second;
|
||||
|
||||
data.marker_.reset(createMarker(data.msg_->type, context, node));
|
||||
if (!data.marker_) continue; // failed to create marker
|
||||
|
||||
// setMessage() initializes the marker, placing it at the message-specified frame
|
||||
// w.r.t. rviz' current fixed frame. However, we want to place the marker w.r.t.
|
||||
// the planning frame of the planning scene!
|
||||
|
||||
// Hence, temporarily modify the message-specified frame to planning_frame_
|
||||
const std::string msg_frame = data.msg_->header.frame_id;
|
||||
data.msg_->header.frame_id = planning_frame_;
|
||||
data.marker_->setMessage(data.msg_);
|
||||
data.msg_->header.frame_id = msg_frame;
|
||||
|
||||
// ... and subsequently revert any transform between rviz' fixed frame and planning_frame_
|
||||
data.marker_->setOrientation(quat * data.marker_->getOrientation());
|
||||
data.marker_->setPosition(quat * data.marker_->getPosition() + pos);
|
||||
}
|
||||
markers_created_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
void MarkerVisualization::update(MarkerData& data,
|
||||
const planning_scene::PlanningScene &scene,
|
||||
const moveit::core::RobotState &robot_state) const
|
||||
{
|
||||
Q_ASSERT(scene.getPlanningFrame() == planning_frame_);
|
||||
|
||||
const visualization_msgs::Marker& marker = *data.msg_;
|
||||
if (marker.header.frame_id == scene.getPlanningFrame())
|
||||
return; // no need to transform nodes placed at planning frame
|
||||
|
||||
// fetch base pose from robot_state / scene
|
||||
Eigen::Affine3d pose;
|
||||
if (robot_state.knowsFrameTransform(marker.header.frame_id))
|
||||
pose = robot_state.getFrameTransform(marker.header.frame_id);
|
||||
else if (scene.knowsFrameTransform(marker.header.frame_id))
|
||||
pose = scene.getFrameTransform(marker.header.frame_id);
|
||||
else {
|
||||
ROS_WARN_ONCE_NAMED("MarkerVisualization",
|
||||
"unknown frame '%s' for solution marker in namespace '%s'",
|
||||
marker.header.frame_id.c_str(), marker.ns.c_str());
|
||||
return; // ignore markers with unknown frame
|
||||
}
|
||||
|
||||
auto ns_it = namespaces_.find(marker.ns);
|
||||
Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before
|
||||
auto frame_it = ns_it->second.frames_.find(marker.header.frame_id);
|
||||
Q_ASSERT(frame_it != ns_it->second.frames_.end()); // we have created all of them
|
||||
|
||||
const Eigen::Quaterniond q = (Eigen::Quaterniond)pose.linear();
|
||||
const Eigen::Vector3d& p = pose.translation();
|
||||
frame_it->second->setOrientation(Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()));
|
||||
frame_it->second->setPosition(Ogre::Vector3(p.x(), p.y(), p.z()));
|
||||
}
|
||||
|
||||
void MarkerVisualization::update(const planning_scene::PlanningScene &end_scene,
|
||||
const moveit::core::RobotState &robot_state)
|
||||
{
|
||||
for (MarkerData& data : markers_)
|
||||
update(data, end_scene, robot_state);
|
||||
}
|
||||
|
||||
|
||||
MarkerVisualizationProperty::MarkerVisualizationProperty(const QString &name, rviz::Property *parent)
|
||||
: rviz::BoolProperty(name, true, "Enable/disable markers", parent)
|
||||
{
|
||||
all_markers_at_once_ = new rviz::BoolProperty("All at once?", false, "Show all markers of multiple subsolutions at once?",
|
||||
this, SLOT(onAllAtOnceChanged()), this);
|
||||
|
||||
connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged()));
|
||||
}
|
||||
|
||||
@ -194,24 +250,42 @@ void MarkerVisualizationProperty::addMarkers(MarkerVisualizationPtr markers)
|
||||
|
||||
// remember that those markers are hosted
|
||||
hosted_markers_.push_back(markers);
|
||||
// create markers if not yet done
|
||||
if (!markers->created() && !markers->createMarkers(context_, marker_scene_node_))
|
||||
return; // if markers not created, nothing to do here
|
||||
|
||||
// attach all scene nodes from markers
|
||||
for (const auto& pair : markers->namespaces()) {
|
||||
QString ns = QString::fromStdString(pair.first);
|
||||
// create sub property for newly encountered namespace, enabling visibility by default
|
||||
auto ns_it = namespaces_.insert(std::make_pair(pair.first, nullptr)).first;
|
||||
auto ns_it = namespaces_.insert(std::make_pair(ns, nullptr)).first;
|
||||
if (ns_it->second == nullptr) {
|
||||
ns_it->second = new rviz::BoolProperty(pair.first, true, "Show/hide markers of this namespace", this,
|
||||
ns_it->second = new rviz::BoolProperty(ns, true, "Show/hide markers of this namespace", this,
|
||||
SLOT(onNSEnableChanged()), this);
|
||||
}
|
||||
if (!pair.second) // invalid scene node indicates that we still need to create the rviz markers
|
||||
markers->createMarkers(context_, marker_scene_node_);
|
||||
Q_ASSERT(pair.second);
|
||||
Q_ASSERT(pair.second.ns_node_); // nodes should have been created in createMarkers()
|
||||
|
||||
if (ns_it->second->getBool())
|
||||
marker_scene_node_->addChild(pair.second);
|
||||
marker_scene_node_->addChild(pair.second.ns_node_);
|
||||
}
|
||||
}
|
||||
|
||||
void MarkerVisualizationProperty::update(const planning_scene::PlanningScene &scene,
|
||||
const moveit::core::RobotState &robot_state)
|
||||
{
|
||||
for (const auto& markers : hosted_markers_) {
|
||||
if (!markers->created())
|
||||
if (!markers->createMarkers(context_, marker_scene_node_))
|
||||
continue;
|
||||
markers->update(scene, robot_state);
|
||||
}
|
||||
}
|
||||
|
||||
bool MarkerVisualizationProperty::allAtOnce() const
|
||||
{
|
||||
return all_markers_at_once_->getBool();
|
||||
}
|
||||
|
||||
void MarkerVisualizationProperty::onEnableChanged()
|
||||
{
|
||||
setVisibility(marker_scene_node_, parent_scene_node_, getBool());
|
||||
@ -227,4 +301,9 @@ void MarkerVisualizationProperty::onNSEnableChanged()
|
||||
markers->setVisible(ns, marker_scene_node_, visible);
|
||||
}
|
||||
|
||||
void MarkerVisualizationProperty::onAllAtOnceChanged()
|
||||
{
|
||||
Q_EMIT allAtOnceChanged(all_markers_at_once_->getBool());
|
||||
}
|
||||
|
||||
} // namespace moveit_rviz_plugin
|
||||
|
||||
@ -36,6 +36,7 @@
|
||||
|
||||
#include <moveit/visualization_tools/display_solution.h>
|
||||
#include <moveit/visualization_tools/task_solution_visualization.h>
|
||||
#include <moveit/visualization_tools/marker_visualization.h>
|
||||
#include <moveit/visualization_tools/task_solution_panel.h>
|
||||
|
||||
#include <moveit/rviz_plugin_render_tools/planning_scene_render.h>
|
||||
@ -152,6 +153,9 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi
|
||||
|
||||
octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR);
|
||||
octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR);
|
||||
|
||||
marker_visual_ = new MarkerVisualizationProperty("Markers", parent);
|
||||
connect(marker_visual_, SIGNAL(allAtOnceChanged(bool)), this, SLOT(onAllAtOnceChanged(bool)));
|
||||
}
|
||||
|
||||
TaskSolutionVisualization::~TaskSolutionVisualization()
|
||||
@ -165,6 +169,8 @@ TaskSolutionVisualization::~TaskSolutionVisualization()
|
||||
if (slider_dock_panel_)
|
||||
delete slider_dock_panel_;
|
||||
|
||||
delete marker_visual_;
|
||||
|
||||
if (main_scene_node_)
|
||||
main_scene_node_->getCreator()->destroySceneNode(main_scene_node_);
|
||||
}
|
||||
@ -188,6 +194,8 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::
|
||||
scene_render_.reset(new PlanningSceneRender(main_scene_node_, context_, RobotStateVisualizationPtr()));
|
||||
scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
|
||||
|
||||
marker_visual_->onInitialize(main_scene_node_, context_);
|
||||
|
||||
rviz::WindowManagerInterface* window_context = context_->getWindowManager();
|
||||
if (window_context)
|
||||
{
|
||||
@ -330,6 +338,18 @@ void TaskSolutionVisualization::interruptCurrentDisplay()
|
||||
drop_displaying_solution_ = true;
|
||||
}
|
||||
|
||||
void TaskSolutionVisualization::onAllAtOnceChanged(bool all_at_once)
|
||||
{
|
||||
if (!displaying_solution_)
|
||||
return;
|
||||
clearMarkers();
|
||||
|
||||
if (all_at_once)
|
||||
addMarkers(displaying_solution_);
|
||||
else if (current_state_ >= 0)
|
||||
renderWayPoint(current_state_, -1);
|
||||
}
|
||||
|
||||
float TaskSolutionVisualization::getStateDisplayTime()
|
||||
{
|
||||
std::string tm = state_display_time_property_->getStdString();
|
||||
@ -458,6 +478,11 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
|
||||
displaying_solution_->indexPair(previous_index).first != idx_pair.first) {
|
||||
// switch to new stage: show new planning scene
|
||||
renderPlanningScene (scene);
|
||||
// switch to markers of next sub trajectory?
|
||||
if (!marker_visual_->allAtOnce()) {
|
||||
marker_visual_->clearMarkers();
|
||||
marker_visual_->addMarkers(displaying_solution_->markersOfSubTrajectory(idx_pair.first));
|
||||
}
|
||||
Q_EMIT activeStageChanged(displaying_solution_->creatorId(idx_pair));
|
||||
}
|
||||
robot_state = displaying_solution_->getWayPointPtr(idx_pair);
|
||||
@ -473,6 +498,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
|
||||
planning_scene::ObjectColorMap color_map;
|
||||
scene->getKnownObjectColors(color_map);
|
||||
robot_render_->update(robot_state, color, color_map);
|
||||
marker_visual_->update(*scene, *robot_state);
|
||||
|
||||
if (slider_panel_ && index >= 0)
|
||||
slider_panel_->setSliderPosition(index);
|
||||
@ -502,7 +528,7 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msg
|
||||
showTrajectory(s, false);
|
||||
}
|
||||
|
||||
void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s, bool lock_display)
|
||||
void TaskSolutionVisualization::showTrajectory(const DisplaySolutionPtr& s, bool lock_display)
|
||||
{
|
||||
if (lock_display || !s->empty()) {
|
||||
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
||||
@ -516,7 +542,22 @@ void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s, bool lock_d
|
||||
|
||||
void TaskSolutionVisualization::unlock()
|
||||
{
|
||||
locked_ = false;
|
||||
locked_ = false;
|
||||
}
|
||||
|
||||
void TaskSolutionVisualization::clearMarkers()
|
||||
{
|
||||
marker_visual_->clearMarkers();
|
||||
}
|
||||
|
||||
void TaskSolutionVisualization::addMarkers(const moveit_rviz_plugin::DisplaySolutionPtr &s)
|
||||
{
|
||||
if (!s || (!marker_visual_->allAtOnce() && s->numSubSolutions() > 1))
|
||||
return;
|
||||
|
||||
for (size_t i = 0, end = s->numSubSolutions(); i != end; ++i) {
|
||||
marker_visual_->addMarkers(s->markersOfSubTrajectory(i));
|
||||
}
|
||||
}
|
||||
|
||||
void TaskSolutionVisualization::changedRobotColor()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user