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