renamed Task, Stage msgs

- Stage -> StageStatistics + StageDescription
- Task -> TaskStatistics + TaskDescription
- removed GetInterfaceState.srv
This commit is contained in:
Robert Haschke 2017-11-08 17:41:04 +01:00
parent 18119a7985
commit a43692fc25
24 changed files with 268 additions and 195 deletions

View File

@ -18,15 +18,16 @@ find_package(catkin REQUIRED COMPONENTS
# ROS messages, services and actions # ROS messages, services and actions
add_message_files(DIRECTORY msg add_message_files(DIRECTORY msg
FILES FILES
Stage.msg StageDescription.msg
Task.msg StageStatistics.msg
TaskDescription.msg
TaskStatistics.msg
Solution.msg
SubSolution.msg SubSolution.msg
SubTrajectory.msg SubTrajectory.msg
Solution.msg
) )
add_service_files(DIRECTORY srv add_service_files(DIRECTORY srv
FILES FILES
GetInterfaceState.srv
GetSolution.srv GetSolution.srv
) )
generate_messages(DEPENDENCIES ${MSG_DEPS}) generate_messages(DEPENDENCIES ${MSG_DEPS})

View File

@ -3,40 +3,47 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <ros/publisher.h> #include <ros/publisher.h>
#include <ros/service.h> #include <ros/service.h>
#include <moveit_task_constructor/Task.h> #include <moveit_task_constructor/TaskDescription.h>
#include <moveit_task_constructor/TaskStatistics.h>
#include <moveit_task_constructor/Solution.h> #include <moveit_task_constructor/Solution.h>
#include <moveit_task_constructor/GetInterfaceState.h>
#include <moveit_task_constructor/GetSolution.h> #include <moveit_task_constructor/GetSolution.h>
#define DESCRIPTION_TOPIC "description"
#define STATISTICS_TOPIC "statistics"
#define SOLUTION_TOPIC "solution"
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(Task) MOVEIT_CLASS_FORWARD(Task)
MOVEIT_CLASS_FORWARD(SolutionBase) MOVEIT_CLASS_FORWARD(SolutionBase)
#define DEFAULT_TASK_MONITOR_TOPIC "task_monitoring"
#define DEFAULT_TASK_SOLUTION_TOPIC "task_solutions"
class Introspection { class Introspection {
// publish Task state and (new) Solutions // publish task detailed description and current state
ros::Publisher task_publisher_; ros::Publisher task_description_publisher_;
ros::Publisher task_statistics_publisher_;
// publish new solutions
ros::Publisher solution_publisher_; ros::Publisher solution_publisher_;
// services to provide InterfaceState and Solution // services to provide an individual Solution
ros::ServiceServer get_interface_state_service_;
ros::ServiceServer get_solution_service_; ros::ServiceServer get_solution_service_;
public: public:
Introspection(const std::string &task_topic = DEFAULT_TASK_MONITOR_TOPIC, Introspection();
const std::string &solution_topic = DEFAULT_TASK_SOLUTION_TOPIC);
Introspection(const Introspection &other) = delete; Introspection(const Introspection &other) = delete;
static Introspection &instance(); static Introspection &instance();
// publish the current state of given task // publish detailed task description
void publishTask(const Task &t); void publishTaskDescription(const Task &t);
void publishTask(const ::moveit_task_constructor::Task &msg) { void publishTaskDescription(const ::moveit_task_constructor::TaskDescription &msg) {
task_publisher_.publish(msg); task_description_publisher_.publish(msg);
} }
void operator()(const Task &t) { publishTask(t); } // publish the current state of given task
void publishTaskState(const Task &t);
void publishTaskState(const ::moveit_task_constructor::TaskStatistics &msg) {
task_statistics_publisher_.publish(msg);
}
void operator()(const Task &t) { publishTaskState(t); }
// publish the given solution // publish the given solution
void publishSolution(const SolutionBase &s); void publishSolution(const SolutionBase &s);
@ -45,9 +52,6 @@ public:
} }
void operator()(const SolutionBase &s) { publishSolution(s); } void operator()(const SolutionBase &s) { publishSolution(s); }
// get interface state
bool getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req,
moveit_task_constructor::GetInterfaceState::Response &res);
// get solution // get solution
bool getSolution(moveit_task_constructor::GetSolution::Request &req, bool getSolution(moveit_task_constructor::GetSolution::Request &req,
moveit_task_constructor::GetSolution::Response &res); moveit_task_constructor::GetSolution::Response &res);

View File

@ -6,7 +6,8 @@
#include "container.h" #include "container.h"
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_task_constructor/Task.h> #include <moveit_task_constructor/TaskDescription.h>
#include <moveit_task_constructor/TaskStatistics.h>
#include <moveit_task_constructor/Solution.h> #include <moveit_task_constructor/Solution.h>
namespace robot_model_loader { namespace robot_model_loader {
@ -57,8 +58,10 @@ public:
bool plan(); bool plan();
/// print current state std::cout /// print current state std::cout
static void printState(const Task &t); static void printState(const Task &t);
/// fill task message for publishing the current state /// fill task description message for publishing the task configuration
moveit_task_constructor::Task& fillMessage(moveit_task_constructor::Task& msg) const; moveit_task_constructor::TaskDescription& fillTaskDescription(moveit_task_constructor::TaskDescription& msg) const;
/// fill task state message for publishing the current task state
moveit_task_constructor::TaskStatistics& fillTaskStatistics(moveit_task_constructor::TaskStatistics& msg) const;
size_t numSolutions() const override; size_t numSolutions() const override;

View File

@ -1,8 +1,8 @@
# id of associated task # id of associated task
string task_id string task_id
# id of robot model # planning scene of start state
string model_id moveit_msgs/PlanningScene start_scene
# set of all sub solutions involved # set of all sub solutions involved
SubSolution[] sub_solution SubSolution[] sub_solution

17
msg/StageDescription.msg Normal file
View File

@ -0,0 +1,17 @@
# static description of a stage
# unique id within task
uint32 id
# parent id, parent_id == id means root
uint32 parent_id
# name of this stage
string name
# flags: interface, ...
uint32 flags
# properties
string property_name
string property_value

View File

@ -1,12 +1,8 @@
# dynamically changing information for a stage
# unique id within task # unique id within task
uint32 id uint32 id
# name of this stage
string name
# flags: interface, ...
uint32 flags
# parent id, parent_id == id means root # parent id, parent_id == id means root
uint32 parent_id uint32 parent_id

View File

@ -4,9 +4,5 @@ uint32 id
# associated cost # associated cost
float32 cost float32 cost
# IDs of start and end state?
#uint32 start_id
#uint32 end_id
# IDs of subsolutions # IDs of subsolutions
uint32[] sub_solution_id uint32[] sub_solution_id

View File

@ -1,22 +1,18 @@
# unique id within task # unique id within task
uint32 id uint32 id
# associated names
string name
# associated cost # associated cost
float32 cost float32 cost
# name of task stage # optional associated name of sub trajectory
string stage string name
# IDs of start and end state?
# Or have an array of them in Solution.msg?
#uint32 start_id
#uint32 end_id
# trajectory # trajectory
moveit_msgs/RobotTrajectory trajectory moveit_msgs/RobotTrajectory trajectory
# markers, e.g. providing additional hints or illustrating failure
visualization_msgs/MarkerArray markers visualization_msgs/MarkerArray markers
# TODO: add PlanningScene diff w.r.t. previous end state? # ID of end state
uint32 end_id
# planning scene of end state as diff w.r.t. start state
moveit_msgs/PlanningScene scene_diff

7
msg/TaskDescription.msg Normal file
View File

@ -0,0 +1,7 @@
# unique ID of this task
string id
# list of all stages, including the task stage itself
StageDescription[] description
StageStatistics[] statistics

View File

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

View File

@ -7,15 +7,14 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
Introspection::Introspection(const std::string &task_topic, Introspection::Introspection()
const std::string &solution_topic)
{ {
ros::NodeHandle n; ros::NodeHandle n;
task_publisher_ = n.advertise<moveit_task_constructor::Task>(task_topic, 1); task_description_publisher_ = n.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1);
solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(solution_topic, 1, true); task_statistics_publisher_ = n.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
n = ros::NodeHandle("~"); // services are advertised in private namespace n = ros::NodeHandle("~"); // services are advertised in private namespace
get_interface_state_service_ = n.advertiseService("get_interface_state", &Introspection::getInterfaceState, this);
get_solution_service_ = n.advertiseService("get_solution", &Introspection::getSolution, this); get_solution_service_ = n.advertiseService("get_solution", &Introspection::getSolution, this);
} }
@ -25,10 +24,16 @@ Introspection &Introspection::instance()
return instance_; return instance_;
} }
void Introspection::publishTask(const Task &t) void Introspection::publishTaskDescription(const Task &t)
{ {
::moveit_task_constructor::Task msg; ::moveit_task_constructor::TaskDescription msg;
task_publisher_.publish(t.fillMessage(msg)); task_description_publisher_.publish(t.fillTaskDescription(msg));
}
void Introspection::publishTaskState(const Task &t)
{
::moveit_task_constructor::TaskStatistics msg;
task_statistics_publisher_.publish(t.fillTaskStatistics(msg));
} }
void Introspection::publishSolution(const SolutionBase &s) void Introspection::publishSolution(const SolutionBase &s)
@ -38,16 +43,6 @@ void Introspection::publishSolution(const SolutionBase &s)
publishSolution(msg); publishSolution(msg);
} }
bool Introspection::getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req,
moveit_task_constructor::GetInterfaceState::Response &res)
{
const InterfaceState& state = Repository<InterfaceState>::instance()[req.state_id];
moveit_msgs::PlanningScene msg;
state.scene()->getPlanningSceneDiffMsg(msg);
res.scene = msg;
return true;
}
bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req, bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req,
moveit_task_constructor::GetSolution::Response &res) moveit_task_constructor::GetSolution::Response &res)
{ {

View File

@ -68,9 +68,8 @@ void SubTrajectory::fillMessage(moveit_task_constructor::Solution &msg) const {
msg.sub_trajectory.emplace_back(); msg.sub_trajectory.emplace_back();
moveit_task_constructor::SubTrajectory& t = msg.sub_trajectory.back(); moveit_task_constructor::SubTrajectory& t = msg.sub_trajectory.back();
t.id = this->id(); t.id = this->id();
t.name = this->name();
t.cost = this->cost(); t.cost = this->cost();
t.stage = this->creator()->name(); t.name = this->name();
if (trajectory()) if (trajectory())
trajectory()->getRobotTrajectoryMsg(t.trajectory); trajectory()->getRobotTrajectoryMsg(t.trajectory);
t.markers = this->markers(); t.markers = this->markers();

View File

@ -216,7 +216,7 @@ void Task::printState(const Task &t){
t.stages()->traverseRecursively(processor); t.stages()->traverseRecursively(processor);
} }
void fillStateList(moveit_task_constructor::Stage::_received_starts_type &c, void fillInterfaceList(moveit_task_constructor::StageStatistics::_received_starts_type &c,
const InterfaceConstPtr& interface) { const InterfaceConstPtr& interface) {
c.clear(); c.clear();
if (!interface) return; if (!interface) return;
@ -224,50 +224,94 @@ void fillStateList(moveit_task_constructor::Stage::_received_starts_type &c,
c.push_back(state.id()); c.push_back(state.id());
} }
moveit_task_constructor::Task& Task::fillMessage(moveit_task_constructor::Task &msg) const void fillStageStatistics(const Stage& stage, moveit_task_constructor::StageStatistics& s)
{ {
std::map<const Stage*, moveit_task_constructor::Stage::_id_type> stage_to_id_map; const StagePrivate *simpl = stage.pimpl();
fillInterfaceList(s.received_starts, simpl->starts());
fillInterfaceList(s.received_ends, simpl->ends());
fillInterfaceList(s.generated_starts, simpl->nextStarts());
fillInterfaceList(s.generated_ends, simpl->prevEnds());
// insert solution IDs
Stage::SolutionProcessor solutionProcessor = [&s](const SolutionBase& solution) {
s.solved.push_back(solution.id());
return true;
};
stage.processSolutions(solutionProcessor);
solutionProcessor = [&s](const SolutionBase& solution) {
s.failed.push_back(solution.id());
return true;
};
stage.processFailures(solutionProcessor);
}
moveit_task_constructor::TaskDescription& Task::fillTaskDescription(moveit_task_constructor::TaskDescription &msg) const
{
std::map<const Stage*, moveit_task_constructor::StageStatistics::_id_type> stage_to_id_map;
stage_to_id_map[this] = 0; // ID for root
ContainerBase::StageCallback stageProcessor = ContainerBase::StageCallback stageProcessor =
[&stage_to_id_map, &msg](const Stage& stage, int) -> bool { [&stage_to_id_map, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent // this method is called for each child stage of a given parent
const StagePrivate *simpl = stage.pimpl(); moveit_task_constructor::StageDescription desc;
moveit_task_constructor::StageStatistics stat;
desc.id = stat.id = stage_to_id_map.size();
stage_to_id_map[&stage] = stat.id;
moveit_task_constructor::Stage s; // create new Stage msg desc.name = stage.name();
s.id = stage_to_id_map.size(); desc.flags = stage.pimpl()->interfaceFlags();
stage_to_id_map[&stage] = s.id; // TODO fill stage properties
s.name = stage.name();
s.flags = stage.pimpl()->interfaceFlags(); auto it = stage_to_id_map.find(stage.pimpl()->parent());
auto it = stage_to_id_map.find(simpl->parent());
assert (it != stage_to_id_map.cend()); assert (it != stage_to_id_map.cend());
s.parent_id = it->second; desc.parent_id = stat.parent_id = it->second;
fillStateList(s.received_starts, simpl->starts()); fillStageStatistics(stage, stat);
fillStateList(s.received_ends, simpl->ends());
fillStateList(s.generated_starts, simpl->nextStarts());
fillStateList(s.generated_ends, simpl->prevEnds());
// insert solution IDs // finally store in msg
Stage::SolutionProcessor solutionProcessor = [&s](const SolutionBase& solution) { msg.description.push_back(std::move(desc));
s.solved.push_back(solution.id()); msg.statistics.push_back(std::move(stat));
return true;
};
stage.processSolutions(solutionProcessor);
solutionProcessor = [&s](const SolutionBase& solution) {
s.failed.push_back(solution.id());
return true;
};
stage.processFailures(solutionProcessor);
// finally store in msg.stages
msg.stages.push_back(std::move(s));
return true; return true;
}; };
msg.id = id(); msg.description.clear();
msg.stages.clear(); msg.statistics.clear();
stage_to_id_map[this] = 0; // ID for root
stages()->traverseRecursively(stageProcessor); stages()->traverseRecursively(stageProcessor);
msg.id = id();
return msg;
}
moveit_task_constructor::TaskStatistics& Task::fillTaskStatistics(moveit_task_constructor::TaskStatistics &msg) const
{
std::map<const Stage*, moveit_task_constructor::StageStatistics::_id_type> stage_to_id_map;
stage_to_id_map[this] = 0; // ID for root
ContainerBase::StageCallback stageProcessor =
[&stage_to_id_map, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent
moveit_task_constructor::StageStatistics stat; // create new Stage msg
stat.id = stage_to_id_map.size();
stage_to_id_map[&stage] = stat.id;
auto it = stage_to_id_map.find(stage.pimpl()->parent());
assert (it != stage_to_id_map.cend());
stat.parent_id = it->second;
fillStageStatistics(stage, stat);
// finally store in msg.stages
msg.stages.push_back(std::move(stat));
return true;
};
msg.stages.clear();
stages()->traverseRecursively(stageProcessor);
msg.id = id();
return msg; return msg;
} }

View File

@ -53,23 +53,25 @@ protected:
int num_inserts = 0; int num_inserts = 0;
int num_updates = 0; int num_updates = 0;
moveit_task_constructor::Task genMsg(const std::string &name) { moveit_task_constructor::TaskDescription genMsg(const std::string &name) {
moveit_task_constructor::Task t; moveit_task_constructor::TaskDescription t;
t.id = name; t.id = name;
uint id = 0; uint id = 0, root_id;
moveit_task_constructor::Stage root; moveit_task_constructor::StageDescription desc;
root.parent_id = id; moveit_task_constructor::StageStatistics stat;
root.id = ++id; desc.parent_id = stat.parent_id = id;
root.name = name; desc.id = stat.id = root_id = ++id;
t.stages.push_back(root); desc.name = name;
t.description.push_back(desc);
t.statistics.push_back(stat);
for (int i = 0; i != children; ++i) { for (int i = 0; i != children; ++i) {
moveit_task_constructor::Stage child; desc.parent_id = stat.parent_id = root_id;
child.parent_id = root.id; desc.id = stat.id = ++id;
child.id = ++id; desc.name = std::to_string(i);
child.name = std::to_string(i); t.description.push_back(desc);
t.stages.push_back(child); t.statistics.push_back(stat);
} }
return t; return t;
} }
@ -123,7 +125,7 @@ protected:
SCOPED_TRACE("first i=" + std::to_string(i)); SCOPED_TRACE("first i=" + std::to_string(i));
num_inserts = 0; num_inserts = 0;
num_updates = 0; num_updates = 0;
model.processTaskMessage(genMsg("first")); model.processTaskDescriptionMessage(genMsg("first"));
if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task
else EXPECT_EQ(num_inserts, 0); else EXPECT_EQ(num_inserts, 0);
@ -135,7 +137,7 @@ protected:
SCOPED_TRACE("second i=" + std::to_string(i)); SCOPED_TRACE("second i=" + std::to_string(i));
num_inserts = 0; num_inserts = 0;
num_updates = 0; num_updates = 0;
model.processTaskMessage(genMsg("second")); // 1 notify for inserted task model.processTaskDescriptionMessage(genMsg("second")); // 1 notify for inserted task
if (i == 0) EXPECT_EQ(num_inserts, 1); if (i == 0) EXPECT_EQ(num_inserts, 1);
else EXPECT_EQ(num_inserts, 0); else EXPECT_EQ(num_inserts, 0);
@ -157,7 +159,7 @@ protected:
TEST_F(TaskListModelTest, remoteTaskModel) { TEST_F(TaskListModelTest, remoteTaskModel) {
children = 3; children = 3;
moveit_rviz_plugin::RemoteTaskModel m; moveit_rviz_plugin::RemoteTaskModel m;
m.processTaskMessage(genMsg("first")); m.processStageDescriptions(genMsg("first").description);
SCOPED_TRACE("first"); SCOPED_TRACE("first");
validate(m, {"first"}); validate(m, {"first"});
} }
@ -188,13 +190,13 @@ TEST_F(TaskListModelTest, threeChildren) {
TEST_F(TaskListModelTest, visitedPopulate) { TEST_F(TaskListModelTest, visitedPopulate) {
// first population without children // first population without children
children = 0; children = 0;
model.processTaskMessage(genMsg("first")); model.processTaskDescriptionMessage(genMsg("first"));
validate(model, {"first"}); // validation visits root node validate(model, {"first"}); // validation visits root node
EXPECT_EQ(num_inserts, 1); EXPECT_EQ(num_inserts, 1);
children = 3; children = 3;
num_inserts = 0; num_inserts = 0;
model.processTaskMessage(genMsg("first")); model.processTaskDescriptionMessage(genMsg("first"));
validate(model, {"first"}); validate(model, {"first"});
// second population with children should emit insert notifies for them // second population with children should emit insert notifies for them
EXPECT_EQ(num_inserts, 3); EXPECT_EQ(num_inserts, 3);

View File

@ -1,7 +0,0 @@
# ID of InterfaceState
uint32 state_id
---
# planning scene diff to tasks' main planning scene
moveit_msgs/PlanningScene scene

View File

@ -182,11 +182,10 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i
return n->setName(value.toString()); return n->setName(value.toString());
} }
typedef moveit_task_constructor::Stage StageMsg; void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor::TaskDescription::_description_type &msg)
void RemoteTaskModel::processTaskMessage(const moveit_task_constructor::Task &msg)
{ {
// iterate over msg.stages and create new nodes where needed // iterate over descriptions and create new / update existing nodes where needed
for (const StageMsg &s : msg.stages) { for (const auto &s : msg) {
// find parent node for stage s, this should always exist // find parent node for stage s, this should always exist
auto parent_it = id_to_stage_.find(s.parent_id); auto parent_it = id_to_stage_.find(s.parent_id);
if (parent_it == id_to_stage_.end()) { if (parent_it == id_to_stage_.end()) {
@ -229,4 +228,8 @@ void RemoteTaskModel::processTaskMessage(const moveit_task_constructor::Task &ms
} }
} }
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor::TaskDescription::_statistics_type &msg)
{
}
} }

View File

@ -62,7 +62,8 @@ public:
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override;
void processTaskMessage(const moveit_task_constructor::Task &msg); void processStageDescriptions(const moveit_task_constructor::TaskDescription::_description_type &msg);
void processStageStatistics(const moveit_task_constructor::TaskDescription::_statistics_type &msg);
}; };
} }

View File

@ -57,19 +57,12 @@ TaskDisplay::TaskDisplay() : Display()
"Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
this, SLOT(changedRobotDescription()), this); this, SLOT(changedRobotDescription()), this);
task_monitor_topic_property_ =
new rviz::RosTopicProperty("Task Monitor Topic", DEFAULT_TASK_MONITOR_TOPIC,
ros::message_traits::datatype<moveit_task_constructor::Task>(),
"The topic on which task updates (moveit_msgs::Task messages) are received",
this, SLOT(changedTaskMonitorTopic()), this);
task_solution_topic_property_ = task_solution_topic_property_ =
new rviz::RosTopicProperty("Task Solution Topic", DEFAULT_TASK_SOLUTION_TOPIC, new rviz::RosTopicProperty("Task Solution Topic", "",
ros::message_traits::datatype<moveit_task_constructor::Solution>(), ros::message_traits::datatype<moveit_task_constructor::Solution>(),
"The topic on which task solutions (moveit_msgs::Solution messages) are received", "The topic on which task solutions (moveit_msgs::Solution messages) are received",
this, SLOT(changedTaskSolutionTopic()), this); this, SLOT(changedTaskSolutionTopic()), this);
trajectory_visual_.reset(new TaskSolutionVisualization(this, this)); trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
} }
@ -122,7 +115,8 @@ void TaskDisplay::onDisable()
trajectory_visual_->onDisable(); trajectory_visual_->onDisable();
// don't monitor topics when disabled // don't monitor topics when disabled
task_monitor_sub.shutdown(); task_description_sub.shutdown();
task_statistics_sub.shutdown();
task_solution_sub.shutdown(); task_solution_sub.shutdown();
} }
@ -149,44 +143,49 @@ void TaskDisplay::changedRobotDescription()
void TaskDisplay::updateTaskListModel() void TaskDisplay::updateTaskListModel()
{ {
task_model_ = TaskListModelCache::instance().getModel( // generate task monitoring topics from solution topic
task_monitor_topic_property_->getString(), std::string solution_topic = task_solution_topic_property_->getStdString();
task_solution_topic_property_->getString()); auto lastSep = solution_topic.find_last_of('/');
std::string base_ns = solution_topic.substr(0, lastSep);
if (!task_model_) { task_model_ = TaskListModelCache::instance().getModel(base_ns);
if (task_monitor_topic_property_->getString().isEmpty())
setStatus(rviz::StatusProperty::Warn, "Task Monitor", "invalid task monitor topic"); if (task_model_) {
else if (task_solution_topic_property_->getString().isEmpty()) // listen to task descriptions updates
setStatus(rviz::StatusProperty::Warn, "Task Monitor", "invalid task solution topic"); boost::function<void(const moveit_task_constructor::TaskDescriptionConstPtr &)> taskDescCB
else ([this](const moveit_task_constructor::TaskDescriptionConstPtr &msg){
setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel"); mainloop_jobs_.addJob([this, msg]() { task_model_->processTaskDescriptionMessage(*msg); });
});
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, taskDescCB);
// listen to task statistics updates
boost::function<void(const moveit_task_constructor::TaskDescriptionConstPtr &)> taskStatCB
([this](const moveit_task_constructor::TaskDescriptionConstPtr &msg){
mainloop_jobs_.addJob([this, msg]() { task_model_->processTaskDescriptionMessage(*msg); });
});
task_description_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, taskStatCB);
} else { } else {
boost::function<void(const moveit_task_constructor::TaskConstPtr &)> taskCB setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel");
([this](const moveit_task_constructor::TaskConstPtr &msg){
mainloop_jobs_.addJob([this, msg]() { task_model_->processTaskMessage(*msg); });
});
task_monitor_sub = update_nh_.subscribe(task_monitor_topic_property_->getStdString(), 2, taskCB);
boost::function<void(const moveit_task_constructor::SolutionConstPtr &)> solCB
([this](const moveit_task_constructor::SolutionConstPtr &msg){
mainloop_jobs_.addJob([this, msg]() { task_model_->processSolutionMessage(*msg); });
// TODO: use already processed trajectory (e.g. by ID)
mainloop_jobs_.addJob([this, msg]() {trajectory_visual_->showTrajectory(*msg);});
});
task_solution_sub = update_nh_.subscribe(task_solution_topic_property_->getStdString(), 2, solCB);
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
} }
}
void TaskDisplay::changedTaskMonitorTopic() // listen to task solutions
{ boost::function<void(const moveit_task_constructor::SolutionConstPtr &)> solCB
task_monitor_sub.shutdown(); ([this](const moveit_task_constructor::SolutionConstPtr &msg){
updateTaskListModel(); mainloop_jobs_.addJob([this, msg]() {
if (task_model_) task_model_->processSolutionMessage(*msg);
// TODO: use already processed trajectory (e.g. by ID)
trajectory_visual_->showTrajectory(*msg);
});
});
task_solution_sub = update_nh_.subscribe(solution_topic, 2, solCB);
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
} }
void TaskDisplay::changedTaskSolutionTopic() void TaskDisplay::changedTaskSolutionTopic()
{ {
task_description_sub.shutdown();
task_statistics_sub.shutdown();
task_solution_sub.shutdown(); task_solution_sub.shutdown();
updateTaskListModel(); updateTaskListModel();
} }

View File

@ -84,15 +84,15 @@ private Q_SLOTS:
* \brief Slot Event Functions * \brief Slot Event Functions
*/ */
void changedRobotDescription(); void changedRobotDescription();
void changedTaskMonitorTopic();
void changedTaskSolutionTopic(); void changedTaskSolutionTopic();
protected: protected:
void updateTaskListModel(); void updateTaskListModel();
protected: protected:
ros::Subscriber task_monitor_sub;
ros::Subscriber task_solution_sub; ros::Subscriber task_solution_sub;
ros::Subscriber task_description_sub;
ros::Subscriber task_statistics_sub;
// handle processing of task+solution messages in Qt mainloop // handle processing of task+solution messages in Qt mainloop
moveit::tools::JobQueue mainloop_jobs_; moveit::tools::JobQueue mainloop_jobs_;
@ -107,7 +107,6 @@ protected:
// Properties // Properties
rviz::StringProperty* robot_description_property_; rviz::StringProperty* robot_description_property_;
rviz::RosTopicProperty* task_monitor_topic_property_;
rviz::RosTopicProperty* task_solution_topic_property_; rviz::RosTopicProperty* task_solution_topic_property_;
}; };

View File

@ -254,10 +254,9 @@ bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent)
return false; return false;
} }
// process a task monitoring message: // process a task description message:
// update existing RemoteTask, create a new one, // update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
// or (if msg.stages is empty) delete an existing one void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor::TaskDescription &msg)
void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg)
{ {
Q_D(TaskListModel); Q_D(TaskListModel);
@ -266,8 +265,8 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg)
bool created = it_inserted.second; bool created = it_inserted.second;
RemoteTaskModel*& remote_task = it_inserted.first->second; RemoteTaskModel*& remote_task = it_inserted.first->second;
// empty stages list indicates, that this remote task is not available anymore // empty list indicates, that this remote task is not available anymore
if (msg.stages.empty()) { if (msg.description.empty()) {
if (!remote_task) { // task was already deleted locally if (!remote_task) { // task was already deleted locally
// we can now remove it from remote_tasks_ // we can now remove it from remote_tasks_
d->remote_tasks_.erase(it_inserted.first); d->remote_tasks_.erase(it_inserted.first);
@ -287,7 +286,8 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg)
if (!remote_task) if (!remote_task)
return; // task is not in use anymore return; // task is not in use anymore
remote_task->processTaskMessage(msg); remote_task->processStageDescriptions(msg.description);
remote_task->processStageStatistics(msg.statistics);
// insert newly created model into this' model instance // insert newly created model into this' model instance
if (created) { if (created) {
@ -296,6 +296,22 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg)
} }
} }
// process a task statistics message
void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor::TaskStatistics &msg)
{
Q_D(TaskListModel);
auto it = d->remote_tasks_.find(msg.id);
if (it == d->remote_tasks_.cend())
return; // unkown task
RemoteTaskModel* remote_task = it->second;
if (!remote_task)
return; // task is not in use anymore
remote_task->processStageStatistics(msg.stages);
}
void TaskListModel::processSolutionMessage(const moveit_task_constructor::Solution &msg) void TaskListModel::processSolutionMessage(const moveit_task_constructor::Solution &msg)
{ {
// TODO // TODO

View File

@ -37,7 +37,8 @@
#pragma once #pragma once
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_task_constructor/Task.h> #include <moveit_task_constructor/TaskDescription.h>
#include <moveit_task_constructor/TaskStatistics.h>
#include <moveit_task_constructor/Solution.h> #include <moveit_task_constructor/Solution.h>
#include <QAbstractItemModel> #include <QAbstractItemModel>
@ -89,8 +90,10 @@ public:
bool removeRows(int row, int count, const QModelIndex &parent) override; bool removeRows(int row, int count, const QModelIndex &parent) override;
/// process an incoming task message - only call in Qt's main loop /// process an incoming task description message - only call in Qt's main loop
void processTaskMessage(const moveit_task_constructor::Task &msg); void processTaskDescriptionMessage(const moveit_task_constructor::TaskDescription &msg);
/// process an incoming task description message - only call in Qt's main loop
void processTaskStatisticsMessage(const moveit_task_constructor::TaskStatistics &msg);
/// process an incoming solution message - only call in Qt's main loop /// process an incoming solution message - only call in Qt's main loop
void processSolutionMessage(const moveit_task_constructor::Solution &msg); void processSolutionMessage(const moveit_task_constructor::Solution &msg);

View File

@ -49,14 +49,13 @@ TaskListModelCache &TaskListModelCache::instance()
return instance_; return instance_;
} }
TaskListModelPtr TaskListModelCache::getModel(const QString &task_monitor_topic, TaskListModelPtr TaskListModelCache::getModel(const std::string& ns)
const QString &task_solution_topic)
{ {
if (task_monitor_topic.isEmpty() || task_solution_topic.isEmpty()) { if (ns.empty()) {
return TaskListModelPtr(); return TaskListModelPtr();
} else { } else {
// retrieve existing model for given topic pair // retrieve existing model for given topic pair
TaskListModelWeakPtr& model = cache_[std::make_pair(task_monitor_topic, task_solution_topic)]; TaskListModelWeakPtr& model = cache_[ns];
TaskListModelPtr result; TaskListModelPtr result;
if (model.expired()) { if (model.expired()) {

View File

@ -45,7 +45,7 @@ namespace moveit_rviz_plugin {
* *
* This global model instance is used by TaskPanels and can be retrieved via getGlobalModel(). * This global model instance is used by TaskPanels and can be retrieved via getGlobalModel().
* Additionally, this instance maintains a cache for all TaskListModels used e.g. by TaskDisplays. * Additionally, this instance maintains a cache for all TaskListModels used e.g. by TaskDisplays.
* Displays subscribing to the same (monitor, solution) topic pair, will share the same model. * Displays subscribing to the same topic namespace, will share the same model.
* *
* This is a singleton instance. * This is a singleton instance.
*/ */
@ -53,7 +53,7 @@ class TaskListModelCache : public QObject {
Q_OBJECT Q_OBJECT
TaskListModelPtr global_model_; TaskListModelPtr global_model_;
std::map<std::pair<QString, QString>, TaskListModelWeakPtr> cache_; std::map<std::string, TaskListModelWeakPtr> cache_;
/// class is singleton /// class is singleton
TaskListModelCache(); TaskListModelCache();
@ -68,8 +68,7 @@ public:
static TaskListModelCache& instance(); static TaskListModelCache& instance();
/// get TaskListModel for a TaskDisplay /// get TaskListModel for a TaskDisplay
TaskListModelPtr getModel(const QString& task_monitor_topic, TaskListModelPtr getModel(const std::__cxx11::string &ns);
const QString& task_solution_topic);
/// get global TaskListModel instance used for panels /// get global TaskListModel instance used for panels
TaskListModelPtr getGlobalModel(); TaskListModelPtr getGlobalModel();

View File

@ -431,8 +431,9 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::So
return; return;
} }
if (!msg.model_id.empty() && msg.model_id != robot_model_->getName()) if (msg.start_scene.robot_model_name != robot_model_->getName())
ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", msg.model_id.c_str(), ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected",
msg.start_scene.robot_model_name .c_str(),
robot_model_->getName().c_str()); robot_model_->getName().c_str());
trajectory_message_to_display_.reset(); trajectory_message_to_display_.reset();