connect to statistics *after* first description

This avoids the case where we would asynchronously receive the statistics
*before* the description and wouldn't know what to do with it.
This commit is contained in:
v4hn 2020-06-10 23:09:18 +02:00
parent 701589b651
commit 7270a658af
2 changed files with 20 additions and 10 deletions

View File

@ -59,7 +59,7 @@
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
TaskDisplay::TaskDisplay() : Display() { TaskDisplay::TaskDisplay() : Display(), received_task_description_(false) {
task_list_model_.reset(new TaskListModel); task_list_model_.reset(new TaskListModel);
task_list_model_->setSolutionClient(&get_solution_client); task_list_model_->setSolutionClient(&get_solution_client);
@ -197,6 +197,15 @@ void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDesc
mainloop_jobs_.addJob([this, id, msg]() { mainloop_jobs_.addJob([this, id, msg]() {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
task_list_model_->processTaskDescriptionMessage(id, *msg); task_list_model_->processTaskDescriptionMessage(id, *msg);
// Start listening to other topics if this is the first description
// Waiting for the description ensures we do not receive data that cannot be interpreted yet
if (!received_task_description_) {
received_task_description_ = true;
task_statistics_sub =
update_nh_.subscribe(base_ns_ + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
task_solution_sub = update_nh_.subscribe(base_ns_ + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this);
}
}); });
} }
@ -229,6 +238,8 @@ void TaskDisplay::changedTaskSolutionTopic() {
task_solution_sub.shutdown(); task_solution_sub.shutdown();
get_solution_client.shutdown(); get_solution_client.shutdown();
received_task_description_ = false;
// generate task monitoring topics from solution topic // generate task monitoring topics from solution topic
const QString& solution_topic = task_solution_topic_property_->getString(); const QString& solution_topic = task_solution_topic_property_->getString();
if (!solution_topic.endsWith(QString("/").append(SOLUTION_TOPIC))) { if (!solution_topic.endsWith(QString("/").append(SOLUTION_TOPIC))) {
@ -237,20 +248,14 @@ void TaskDisplay::changedTaskSolutionTopic() {
return; return;
} }
std::string base_ns = solution_topic.toStdString().substr(0, solution_topic.length() - strlen(SOLUTION_TOPIC)); 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);
// listen to task statistics updates
task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
// listen to task solutions
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 = get_solution_client =
update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(base_ns + GET_SOLUTION_SERVICE); update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(base_ns_ + GET_SOLUTION_SERVICE);
setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received"); setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received");
} }

View File

@ -133,6 +133,11 @@ protected:
rdf_loader::RDFLoaderPtr rdf_loader_; rdf_loader::RDFLoaderPtr rdf_loader_;
moveit::core::RobotModelConstPtr robot_model_; moveit::core::RobotModelConstPtr robot_model_;
// topic namespace for ROS interfaces of task
std::string base_ns_;
// Indicates whether description was received for current task
bool received_task_description_;
// Properties // Properties
rviz::StringProperty* robot_description_property_; rviz::StringProperty* robot_description_property_;
rviz::RosTopicProperty* task_solution_topic_property_; rviz::RosTopicProperty* task_solution_topic_property_;