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
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;

View File

@ -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::MessageEvent<const moveit_task_co
const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg = event.getMessage();
const std::string id = event.getPublisherName() + "/" + msg->id;
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::MessageEvent<const moveit_task_con
const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg = event.getMessage();
const std::string id = event.getPublisherName() + "/" + msg->id;
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::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]() {
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<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)

View File

@ -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(); }

View File

@ -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<RemoteTaskModel*>(getModel(index(row, 0)).first);
if (!task) continue;
task->setSolutionClient(nullptr);
}
}
get_solution_client_ = client;
}

View File

@ -231,8 +231,10 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex &current, 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);
}
}