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 <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)
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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"});
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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());
|
||||
|
||||
@ -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())
|
||||
|
||||
Loading…
Reference in New Issue
Block a user