removed statistics from task description

- make TaskStatistics a latched topic too
- subscribing to topics in order (1. description, 2. statistics, 3. solution)
  should ensure that we receive those latched messages in this order
This commit is contained in:
Robert Haschke 2017-11-12 21:21:40 +01:00
parent 12095405f4
commit d0e3783d4e
6 changed files with 17 additions and 28 deletions

View File

@ -20,7 +20,7 @@ public:
{
resetMaps();
task_description_publisher_ = nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1);
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1, true);
solution_publisher_ = nh_.advertise<moveit_task_constructor_msgs::Solution>(SOLUTION_TOPIC, 1, true);
}
void resetMaps () {
@ -168,9 +168,7 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio
[this, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent
moveit_task_constructor_msgs::StageDescription desc;
moveit_task_constructor_msgs::StageStatistics stat;
desc.id = stat.id = stageId(&stage);
desc.id = stageId(&stage);
desc.name = stage.name();
desc.flags = stage.pimpl()->interfaceFlags();
// TODO fill stage properties
@ -179,16 +177,12 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio
assert (it != impl->stage_to_id_map_.cend());
desc.parent_id = it->second;
fillStageStatistics(stage, stat);
// finally store in msg
msg.description.push_back(std::move(desc));
msg.statistics.push_back(std::move(stat));
msg.stages.push_back(std::move(desc));
return true;
};
msg.description.clear();
msg.statistics.clear();
msg.stages.clear();
impl->task_.stages()->traverseRecursively(stageProcessor);
msg.id = impl->task_.id();

View File

@ -2,5 +2,4 @@
string id
# list of all stages, including the task stage itself
StageDescription[] description
StageStatistics[] statistics
StageDescription[] stages

View File

@ -198,7 +198,7 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i
return true;
}
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_description_type &msg)
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg)
{
// iterate over descriptions and create new / update existing nodes where needed
for (const auto &s : msg) {
@ -251,7 +251,7 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg
}
}
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg)
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg)
{
// iterate over statistics and update node's solutions where needed
for (const auto &s : msg) {

View File

@ -68,8 +68,8 @@ public:
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override;
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_description_type &msg);
void processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg);
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg);
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg);
};

View File

@ -306,11 +306,11 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
bool created = it_inserted.second;
RemoteTaskModel*& remote_task = it_inserted.first->second;
if (!msg.description.empty() && remote_task && remote_task->taskFlags() & BaseTaskModel::IS_DESTROYED)
if (!msg.stages.empty() && remote_task && remote_task->taskFlags() & BaseTaskModel::IS_DESTROYED)
created = true; // re-create remote task after it was destroyed beforehand
// empty list indicates, that this remote task is not available anymore
if (msg.description.empty()) {
if (msg.stages.empty()) {
if (!remote_task) { // task was already deleted locally
// we can now remove it from remote_tasks_
d->remote_tasks_.erase(it_inserted.first);
@ -323,8 +323,7 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
if (!remote_task)
return; // task is not in use anymore
remote_task->processStageDescriptions(msg.description);
remote_task->processStageStatistics(msg.statistics);
remote_task->processStageDescriptions(msg.stages);
// insert newly created model into this' model instance
if (created) {

View File

@ -60,19 +60,16 @@ protected:
uint id = 0, root_id;
moveit_task_constructor_msgs::StageDescription desc;
moveit_task_constructor_msgs::StageStatistics stat;
desc.parent_id = id;
desc.id = stat.id = root_id = ++id;
desc.id = root_id = ++id;
desc.name = name;
t.description.push_back(desc);
t.statistics.push_back(stat);
t.stages.push_back(desc);
for (int i = 0; i != children; ++i) {
desc.parent_id = root_id;
desc.id = stat.id = ++id;
desc.id = ++id;
desc.name = std::to_string(i);
t.description.push_back(desc);
t.statistics.push_back(stat);
t.stages.push_back(desc);
}
return t;
}
@ -160,7 +157,7 @@ protected:
TEST_F(TaskListModelTest, remoteTaskModel) {
children = 3;
moveit_rviz_plugin::RemoteTaskModel m;
m.processStageDescriptions(genMsg("first").description);
m.processStageDescriptions(genMsg("first").stages);
SCOPED_TRACE("first");
validate(m, {"first"});
}