bug fixes

- only attempt to retrieve solution if remote was not yet destroyed
- setting get_solution_client is neccessary only once (pointer never changes)
- more reliable topic handling + status feedback
This commit is contained in:
Robert Haschke 2018-01-08 16:52:26 +01:00
parent 3e788084b2
commit 5089de890f
5 changed files with 24 additions and 19 deletions

View File

@ -350,7 +350,7 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
// to avoid some communication overhead // to avoid some communication overhead
DisplaySolutionPtr result; DisplaySolutionPtr result;
if (get_solution_client_) { if (!(flags_ & IS_DESTROYED) && get_solution_client_) {
// request solution via service // request solution via service
moveit_task_constructor_msgs::GetSolution srv; moveit_task_constructor_msgs::GetSolution srv;
srv.request.solution_id = id; srv.request.solution_id = id;

View File

@ -58,6 +58,8 @@ namespace moveit_rviz_plugin
TaskDisplay::TaskDisplay() : Display() TaskDisplay::TaskDisplay() : Display()
{ {
task_list_model_.reset(new TaskListModel); task_list_model_.reset(new TaskListModel);
task_list_model_->setSolutionClient(&get_solution_client);
MetaTaskListModel::instance().insertModel(task_list_model_.get(), this); MetaTaskListModel::instance().insertModel(task_list_model_.get(), this);
connect(task_list_model_.get(), SIGNAL(rowsInserted(QModelIndex,int,int)), connect(task_list_model_.get(), SIGNAL(rowsInserted(QModelIndex,int,int)),
@ -165,6 +167,7 @@ void TaskDisplay::taskDescriptionCB(const ros::MessageEvent<const moveit_task_co
const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg = event.getMessage(); const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg = event.getMessage();
const std::string id = event.getPublisherName() + "/" + msg->id; const std::string id = event.getPublisherName() + "/" + msg->id;
mainloop_jobs_.addJob([this, id, msg]() { mainloop_jobs_.addJob([this, id, msg]() {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
task_list_model_->processTaskDescriptionMessage(id, *msg); task_list_model_->processTaskDescriptionMessage(id, *msg);
}); });
} }
@ -174,6 +177,7 @@ void TaskDisplay::taskStatisticsCB(const ros::MessageEvent<const moveit_task_con
const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg = event.getMessage(); const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg = event.getMessage();
const std::string id = event.getPublisherName() + "/" + msg->id; const std::string id = event.getPublisherName() + "/" + msg->id;
mainloop_jobs_.addJob([this, id, msg]() { mainloop_jobs_.addJob([this, id, msg]() {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
task_list_model_->processTaskStatisticsMessage(id, *msg); task_list_model_->processTaskStatisticsMessage(id, *msg);
}); });
} }
@ -183,6 +187,7 @@ 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]() {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg); const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
if (s) trajectory_visual_->showTrajectory(s, false); if (s) trajectory_visual_->showTrajectory(s, false);
return; return;
@ -210,14 +215,14 @@ void TaskDisplay::changedTaskSolutionTopic()
tasks_property_->removeChildren(); tasks_property_->removeChildren();
// generate task monitoring topics from solution topic // generate task monitoring topics from solution topic
std::string solution_topic = task_solution_topic_property_->getStdString(); const QString& solution_topic = task_solution_topic_property_->getString();
auto last_sep = solution_topic.find_last_of('/'); if (!solution_topic.endsWith(QString("/").append(SOLUTION_TOPIC))) {
if (last_sep == std::string::npos) { setStatus(rviz::StatusProperty::Error, "Task Monitor",
setStatus(rviz::StatusProperty::Error, "Task Monitor", "invalid topic"); QString("Invalid topic. Expecting a name ending on \"/%1\"").arg(SOLUTION_TOPIC));
return; 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 // 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);
@ -226,13 +231,18 @@ void TaskDisplay::changedTaskSolutionTopic()
task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this); task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
// listen to task solutions // 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 // service to request solutions
get_solution_client = update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(base_ns + GET_SOLUTION_SERVICE); get_solution_client = update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(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) void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last)

View File

@ -85,6 +85,7 @@ public:
virtual void onEnable(); virtual void onEnable();
virtual void onDisable(); virtual void onDisable();
void setName(const QString& name); void setName(const QString& name);
void setSolutionStatus(bool ok);
TaskListModel& getTaskListModel() { return *task_list_model_; } TaskListModel& getTaskListModel() { return *task_list_model_; }
TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); } TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); }

View File

@ -156,15 +156,6 @@ void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr &scene)
void TaskListModel::setSolutionClient(ros::ServiceClient *client) 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<RemoteTaskModel*>(getModel(index(row, 0)).first);
if (!task) continue;
task->setSolutionClient(nullptr);
}
}
get_solution_client_ = client; get_solution_client_ = client;
} }

View File

@ -231,8 +231,10 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex &current, const QMode
d_ptr->locked_display_ = display; d_ptr->locked_display_ = display;
TaskSolutionVisualization* vis = display->visualization(); TaskSolutionVisualization* vis = display->visualization();
const DisplaySolutionPtr& solution = task->getSolution(current);
display->setSolutionStatus(bool(solution));
vis->interruptCurrentDisplay(); vis->interruptCurrentDisplay();
vis->showTrajectory(task->getSolution(current), true); vis->showTrajectory(solution, true);
} }
void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected) void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected)
@ -248,6 +250,7 @@ void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const
display->clearMarkers(); display->clearMarkers();
for (const auto& index : selected_rows) { for (const auto& index : selected_rows) {
const DisplaySolutionPtr &solution = task->getSolution(index); const DisplaySolutionPtr &solution = task->getSolution(index);
display->setSolutionStatus(bool(solution));
display->showMarkers(solution); display->showMarkers(solution);
} }
} }