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.
This commit is contained in:
v4hn 2020-07-22 11:08:12 +02:00 committed by Michael Görner
parent e8324abb78
commit d031f1b1e3

View File

@ -66,13 +66,26 @@ public:
: nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace : nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace
, task_(task) , task_(task)
, process_id_(getProcessId()) { , process_id_(getProcessId()) {
resetMaps();
task_description_publisher_ = task_description_publisher_ =
nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 1, true); nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 2, true);
// send reset message as early as possible to give subscribers time to see it
indicateReset();
task_statistics_publisher_ = task_statistics_publisher_ =
nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1, true); nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1, true);
solution_publisher_ = nh_.advertise<moveit_task_constructor_msgs::Solution>(SOLUTION_TOPIC, 1, true); solution_publisher_ = nh_.advertise<moveit_task_constructor_msgs::Solution>(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() { void resetMaps() {
// reset maps // reset maps
stage_to_id_map_.clear(); stage_to_id_map_.clear();
@ -118,12 +131,7 @@ void Introspection::publishTaskState() {
} }
void Introspection::reset() { void Introspection::reset() {
// send empty task description message to indicate reset impl->indicateReset();
::moveit_task_constructor_msgs::TaskDescription msg;
msg.process_id = impl->process_id_;
msg.id = impl->task_->id();
impl->task_description_publisher_.publish(msg);
impl->resetMaps(); impl->resetMaps();
} }