mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
701589b651
commit
7270a658af
@ -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");
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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_;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user