diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 196739d3..500e1796 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -38,6 +38,7 @@ #include "remote_task_model.h" #include +#include #include #include @@ -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) diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index 11755cfc..91b7e61a 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -50,6 +50,7 @@ class RemoteTaskModel : public BaseTaskModel { Q_OBJECT class Node; Node* const root_; + planning_scene::PlanningSceneConstPtr scene_; std::map id_to_stage_; std::map 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); }; diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 93a8aacb..3f4e455b 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -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::MessageEventtask_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); diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index d0bd5bac..3deae368 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -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, - const moveit_task_constructor_msgs::Solution &msg) +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 diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 7b0b70b8..bd177387 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -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; diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 92b19ea7..60df7d47 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -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"}); diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index eb7e0048..88c4c3dc 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -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 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); }; diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index 22e13efa..fef7efb8 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -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: diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index 3e57b83f..9dc4f9d8 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -1,6 +1,7 @@ #include #include #include +#include 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()); diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index f4e49046..cd6923c8 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -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())