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());
|
auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent());
|
||||||
assert (it != impl->stage_to_id_map_.cend());
|
assert (it != impl->stage_to_id_map_.cend());
|
||||||
desc.parent_id = stat.parent_id = it->second;
|
desc.parent_id = it->second;
|
||||||
|
|
||||||
fillStageStatistics(stage, stat);
|
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
|
// this method is called for each child stage of a given parent
|
||||||
moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg
|
moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg
|
||||||
stat.id = stageId(&stage);
|
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);
|
fillStageStatistics(stage, stat);
|
||||||
|
|
||||||
// finally store in msg.stages
|
// finally store in msg.stages
|
||||||
|
|||||||
@ -3,9 +3,6 @@
|
|||||||
# unique id within task
|
# unique id within task
|
||||||
uint32 id
|
uint32 id
|
||||||
|
|
||||||
# parent id, parent_id == id means root
|
|
||||||
uint32 parent_id
|
|
||||||
|
|
||||||
# successful and failed solution IDs of this stage
|
# successful and failed solution IDs of this stage
|
||||||
uint32[] solved
|
uint32[] solved
|
||||||
uint32[] failed
|
uint32[] failed
|
||||||
|
|||||||
@ -4,4 +4,3 @@ string id
|
|||||||
# list of all stages, including the task stage itself
|
# list of all stages, including the task stage itself
|
||||||
StageDescription[] description
|
StageDescription[] description
|
||||||
StageStatistics[] statistics
|
StageStatistics[] statistics
|
||||||
|
|
||||||
|
|||||||
@ -60,8 +60,13 @@ struct RemoteTaskModel::Node {
|
|||||||
QString name_;
|
QString name_;
|
||||||
InterfaceFlags interface_flags_;
|
InterfaceFlags interface_flags_;
|
||||||
NodeFlags node_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) {
|
bool setName(const QString& name) {
|
||||||
if (name == name_) return false;
|
if (name == name_) return false;
|
||||||
@ -170,8 +175,8 @@ QVariant RemoteTaskModel::data(const QModelIndex &index, int role) const
|
|||||||
case Qt::DisplayRole:
|
case Qt::DisplayRole:
|
||||||
switch (index.column()) {
|
switch (index.column()) {
|
||||||
case 0: return n->name_;
|
case 0: return n->name_;
|
||||||
case 1: return 0;
|
case 1: return n->solved_->rowCount();
|
||||||
case 2: return 0;
|
case 2: return n->failed_->rowCount();
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case Qt::ForegroundRole:
|
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)
|
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
|
#pragma once
|
||||||
|
|
||||||
#include "task_list_model.h"
|
#include "task_list_model.h"
|
||||||
|
#include <moveit/visualization_tools/display_solution.h>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
|
/** Model representing a remote task
|
||||||
|
*
|
||||||
|
* filled via TaskDescription + TaskStatistics messages
|
||||||
|
*/
|
||||||
class RemoteTaskModel : public BaseTaskModel {
|
class RemoteTaskModel : public BaseTaskModel {
|
||||||
Q_OBJECT
|
Q_OBJECT
|
||||||
class Node;
|
class Node;
|
||||||
Node* const root_;
|
Node* const root_;
|
||||||
std::map<uint32_t, Node*> id_to_stage_;
|
std::map<uint32_t, Node*> id_to_stage_;
|
||||||
|
std::map<uint32_t, DisplaySolutionPtr> id_to_solution_;
|
||||||
|
|
||||||
inline Node* node(const QModelIndex &index) const;
|
inline Node* node(const QModelIndex &index) const;
|
||||||
QModelIndex index(const Node* n) const;
|
QModelIndex index(const Node* n) const;
|
||||||
@ -66,4 +72,18 @@ public:
|
|||||||
void processStageStatistics(const moveit_task_constructor_msgs::TaskDescription::_statistics_type &msg);
|
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::StageDescription desc;
|
||||||
moveit_task_constructor_msgs::StageStatistics stat;
|
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.id = stat.id = root_id = ++id;
|
||||||
desc.name = name;
|
desc.name = name;
|
||||||
t.description.push_back(desc);
|
t.description.push_back(desc);
|
||||||
t.statistics.push_back(stat);
|
t.statistics.push_back(stat);
|
||||||
|
|
||||||
for (int i = 0; i != children; ++i) {
|
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.id = stat.id = ++id;
|
||||||
desc.name = std::to_string(i);
|
desc.name = std::to_string(i);
|
||||||
t.description.push_back(desc);
|
t.description.push_back(desc);
|
||||||
|
|||||||
@ -15,6 +15,8 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
|||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
|
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||||
|
|
||||||
/** Class representing a task solution for display */
|
/** Class representing a task solution for display */
|
||||||
class DisplaySolution
|
class DisplaySolution
|
||||||
{
|
{
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user