share scene/robot model between TaskDisplay and associated TaskListModel

This commit is contained in:
Robert Haschke 2017-11-20 00:57:25 +01:00
parent 957b10a7a8
commit c6c97e1836
10 changed files with 87 additions and 35 deletions

View File

@ -38,6 +38,7 @@
#include "remote_task_model.h"
#include <moveit/task_constructor/container.h>
#include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
#include <QApplication>
@ -107,8 +108,8 @@ QModelIndex RemoteTaskModel::index(const Node *n) const
Q_ASSERT(false);
}
RemoteTaskModel::RemoteTaskModel(QObject *parent)
: BaseTaskModel(parent), root_(new Node(nullptr))
RemoteTaskModel::RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, QObject *parent)
: BaseTaskModel(parent), root_(new Node(nullptr)), scene_(scene)
{
id_to_stage_[0] = root_; // root node has ID 0
}
@ -273,6 +274,23 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs:
}
}
DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg)
{
DisplaySolutionPtr s(new DisplaySolution);
s->setFromMessage(scene_->diff(), msg);
// if this is not a top-level solution, we are done here
if (msg.sub_solution.empty() ||
msg.sub_solution.front().stage_id != 1 ||
msg.sub_solution.front().id == 0)
return s;
// cache top-level solution for future use
id_to_solution_[msg.sub_solution.front().id] = s;
return s;
}
RemoteSolutionModel::RemoteSolutionModel(QObject *parent)
: QAbstractListModel(parent)

View File

@ -50,6 +50,7 @@ class RemoteTaskModel : public BaseTaskModel {
Q_OBJECT
class Node;
Node* const root_;
planning_scene::PlanningSceneConstPtr scene_;
std::map<uint32_t, Node*> id_to_stage_;
std::map<uint32_t, DisplaySolutionPtr> id_to_solution_;
@ -57,7 +58,7 @@ class RemoteTaskModel : public BaseTaskModel {
QModelIndex index(const Node* n) const;
public:
RemoteTaskModel(QObject *parent = nullptr);
RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, QObject *parent = nullptr);
~RemoteTaskModel();
int rowCount(const QModelIndex &parent = QModelIndex()) const override;
@ -70,6 +71,7 @@ public:
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg);
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg);
};

View File

@ -93,6 +93,10 @@ void TaskDisplay::loadRobotModel()
// Send to child class
trajectory_visual_->onRobotModelLoaded(robot_model_);
trajectory_visual_->onEnable();
// share the planning scene with task models
if (task_list_model_)
task_list_model_->setScene(trajectory_visual_->getScene());
}
void TaskDisplay::reset()
@ -106,20 +110,12 @@ void TaskDisplay::onEnable()
{
Display::onEnable();
loadRobotModel();
// (re)initialize task model
updateTaskListModel();
}
void TaskDisplay::onDisable()
{
Display::onDisable();
trajectory_visual_->onDisable();
// don't monitor topics when disabled
task_description_sub.shutdown();
task_statistics_sub.shutdown();
task_solution_sub.shutdown();
}
void TaskDisplay::update(float wall_dt, float ros_dt)
@ -166,8 +162,11 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_const
const moveit_task_constructor_msgs::SolutionConstPtr& msg = event.getMessage();
const std::string id = event.getPublisherName() + "/" + msg->task_id;
mainloop_jobs_.addJob([this, id, msg]() {
if (task_list_model_) task_list_model_->processSolutionMessage(id, *msg);
// TODO: use already processed trajectory (e.g. by ID)
if (task_list_model_) {
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
trajectory_visual_->showTrajectory(s);
return;
}
trajectory_visual_->showTrajectory(*msg);
});
}
@ -196,6 +195,8 @@ void TaskDisplay::updateTaskListModel()
task_list_model_ = TaskListModelCache::instance().getModel(base_ns);
if (task_list_model_) {
task_list_model_->setScene(trajectory_visual_->getScene());
// listen to task descriptions updates
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);

View File

@ -97,6 +97,8 @@ class TaskListModelPrivate {
public:
Q_DECLARE_PUBLIC(TaskListModel)
TaskListModel* q_ptr;
// planning scene / robot model used by all tasks in this model
planning_scene::PlanningSceneConstPtr scene_;
struct BaseModelData {
BaseModelData(BaseTaskModel* m) : model_(m) {}
@ -122,7 +124,8 @@ public:
// retrieve the source_index corresponding to proxy_index
QModelIndex mapToSource(const QModelIndex &proxy_index, BaseModelData **task = nullptr) const {
Q_ASSERT(proxy_index.isValid());
if (!proxy_index.isValid())
return QModelIndex();
Q_ASSERT(proxy_index.model() == q_ptr);
void* internal_pointer = proxy_index.internalPointer();
@ -216,6 +219,11 @@ TaskListModel::~TaskListModel() {
delete d_ptr;
}
void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr &scene)
{
d_ptr->scene_ = scene;
}
int TaskListModel::rowCount(const QModelIndex &parent) const
{
Q_D(const TaskListModel);
@ -341,7 +349,7 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
}
} else if (created) { // create new task model, if ID was not known before
// the model is managed by this instance via Qt's parent-child mechanism
remote_task = new RemoteTaskModel(this);
remote_task = new RemoteTaskModel(d->scene_, this);
}
if (!remote_task)
return; // task is not in use anymore
@ -372,10 +380,20 @@ void TaskListModel::processTaskStatisticsMessage(const std::string &id,
remote_task->processStageStatistics(msg.stages);
}
void TaskListModel::processSolutionMessage(const std::string &id,
DisplaySolutionPtr TaskListModel::processSolutionMessage(const std::string &id,
const moveit_task_constructor_msgs::Solution &msg)
{
// TODO
Q_D(TaskListModel);
auto it = d->remote_tasks_.find(id);
if (it == d->remote_tasks_.cend())
return DisplaySolutionPtr(); // unkown task
RemoteTaskModel* remote_task = it->second;
if (!remote_task)
return DisplaySolutionPtr(); // task is not in use anymore
return remote_task->processSolutionMessage(msg);
}
BaseTaskModel *TaskListModel::getTask(int row) const

View File

@ -50,6 +50,8 @@
namespace moveit_rviz_plugin {
MOVEIT_CLASS_FORWARD(DisplaySolution)
/** Base class to represent a single local or remote Task as a Qt model. */
class BaseTaskModel : public QAbstractItemModel {
Q_OBJECT
@ -96,6 +98,8 @@ public:
TaskListModel(QObject *parent = nullptr);
~TaskListModel();
void setScene(const planning_scene::PlanningSceneConstPtr& scene);
static QString horizontalHeader(int column);
QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
@ -116,7 +120,7 @@ public:
/// process an incoming task description message - only call in Qt's main loop
void processTaskStatisticsMessage(const std::string &id, const moveit_task_constructor_msgs::TaskStatistics &msg);
/// process an incoming solution message - only call in Qt's main loop
void processSolutionMessage(const std::string &id, const moveit_task_constructor_msgs::Solution &msg);
DisplaySolutionPtr processSolutionMessage(const std::string &id, const moveit_task_constructor_msgs::Solution &msg);
/// retrieve TaskModel in given row
BaseTaskModel* getTask(int row) const;

View File

@ -156,7 +156,8 @@ protected:
TEST_F(TaskListModelTest, remoteTaskModel) {
children = 3;
moveit_rviz_plugin::RemoteTaskModel m;
planning_scene::PlanningSceneConstPtr scene;
moveit_rviz_plugin::RemoteTaskModel m(scene);
m.processStageDescriptions(genMsg("first").stages);
SCOPED_TRACE("first");
validate(m, {"first"});

View File

@ -22,6 +22,8 @@ class DisplaySolution
{
/// number of overall steps
size_t steps_;
/// start scene
planning_scene::PlanningSceneConstPtr start_scene_;
/// end scenes for each sub trajectory
std::vector<planning_scene::PlanningSceneConstPtr> scene_;
/// sub trajectories, might be empty
@ -56,7 +58,7 @@ public:
return name(indexPair(index));
}
void setFromMessage(const planning_scene::PlanningSceneConstPtr &parent,
void setFromMessage(const planning_scene::PlanningScenePtr &start_scene,
const moveit_task_constructor_msgs::Solution& msg);
};

View File

@ -103,7 +103,9 @@ public:
void onDisable();
void setName(const QString& name);
planning_scene::PlanningSceneConstPtr getScene() const { return scene_; }
void showTrajectory(const moveit_task_constructor_msgs::Solution& msg);
void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s);
void dropTrajectory();
public Q_SLOTS:

View File

@ -1,6 +1,7 @@
#include <moveit/visualization_tools/display_solution.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <ros/console.h>
namespace moveit_rviz_plugin {
@ -39,10 +40,20 @@ const std::string &DisplaySolution::name(const IndexPair &idx_pair) const
return name_[idx_pair.first];
}
void DisplaySolution::setFromMessage(const planning_scene::PlanningSceneConstPtr& parent,
void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene,
const moveit_task_constructor_msgs::Solution &msg)
{
planning_scene::PlanningScenePtr ref_scene = parent->diff();
if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) {
ROS_ERROR("Solution for model '%s' but model '%s' was expected",
msg.start_scene.robot_model_name .c_str(),
start_scene->getRobotModel()->getName().c_str());
return;
}
// initialize parent scene from solution's start scene
start_scene->setPlanningSceneMsg(msg.start_scene);
start_scene_ = start_scene;
planning_scene::PlanningScenePtr ref_scene = start_scene_->diff();
scene_.resize(msg.sub_trajectory.size());
trajectory_.resize(msg.sub_trajectory.size());

View File

@ -489,22 +489,15 @@ void TaskSolutionVisualization::renderPlanningScene(const planning_scene::Planni
void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msgs::Solution& msg)
{
// Error check
if (!scene_)
return;
if (msg.start_scene.robot_model_name != scene_->getRobotModel()->getName())
ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected",
msg.start_scene.robot_model_name .c_str(),
scene_->getRobotModel()->getName().c_str());
scene_->setPlanningSceneMsg(msg.start_scene);
DisplaySolutionPtr s (new DisplaySolution);
s->setFromMessage(scene_, msg);
showTrajectory(s);
}
if (!s->empty())
{
void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s)
{
if (!s->empty()) {
boost::mutex::scoped_lock lock(display_solution_mutex_);
solution_to_display_ = s;
if (interrupt_display_property_->getBool())