mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
share scene/robot model between TaskDisplay and associated TaskListModel
This commit is contained in:
parent
957b10a7a8
commit
c6c97e1836
@ -38,6 +38,7 @@
|
|||||||
|
|
||||||
#include "remote_task_model.h"
|
#include "remote_task_model.h"
|
||||||
#include <moveit/task_constructor/container.h>
|
#include <moveit/task_constructor/container.h>
|
||||||
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
|
|
||||||
#include <QApplication>
|
#include <QApplication>
|
||||||
@ -107,8 +108,8 @@ QModelIndex RemoteTaskModel::index(const Node *n) const
|
|||||||
Q_ASSERT(false);
|
Q_ASSERT(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
RemoteTaskModel::RemoteTaskModel(QObject *parent)
|
RemoteTaskModel::RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, QObject *parent)
|
||||||
: BaseTaskModel(parent), root_(new Node(nullptr))
|
: BaseTaskModel(parent), root_(new Node(nullptr)), scene_(scene)
|
||||||
{
|
{
|
||||||
id_to_stage_[0] = root_; // root node has ID 0
|
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)
|
RemoteSolutionModel::RemoteSolutionModel(QObject *parent)
|
||||||
: QAbstractListModel(parent)
|
: QAbstractListModel(parent)
|
||||||
|
|||||||
@ -50,6 +50,7 @@ class RemoteTaskModel : public BaseTaskModel {
|
|||||||
Q_OBJECT
|
Q_OBJECT
|
||||||
class Node;
|
class Node;
|
||||||
Node* const root_;
|
Node* const root_;
|
||||||
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
std::map<uint32_t, Node*> id_to_stage_;
|
std::map<uint32_t, Node*> id_to_stage_;
|
||||||
std::map<uint32_t, DisplaySolutionPtr> id_to_solution_;
|
std::map<uint32_t, DisplaySolutionPtr> id_to_solution_;
|
||||||
|
|
||||||
@ -57,7 +58,7 @@ class RemoteTaskModel : public BaseTaskModel {
|
|||||||
QModelIndex index(const Node* n) const;
|
QModelIndex index(const Node* n) const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
RemoteTaskModel(QObject *parent = nullptr);
|
RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, QObject *parent = nullptr);
|
||||||
~RemoteTaskModel();
|
~RemoteTaskModel();
|
||||||
int rowCount(const QModelIndex &parent = QModelIndex()) const override;
|
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 processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg);
|
||||||
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg);
|
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg);
|
||||||
|
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -93,6 +93,10 @@ void TaskDisplay::loadRobotModel()
|
|||||||
// Send to child class
|
// Send to child class
|
||||||
trajectory_visual_->onRobotModelLoaded(robot_model_);
|
trajectory_visual_->onRobotModelLoaded(robot_model_);
|
||||||
trajectory_visual_->onEnable();
|
trajectory_visual_->onEnable();
|
||||||
|
|
||||||
|
// share the planning scene with task models
|
||||||
|
if (task_list_model_)
|
||||||
|
task_list_model_->setScene(trajectory_visual_->getScene());
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskDisplay::reset()
|
void TaskDisplay::reset()
|
||||||
@ -106,20 +110,12 @@ void TaskDisplay::onEnable()
|
|||||||
{
|
{
|
||||||
Display::onEnable();
|
Display::onEnable();
|
||||||
loadRobotModel();
|
loadRobotModel();
|
||||||
|
|
||||||
// (re)initialize task model
|
|
||||||
updateTaskListModel();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskDisplay::onDisable()
|
void TaskDisplay::onDisable()
|
||||||
{
|
{
|
||||||
Display::onDisable();
|
Display::onDisable();
|
||||||
trajectory_visual_->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)
|
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 moveit_task_constructor_msgs::SolutionConstPtr& msg = event.getMessage();
|
||||||
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
||||||
mainloop_jobs_.addJob([this, id, msg]() {
|
mainloop_jobs_.addJob([this, id, msg]() {
|
||||||
if (task_list_model_) task_list_model_->processSolutionMessage(id, *msg);
|
if (task_list_model_) {
|
||||||
// TODO: use already processed trajectory (e.g. by ID)
|
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
|
||||||
|
trajectory_visual_->showTrajectory(s);
|
||||||
|
return;
|
||||||
|
}
|
||||||
trajectory_visual_->showTrajectory(*msg);
|
trajectory_visual_->showTrajectory(*msg);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
@ -196,6 +195,8 @@ void TaskDisplay::updateTaskListModel()
|
|||||||
task_list_model_ = TaskListModelCache::instance().getModel(base_ns);
|
task_list_model_ = TaskListModelCache::instance().getModel(base_ns);
|
||||||
|
|
||||||
if (task_list_model_) {
|
if (task_list_model_) {
|
||||||
|
task_list_model_->setScene(trajectory_visual_->getScene());
|
||||||
|
|
||||||
// listen to task descriptions updates
|
// listen to task descriptions updates
|
||||||
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);
|
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);
|
||||||
|
|
||||||
|
|||||||
@ -97,6 +97,8 @@ class TaskListModelPrivate {
|
|||||||
public:
|
public:
|
||||||
Q_DECLARE_PUBLIC(TaskListModel)
|
Q_DECLARE_PUBLIC(TaskListModel)
|
||||||
TaskListModel* q_ptr;
|
TaskListModel* q_ptr;
|
||||||
|
// planning scene / robot model used by all tasks in this model
|
||||||
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
|
|
||||||
struct BaseModelData {
|
struct BaseModelData {
|
||||||
BaseModelData(BaseTaskModel* m) : model_(m) {}
|
BaseModelData(BaseTaskModel* m) : model_(m) {}
|
||||||
@ -122,7 +124,8 @@ public:
|
|||||||
|
|
||||||
// retrieve the source_index corresponding to proxy_index
|
// retrieve the source_index corresponding to proxy_index
|
||||||
QModelIndex mapToSource(const QModelIndex &proxy_index, BaseModelData **task = nullptr) const {
|
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);
|
Q_ASSERT(proxy_index.model() == q_ptr);
|
||||||
|
|
||||||
void* internal_pointer = proxy_index.internalPointer();
|
void* internal_pointer = proxy_index.internalPointer();
|
||||||
@ -216,6 +219,11 @@ TaskListModel::~TaskListModel() {
|
|||||||
delete d_ptr;
|
delete d_ptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
|
{
|
||||||
|
d_ptr->scene_ = scene;
|
||||||
|
}
|
||||||
|
|
||||||
int TaskListModel::rowCount(const QModelIndex &parent) const
|
int TaskListModel::rowCount(const QModelIndex &parent) const
|
||||||
{
|
{
|
||||||
Q_D(const TaskListModel);
|
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
|
} 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
|
// 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)
|
if (!remote_task)
|
||||||
return; // task is not in use anymore
|
return; // task is not in use anymore
|
||||||
@ -372,10 +380,20 @@ void TaskListModel::processTaskStatisticsMessage(const std::string &id,
|
|||||||
remote_task->processStageStatistics(msg.stages);
|
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)
|
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
|
BaseTaskModel *TaskListModel::getTask(int row) const
|
||||||
|
|||||||
@ -50,6 +50,8 @@
|
|||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
|
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||||
|
|
||||||
/** Base class to represent a single local or remote Task as a Qt model. */
|
/** Base class to represent a single local or remote Task as a Qt model. */
|
||||||
class BaseTaskModel : public QAbstractItemModel {
|
class BaseTaskModel : public QAbstractItemModel {
|
||||||
Q_OBJECT
|
Q_OBJECT
|
||||||
@ -96,6 +98,8 @@ public:
|
|||||||
TaskListModel(QObject *parent = nullptr);
|
TaskListModel(QObject *parent = nullptr);
|
||||||
~TaskListModel();
|
~TaskListModel();
|
||||||
|
|
||||||
|
void setScene(const planning_scene::PlanningSceneConstPtr& scene);
|
||||||
|
|
||||||
static QString horizontalHeader(int column);
|
static QString horizontalHeader(int column);
|
||||||
QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
|
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
|
/// 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);
|
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
|
/// 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
|
/// retrieve TaskModel in given row
|
||||||
BaseTaskModel* getTask(int row) const;
|
BaseTaskModel* getTask(int row) const;
|
||||||
|
|||||||
@ -156,7 +156,8 @@ protected:
|
|||||||
|
|
||||||
TEST_F(TaskListModelTest, remoteTaskModel) {
|
TEST_F(TaskListModelTest, remoteTaskModel) {
|
||||||
children = 3;
|
children = 3;
|
||||||
moveit_rviz_plugin::RemoteTaskModel m;
|
planning_scene::PlanningSceneConstPtr scene;
|
||||||
|
moveit_rviz_plugin::RemoteTaskModel m(scene);
|
||||||
m.processStageDescriptions(genMsg("first").stages);
|
m.processStageDescriptions(genMsg("first").stages);
|
||||||
SCOPED_TRACE("first");
|
SCOPED_TRACE("first");
|
||||||
validate(m, {"first"});
|
validate(m, {"first"});
|
||||||
|
|||||||
@ -22,6 +22,8 @@ class DisplaySolution
|
|||||||
{
|
{
|
||||||
/// number of overall steps
|
/// number of overall steps
|
||||||
size_t steps_;
|
size_t steps_;
|
||||||
|
/// start scene
|
||||||
|
planning_scene::PlanningSceneConstPtr start_scene_;
|
||||||
/// end scenes for each sub trajectory
|
/// end scenes for each sub trajectory
|
||||||
std::vector<planning_scene::PlanningSceneConstPtr> scene_;
|
std::vector<planning_scene::PlanningSceneConstPtr> scene_;
|
||||||
/// sub trajectories, might be empty
|
/// sub trajectories, might be empty
|
||||||
@ -56,7 +58,7 @@ public:
|
|||||||
return name(indexPair(index));
|
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);
|
const moveit_task_constructor_msgs::Solution& msg);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -103,7 +103,9 @@ public:
|
|||||||
void onDisable();
|
void onDisable();
|
||||||
void setName(const QString& name);
|
void setName(const QString& name);
|
||||||
|
|
||||||
|
planning_scene::PlanningSceneConstPtr getScene() const { return scene_; }
|
||||||
void showTrajectory(const moveit_task_constructor_msgs::Solution& msg);
|
void showTrajectory(const moveit_task_constructor_msgs::Solution& msg);
|
||||||
|
void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s);
|
||||||
void dropTrajectory();
|
void dropTrajectory();
|
||||||
|
|
||||||
public Q_SLOTS:
|
public Q_SLOTS:
|
||||||
|
|||||||
@ -1,6 +1,7 @@
|
|||||||
#include <moveit/visualization_tools/display_solution.h>
|
#include <moveit/visualization_tools/display_solution.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||||
|
#include <ros/console.h>
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
@ -39,10 +40,20 @@ const std::string &DisplaySolution::name(const IndexPair &idx_pair) const
|
|||||||
return name_[idx_pair.first];
|
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)
|
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());
|
scene_.resize(msg.sub_trajectory.size());
|
||||||
trajectory_.resize(msg.sub_trajectory.size());
|
trajectory_.resize(msg.sub_trajectory.size());
|
||||||
|
|||||||
@ -489,22 +489,15 @@ void TaskSolutionVisualization::renderPlanningScene(const planning_scene::Planni
|
|||||||
|
|
||||||
void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msgs::Solution& msg)
|
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);
|
DisplaySolutionPtr s (new DisplaySolution);
|
||||||
s->setFromMessage(scene_, msg);
|
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_);
|
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
||||||
solution_to_display_ = s;
|
solution_to_display_ = s;
|
||||||
if (interrupt_display_property_->getBool())
|
if (interrupt_display_property_->getBool())
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user