From d031f1b1e357f864fa7bd8914e694cf9ca236034 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 22 Jul 2020 11:08:12 +0200 Subject: [PATCH] send a single reset message when introspection is enabled My previous patches accidentally disabled *all* (3) reset messages, instead of keeping exactly one. This patch sends exactly one empty description (reset) message every time an introspection instance is constructed for a task. Notice the additional increase of the description queue_size from 1 to 2 to avoid directly dropping the reset message in favor of the new description likely to be send shortly afterwards. --- core/src/introspection.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index bc38b93c..76924ea4 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -66,13 +66,26 @@ public: : nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace , task_(task) , process_id_(getProcessId()) { - resetMaps(); task_description_publisher_ = - nh_.advertise(DESCRIPTION_TOPIC, 1, true); + nh_.advertise(DESCRIPTION_TOPIC, 2, true); + // send reset message as early as possible to give subscribers time to see it + indicateReset(); + task_statistics_publisher_ = nh_.advertise(STATISTICS_TOPIC, 1, true); solution_publisher_ = nh_.advertise(SOLUTION_TOPIC, 1, true); + + resetMaps(); } + + void indicateReset() { + // send empty task description message to indicate reset + ::moveit_task_constructor_msgs::TaskDescription msg; + msg.process_id = process_id_; + msg.id = task_->id(); + task_description_publisher_.publish(msg); + } + void resetMaps() { // reset maps stage_to_id_map_.clear(); @@ -118,12 +131,7 @@ void Introspection::publishTaskState() { } void Introspection::reset() { - // send empty task description message to indicate reset - ::moveit_task_constructor_msgs::TaskDescription msg; - msg.process_id = impl->process_id_; - msg.id = impl->task_->id(); - impl->task_description_publisher_.publish(msg); - + impl->indicateReset(); impl->resetMaps(); }