mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
receive task solutions
- remove parent_id from StageStatistics message
This commit is contained in:
parent
df074b48a5
commit
67b5a99d8d
@ -177,7 +177,7 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio
|
||||
|
||||
auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent());
|
||||
assert (it != impl->stage_to_id_map_.cend());
|
||||
desc.parent_id = stat.parent_id = it->second;
|
||||
desc.parent_id = it->second;
|
||||
|
||||
fillStageStatistics(stage, stat);
|
||||
|
||||
@ -202,11 +202,6 @@ moveit_task_constructor_msgs::TaskStatistics& Introspection::fillTaskStatistics(
|
||||
// this method is called for each child stage of a given parent
|
||||
moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg
|
||||
stat.id = stageId(&stage);
|
||||
|
||||
auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent());
|
||||
assert (it != impl->stage_to_id_map_.cend());
|
||||
stat.parent_id = it->second;
|
||||
|
||||
fillStageStatistics(stage, stat);
|
||||
|
||||
// finally store in msg.stages
|
||||
|
||||
@ -3,9 +3,6 @@
|
||||
# unique id within task
|
||||
uint32 id
|
||||
|
||||
# parent id, parent_id == id means root
|
||||
uint32 parent_id
|
||||
|
||||
# successful and failed solution IDs of this stage
|
||||
uint32[] solved
|
||||
uint32[] failed
|
||||
|
||||
@ -4,4 +4,3 @@ string id
|
||||
# list of all stages, including the task stage itself
|
||||
StageDescription[] description
|
||||
StageStatistics[] statistics
|
||||
|
||||
|
||||
@ -60,8 +60,13 @@ struct RemoteTaskModel::Node {
|
||||
QString name_;
|
||||
InterfaceFlags interface_flags_;
|
||||
NodeFlags node_flags_;
|
||||
std::unique_ptr<RemoteSolutionModel> solved_;
|
||||
std::unique_ptr<RemoteSolutionModel> failed_;
|
||||
|
||||
inline Node(Node *parent) : parent_(parent) {}
|
||||
inline Node(Node *parent) : parent_(parent) {
|
||||
solved_.reset(new RemoteSolutionModel());
|
||||
failed_.reset(new RemoteSolutionModel());
|
||||
}
|
||||
|
||||
bool setName(const QString& name) {
|
||||
if (name == name_) return false;
|
||||
@ -170,8 +175,8 @@ QVariant RemoteTaskModel::data(const QModelIndex &index, int role) const
|
||||
case Qt::DisplayRole:
|
||||
switch (index.column()) {
|
||||
case 0: return n->name_;
|
||||
case 1: return 0;
|
||||
case 2: return 0;
|
||||
case 1: return n->solved_->rowCount();
|
||||
case 2: return n->failed_->rowCount();
|
||||
}
|
||||
break;
|
||||
case Qt::ForegroundRole:
|
||||
@ -248,6 +253,60 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg
|
||||
|
||||
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg)
|
||||
{
|
||||
// iterate over statistics and update node's solutions where needed
|
||||
for (const auto &s : msg) {
|
||||
// find node for stage s, this should always exist
|
||||
auto it = id_to_stage_.find(s.id);
|
||||
if (it == id_to_stage_.end()) {
|
||||
ROS_ERROR_NAMED("TaskListModel", "No stage %d", s.id);
|
||||
continue;
|
||||
}
|
||||
Node *n = it->second;
|
||||
|
||||
bool changed = n->solved_->processSolutionIDs(s.solved) ||
|
||||
n->failed_->processSolutionIDs(s.failed);
|
||||
// emit notify about model changes when node was already visited
|
||||
if (changed && (n->node_flags_ & WAS_VISITED)) {
|
||||
QModelIndex idx = index(n);
|
||||
dataChanged(idx.sibling(idx.row(), 1), idx.sibling(idx.row(), 2));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
RemoteSolutionModel::RemoteSolutionModel(QObject *parent)
|
||||
: QAbstractListModel(parent)
|
||||
{
|
||||
}
|
||||
|
||||
int RemoteSolutionModel::rowCount(const QModelIndex &parent) const
|
||||
{
|
||||
return ids_.size();
|
||||
}
|
||||
|
||||
QVariant RemoteSolutionModel::data(const QModelIndex &index, int role) const
|
||||
{
|
||||
Q_ASSERT(index.isValid());
|
||||
Q_ASSERT(!index.parent().isValid());
|
||||
|
||||
if (role == Qt::DisplayRole)
|
||||
return index.row();
|
||||
return QVariant();
|
||||
}
|
||||
|
||||
bool RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &ids)
|
||||
{
|
||||
if (ids_ == ids)
|
||||
return false;
|
||||
|
||||
bool size_changed = (ids_.size() != ids.size());
|
||||
QAbstractItemModel::LayoutChangeHint hint = size_changed ? QAbstractItemModel::NoLayoutChangeHint
|
||||
: QAbstractItemModel::VerticalSortHint;
|
||||
layoutAboutToBeChanged({QModelIndex()}, hint);
|
||||
ids_ = ids;
|
||||
layoutChanged({QModelIndex()}, hint);
|
||||
|
||||
return size_changed;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -37,15 +37,21 @@
|
||||
#pragma once
|
||||
|
||||
#include "task_list_model.h"
|
||||
#include <moveit/visualization_tools/display_solution.h>
|
||||
#include <memory>
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
/** Model representing a remote task
|
||||
*
|
||||
* filled via TaskDescription + TaskStatistics messages
|
||||
*/
|
||||
class RemoteTaskModel : public BaseTaskModel {
|
||||
Q_OBJECT
|
||||
class Node;
|
||||
Node* const root_;
|
||||
std::map<uint32_t, Node*> id_to_stage_;
|
||||
std::map<uint32_t, DisplaySolutionPtr> id_to_solution_;
|
||||
|
||||
inline Node* node(const QModelIndex &index) const;
|
||||
QModelIndex index(const Node* n) const;
|
||||
@ -66,4 +72,18 @@ public:
|
||||
void processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg);
|
||||
};
|
||||
|
||||
|
||||
/** Model representing solutions of a remote task */
|
||||
class RemoteSolutionModel : public QAbstractListModel {
|
||||
Q_OBJECT
|
||||
std::vector<uint32_t> ids_;
|
||||
|
||||
public:
|
||||
RemoteSolutionModel(QObject *parent = nullptr);
|
||||
int rowCount(const QModelIndex &parent = QModelIndex()) const override;
|
||||
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
|
||||
|
||||
bool processSolutionIDs(const std::vector<uint32_t>& ids);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -61,14 +61,14 @@ protected:
|
||||
|
||||
moveit_task_constructor_msgs::StageDescription desc;
|
||||
moveit_task_constructor_msgs::StageStatistics stat;
|
||||
desc.parent_id = stat.parent_id = id;
|
||||
desc.parent_id = id;
|
||||
desc.id = stat.id = root_id = ++id;
|
||||
desc.name = name;
|
||||
t.description.push_back(desc);
|
||||
t.statistics.push_back(stat);
|
||||
|
||||
for (int i = 0; i != children; ++i) {
|
||||
desc.parent_id = stat.parent_id = root_id;
|
||||
desc.parent_id = root_id;
|
||||
desc.id = stat.id = ++id;
|
||||
desc.name = std::to_string(i);
|
||||
t.description.push_back(desc);
|
||||
|
||||
@ -15,6 +15,8 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||
|
||||
/** Class representing a task solution for display */
|
||||
class DisplaySolution
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user