diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 78ecfe37..4dc2cb7a 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -350,7 +350,7 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index) // to avoid some communication overhead DisplaySolutionPtr result; - if (get_solution_client_) { + if (!(flags_ & IS_DESTROYED) && get_solution_client_) { // request solution via service moveit_task_constructor_msgs::GetSolution srv; srv.request.solution_id = id; diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 6891e395..f503cff5 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -58,6 +58,8 @@ namespace moveit_rviz_plugin TaskDisplay::TaskDisplay() : Display() { task_list_model_.reset(new TaskListModel); + task_list_model_->setSolutionClient(&get_solution_client); + MetaTaskListModel::instance().insertModel(task_list_model_.get(), this); connect(task_list_model_.get(), SIGNAL(rowsInserted(QModelIndex,int,int)), @@ -165,6 +167,7 @@ void TaskDisplay::taskDescriptionCB(const ros::MessageEventid; mainloop_jobs_.addJob([this, id, msg]() { + setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); task_list_model_->processTaskDescriptionMessage(id, *msg); }); } @@ -174,6 +177,7 @@ void TaskDisplay::taskStatisticsCB(const ros::MessageEventid; mainloop_jobs_.addJob([this, id, msg]() { + setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); task_list_model_->processTaskStatisticsMessage(id, *msg); }); } @@ -183,6 +187,7 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEventtask_id; mainloop_jobs_.addJob([this, id, msg]() { + setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg); if (s) trajectory_visual_->showTrajectory(s, false); return; @@ -210,14 +215,14 @@ void TaskDisplay::changedTaskSolutionTopic() tasks_property_->removeChildren(); // generate task monitoring topics from solution topic - std::string solution_topic = task_solution_topic_property_->getStdString(); - auto last_sep = solution_topic.find_last_of('/'); - if (last_sep == std::string::npos) { - setStatus(rviz::StatusProperty::Error, "Task Monitor", "invalid topic"); + const QString& solution_topic = task_solution_topic_property_->getString(); + if (!solution_topic.endsWith(QString("/").append(SOLUTION_TOPIC))) { + setStatus(rviz::StatusProperty::Error, "Task Monitor", + QString("Invalid topic. Expecting a name ending on \"/%1\"").arg(SOLUTION_TOPIC)); return; } - std::string base_ns = solution_topic.substr(0, last_sep+1); + std::string base_ns = solution_topic.toStdString().substr(0, solution_topic.length() - strlen(SOLUTION_TOPIC)); // listen to task descriptions updates task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this); @@ -226,13 +231,18 @@ void TaskDisplay::changedTaskSolutionTopic() task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this); // listen to task solutions - task_solution_sub = update_nh_.subscribe(solution_topic, 2, &TaskDisplay::taskSolutionCB, this); + task_solution_sub = update_nh_.subscribe(base_ns + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this); // service to request solutions get_solution_client = update_nh_.serviceClient(base_ns + GET_SOLUTION_SERVICE); - task_list_model_->setSolutionClient(&get_solution_client); - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected"); + setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received"); +} + +void TaskDisplay::setSolutionStatus(bool ok) +{ + if (ok) setStatus(rviz::StatusProperty::Ok, "Solution", "Ok"); + else setStatus(rviz::StatusProperty::Warn, "Solution", "Retrieval failed"); } void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last) diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index 4d928a54..3655f74d 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -85,6 +85,7 @@ public: virtual void onEnable(); virtual void onDisable(); void setName(const QString& name); + void setSolutionStatus(bool ok); TaskListModel& getTaskListModel() { return *task_list_model_; } TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); } diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index c5e9e762..bfe54d87 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -156,15 +156,6 @@ void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr &scene) void TaskListModel::setSolutionClient(ros::ServiceClient *client) { - if (!client || get_solution_client_ != client || - get_solution_client_->getService() != client->getService()) { - // service client's address changed: invalidate existing client pointers - for (int row=0, end = rowCount(); row != end; ++row) { - RemoteTaskModel* task = dynamic_cast(getModel(index(row, 0)).first); - if (!task) continue; - task->setSolutionClient(nullptr); - } - } get_solution_client_ = client; } diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index e37c2abe..b16b5810 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -231,8 +231,10 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode d_ptr->locked_display_ = display; TaskSolutionVisualization* vis = display->visualization(); + const DisplaySolutionPtr& solution = task->getSolution(current); + display->setSolutionStatus(bool(solution)); vis->interruptCurrentDisplay(); - vis->showTrajectory(task->getSolution(current), true); + vis->showTrajectory(solution, true); } void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected) @@ -248,6 +250,7 @@ void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const display->clearMarkers(); for (const auto& index : selected_rows) { const DisplaySolutionPtr &solution = task->getSolution(index); + display->setSolutionStatus(bool(solution)); display->showMarkers(solution); } }