mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
renamed Task, Stage msgs
- Stage -> StageStatistics + StageDescription - Task -> TaskStatistics + TaskDescription - removed GetInterfaceState.srv
This commit is contained in:
parent
18119a7985
commit
a43692fc25
@ -18,15 +18,16 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
# ROS messages, services and actions
|
||||
add_message_files(DIRECTORY msg
|
||||
FILES
|
||||
Stage.msg
|
||||
Task.msg
|
||||
StageDescription.msg
|
||||
StageStatistics.msg
|
||||
TaskDescription.msg
|
||||
TaskStatistics.msg
|
||||
Solution.msg
|
||||
SubSolution.msg
|
||||
SubTrajectory.msg
|
||||
Solution.msg
|
||||
)
|
||||
add_service_files(DIRECTORY srv
|
||||
FILES
|
||||
GetInterfaceState.srv
|
||||
GetSolution.srv
|
||||
)
|
||||
generate_messages(DEPENDENCIES ${MSG_DEPS})
|
||||
|
||||
@ -3,40 +3,47 @@
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <ros/publisher.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/GetInterfaceState.h>
|
||||
#include <moveit_task_constructor/GetSolution.h>
|
||||
|
||||
#define DESCRIPTION_TOPIC "description"
|
||||
#define STATISTICS_TOPIC "statistics"
|
||||
#define SOLUTION_TOPIC "solution"
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
MOVEIT_CLASS_FORWARD(Task)
|
||||
MOVEIT_CLASS_FORWARD(SolutionBase)
|
||||
|
||||
#define DEFAULT_TASK_MONITOR_TOPIC "task_monitoring"
|
||||
#define DEFAULT_TASK_SOLUTION_TOPIC "task_solutions"
|
||||
|
||||
class Introspection {
|
||||
// publish Task state and (new) Solutions
|
||||
ros::Publisher task_publisher_;
|
||||
// publish task detailed description and current state
|
||||
ros::Publisher task_description_publisher_;
|
||||
ros::Publisher task_statistics_publisher_;
|
||||
// publish new solutions
|
||||
ros::Publisher solution_publisher_;
|
||||
// services to provide InterfaceState and Solution
|
||||
ros::ServiceServer get_interface_state_service_;
|
||||
// services to provide an individual Solution
|
||||
ros::ServiceServer get_solution_service_;
|
||||
|
||||
public:
|
||||
Introspection(const std::string &task_topic = DEFAULT_TASK_MONITOR_TOPIC,
|
||||
const std::string &solution_topic = DEFAULT_TASK_SOLUTION_TOPIC);
|
||||
Introspection();
|
||||
Introspection(const Introspection &other) = delete;
|
||||
static Introspection &instance();
|
||||
|
||||
// publish the current state of given task
|
||||
void publishTask(const Task &t);
|
||||
void publishTask(const ::moveit_task_constructor::Task &msg) {
|
||||
task_publisher_.publish(msg);
|
||||
// publish detailed task description
|
||||
void publishTaskDescription(const Task &t);
|
||||
void publishTaskDescription(const ::moveit_task_constructor::TaskDescription &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
|
||||
void publishSolution(const SolutionBase &s);
|
||||
@ -45,9 +52,6 @@ public:
|
||||
}
|
||||
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
|
||||
bool getSolution(moveit_task_constructor::GetSolution::Request &req,
|
||||
moveit_task_constructor::GetSolution::Response &res);
|
||||
|
||||
@ -6,7 +6,8 @@
|
||||
#include "container.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>
|
||||
|
||||
namespace robot_model_loader {
|
||||
@ -57,8 +58,10 @@ public:
|
||||
bool plan();
|
||||
/// print current state std::cout
|
||||
static void printState(const Task &t);
|
||||
/// fill task message for publishing the current state
|
||||
moveit_task_constructor::Task& fillMessage(moveit_task_constructor::Task& msg) const;
|
||||
/// fill task description message for publishing the task configuration
|
||||
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;
|
||||
|
||||
|
||||
@ -1,8 +1,8 @@
|
||||
# id of associated task
|
||||
string task_id
|
||||
|
||||
# id of robot model
|
||||
string model_id
|
||||
# planning scene of start state
|
||||
moveit_msgs/PlanningScene start_scene
|
||||
|
||||
# set of all sub solutions involved
|
||||
SubSolution[] sub_solution
|
||||
|
||||
17
msg/StageDescription.msg
Normal file
17
msg/StageDescription.msg
Normal 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
|
||||
@ -1,12 +1,8 @@
|
||||
# dynamically changing information for a stage
|
||||
|
||||
# unique id within task
|
||||
uint32 id
|
||||
|
||||
# name of this stage
|
||||
string name
|
||||
|
||||
# flags: interface, ...
|
||||
uint32 flags
|
||||
|
||||
# parent id, parent_id == id means root
|
||||
uint32 parent_id
|
||||
|
||||
@ -4,9 +4,5 @@ uint32 id
|
||||
# associated cost
|
||||
float32 cost
|
||||
|
||||
# IDs of start and end state?
|
||||
#uint32 start_id
|
||||
#uint32 end_id
|
||||
|
||||
# IDs of subsolutions
|
||||
uint32[] sub_solution_id
|
||||
|
||||
@ -1,22 +1,18 @@
|
||||
# unique id within task
|
||||
uint32 id
|
||||
|
||||
# associated names
|
||||
string name
|
||||
|
||||
# associated cost
|
||||
float32 cost
|
||||
|
||||
# name of task stage
|
||||
string stage
|
||||
|
||||
# IDs of start and end state?
|
||||
# Or have an array of them in Solution.msg?
|
||||
#uint32 start_id
|
||||
#uint32 end_id
|
||||
# optional associated name of sub trajectory
|
||||
string name
|
||||
|
||||
# trajectory
|
||||
moveit_msgs/RobotTrajectory trajectory
|
||||
# markers, e.g. providing additional hints or illustrating failure
|
||||
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
7
msg/TaskDescription.msg
Normal 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
|
||||
|
||||
@ -2,4 +2,4 @@
|
||||
string id
|
||||
|
||||
# list of all stages, including the task stage itself
|
||||
Stage[] stages
|
||||
StageStatistics[] stages
|
||||
@ -7,15 +7,14 @@
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
Introspection::Introspection(const std::string &task_topic,
|
||||
const std::string &solution_topic)
|
||||
Introspection::Introspection()
|
||||
{
|
||||
ros::NodeHandle n;
|
||||
task_publisher_ = n.advertise<moveit_task_constructor::Task>(task_topic, 1);
|
||||
solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(solution_topic, 1, true);
|
||||
task_description_publisher_ = n.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1);
|
||||
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
|
||||
get_interface_state_service_ = n.advertiseService("get_interface_state", &Introspection::getInterfaceState, this);
|
||||
get_solution_service_ = n.advertiseService("get_solution", &Introspection::getSolution, this);
|
||||
}
|
||||
|
||||
@ -25,10 +24,16 @@ Introspection &Introspection::instance()
|
||||
return instance_;
|
||||
}
|
||||
|
||||
void Introspection::publishTask(const Task &t)
|
||||
void Introspection::publishTaskDescription(const Task &t)
|
||||
{
|
||||
::moveit_task_constructor::Task msg;
|
||||
task_publisher_.publish(t.fillMessage(msg));
|
||||
::moveit_task_constructor::TaskDescription 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)
|
||||
@ -38,16 +43,6 @@ void Introspection::publishSolution(const SolutionBase &s)
|
||||
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,
|
||||
moveit_task_constructor::GetSolution::Response &res)
|
||||
{
|
||||
|
||||
@ -68,9 +68,8 @@ void SubTrajectory::fillMessage(moveit_task_constructor::Solution &msg) const {
|
||||
msg.sub_trajectory.emplace_back();
|
||||
moveit_task_constructor::SubTrajectory& t = msg.sub_trajectory.back();
|
||||
t.id = this->id();
|
||||
t.name = this->name();
|
||||
t.cost = this->cost();
|
||||
t.stage = this->creator()->name();
|
||||
t.name = this->name();
|
||||
if (trajectory())
|
||||
trajectory()->getRobotTrajectoryMsg(t.trajectory);
|
||||
t.markers = this->markers();
|
||||
|
||||
110
src/task.cpp
110
src/task.cpp
@ -216,7 +216,7 @@ void Task::printState(const Task &t){
|
||||
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) {
|
||||
c.clear();
|
||||
if (!interface) return;
|
||||
@ -224,50 +224,94 @@ void fillStateList(moveit_task_constructor::Stage::_received_starts_type &c,
|
||||
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 =
|
||||
[&stage_to_id_map, &msg](const Stage& stage, int) -> bool {
|
||||
// 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
|
||||
s.id = stage_to_id_map.size();
|
||||
stage_to_id_map[&stage] = s.id;
|
||||
s.name = stage.name();
|
||||
s.flags = stage.pimpl()->interfaceFlags();
|
||||
auto it = stage_to_id_map.find(simpl->parent());
|
||||
desc.name = stage.name();
|
||||
desc.flags = stage.pimpl()->interfaceFlags();
|
||||
// TODO fill stage properties
|
||||
|
||||
auto it = stage_to_id_map.find(stage.pimpl()->parent());
|
||||
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());
|
||||
fillStateList(s.received_ends, simpl->ends());
|
||||
fillStateList(s.generated_starts, simpl->nextStarts());
|
||||
fillStateList(s.generated_ends, simpl->prevEnds());
|
||||
fillStageStatistics(stage, stat);
|
||||
|
||||
// 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);
|
||||
|
||||
// finally store in msg.stages
|
||||
msg.stages.push_back(std::move(s));
|
||||
// finally store in msg
|
||||
msg.description.push_back(std::move(desc));
|
||||
msg.statistics.push_back(std::move(stat));
|
||||
return true;
|
||||
};
|
||||
|
||||
msg.id = id();
|
||||
msg.stages.clear();
|
||||
stage_to_id_map[this] = 0; // ID for root
|
||||
msg.description.clear();
|
||||
msg.statistics.clear();
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@ -53,23 +53,25 @@ protected:
|
||||
int num_inserts = 0;
|
||||
int num_updates = 0;
|
||||
|
||||
moveit_task_constructor::Task genMsg(const std::string &name) {
|
||||
moveit_task_constructor::Task t;
|
||||
moveit_task_constructor::TaskDescription genMsg(const std::string &name) {
|
||||
moveit_task_constructor::TaskDescription t;
|
||||
t.id = name;
|
||||
uint id = 0;
|
||||
uint id = 0, root_id;
|
||||
|
||||
moveit_task_constructor::Stage root;
|
||||
root.parent_id = id;
|
||||
root.id = ++id;
|
||||
root.name = name;
|
||||
t.stages.push_back(root);
|
||||
moveit_task_constructor::StageDescription desc;
|
||||
moveit_task_constructor::StageStatistics stat;
|
||||
desc.parent_id = stat.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) {
|
||||
moveit_task_constructor::Stage child;
|
||||
child.parent_id = root.id;
|
||||
child.id = ++id;
|
||||
child.name = std::to_string(i);
|
||||
t.stages.push_back(child);
|
||||
desc.parent_id = stat.parent_id = root_id;
|
||||
desc.id = stat.id = ++id;
|
||||
desc.name = std::to_string(i);
|
||||
t.description.push_back(desc);
|
||||
t.statistics.push_back(stat);
|
||||
}
|
||||
return t;
|
||||
}
|
||||
@ -123,7 +125,7 @@ protected:
|
||||
SCOPED_TRACE("first i=" + std::to_string(i));
|
||||
num_inserts = 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
|
||||
else EXPECT_EQ(num_inserts, 0);
|
||||
@ -135,7 +137,7 @@ protected:
|
||||
SCOPED_TRACE("second i=" + std::to_string(i));
|
||||
num_inserts = 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);
|
||||
else EXPECT_EQ(num_inserts, 0);
|
||||
@ -157,7 +159,7 @@ protected:
|
||||
TEST_F(TaskListModelTest, remoteTaskModel) {
|
||||
children = 3;
|
||||
moveit_rviz_plugin::RemoteTaskModel m;
|
||||
m.processTaskMessage(genMsg("first"));
|
||||
m.processStageDescriptions(genMsg("first").description);
|
||||
SCOPED_TRACE("first");
|
||||
validate(m, {"first"});
|
||||
}
|
||||
@ -188,13 +190,13 @@ TEST_F(TaskListModelTest, threeChildren) {
|
||||
TEST_F(TaskListModelTest, visitedPopulate) {
|
||||
// first population without children
|
||||
children = 0;
|
||||
model.processTaskMessage(genMsg("first"));
|
||||
model.processTaskDescriptionMessage(genMsg("first"));
|
||||
validate(model, {"first"}); // validation visits root node
|
||||
EXPECT_EQ(num_inserts, 1);
|
||||
|
||||
children = 3;
|
||||
num_inserts = 0;
|
||||
model.processTaskMessage(genMsg("first"));
|
||||
model.processTaskDescriptionMessage(genMsg("first"));
|
||||
validate(model, {"first"});
|
||||
// second population with children should emit insert notifies for them
|
||||
EXPECT_EQ(num_inserts, 3);
|
||||
|
||||
@ -1,7 +0,0 @@
|
||||
# ID of InterfaceState
|
||||
uint32 state_id
|
||||
|
||||
---
|
||||
|
||||
# planning scene diff to tasks' main planning scene
|
||||
moveit_msgs/PlanningScene scene
|
||||
@ -182,11 +182,10 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i
|
||||
return n->setName(value.toString());
|
||||
}
|
||||
|
||||
typedef moveit_task_constructor::Stage StageMsg;
|
||||
void RemoteTaskModel::processTaskMessage(const moveit_task_constructor::Task &msg)
|
||||
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor::TaskDescription::_description_type &msg)
|
||||
{
|
||||
// iterate over msg.stages and create new nodes where needed
|
||||
for (const StageMsg &s : msg.stages) {
|
||||
// iterate over descriptions and create new / update existing nodes where needed
|
||||
for (const auto &s : msg) {
|
||||
// find parent node for stage s, this should always exist
|
||||
auto parent_it = id_to_stage_.find(s.parent_id);
|
||||
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)
|
||||
{
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -62,7 +62,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 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);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -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",
|
||||
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_ =
|
||||
new rviz::RosTopicProperty("Task Solution Topic", DEFAULT_TASK_SOLUTION_TOPIC,
|
||||
new rviz::RosTopicProperty("Task Solution Topic", "",
|
||||
ros::message_traits::datatype<moveit_task_constructor::Solution>(),
|
||||
"The topic on which task solutions (moveit_msgs::Solution messages) are received",
|
||||
this, SLOT(changedTaskSolutionTopic()), this);
|
||||
|
||||
|
||||
trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
|
||||
}
|
||||
|
||||
@ -122,7 +115,8 @@ void TaskDisplay::onDisable()
|
||||
trajectory_visual_->onDisable();
|
||||
|
||||
// don't monitor topics when disabled
|
||||
task_monitor_sub.shutdown();
|
||||
task_description_sub.shutdown();
|
||||
task_statistics_sub.shutdown();
|
||||
task_solution_sub.shutdown();
|
||||
}
|
||||
|
||||
@ -149,44 +143,49 @@ void TaskDisplay::changedRobotDescription()
|
||||
|
||||
void TaskDisplay::updateTaskListModel()
|
||||
{
|
||||
task_model_ = TaskListModelCache::instance().getModel(
|
||||
task_monitor_topic_property_->getString(),
|
||||
task_solution_topic_property_->getString());
|
||||
// generate task monitoring topics from solution topic
|
||||
std::string solution_topic = task_solution_topic_property_->getStdString();
|
||||
auto lastSep = solution_topic.find_last_of('/');
|
||||
std::string base_ns = solution_topic.substr(0, lastSep);
|
||||
|
||||
if (!task_model_) {
|
||||
if (task_monitor_topic_property_->getString().isEmpty())
|
||||
setStatus(rviz::StatusProperty::Warn, "Task Monitor", "invalid task monitor topic");
|
||||
else if (task_solution_topic_property_->getString().isEmpty())
|
||||
setStatus(rviz::StatusProperty::Warn, "Task Monitor", "invalid task solution topic");
|
||||
else
|
||||
setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel");
|
||||
task_model_ = TaskListModelCache::instance().getModel(base_ns);
|
||||
|
||||
if (task_model_) {
|
||||
// listen to task descriptions updates
|
||||
boost::function<void(const moveit_task_constructor::TaskDescriptionConstPtr &)> taskDescCB
|
||||
([this](const moveit_task_constructor::TaskDescriptionConstPtr &msg){
|
||||
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 {
|
||||
boost::function<void(const moveit_task_constructor::TaskConstPtr &)> taskCB
|
||||
([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");
|
||||
setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel");
|
||||
}
|
||||
}
|
||||
|
||||
void TaskDisplay::changedTaskMonitorTopic()
|
||||
{
|
||||
task_monitor_sub.shutdown();
|
||||
updateTaskListModel();
|
||||
// listen to task solutions
|
||||
boost::function<void(const moveit_task_constructor::SolutionConstPtr &)> solCB
|
||||
([this](const moveit_task_constructor::SolutionConstPtr &msg){
|
||||
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()
|
||||
{
|
||||
task_description_sub.shutdown();
|
||||
task_statistics_sub.shutdown();
|
||||
task_solution_sub.shutdown();
|
||||
updateTaskListModel();
|
||||
}
|
||||
|
||||
@ -84,15 +84,15 @@ private Q_SLOTS:
|
||||
* \brief Slot Event Functions
|
||||
*/
|
||||
void changedRobotDescription();
|
||||
void changedTaskMonitorTopic();
|
||||
void changedTaskSolutionTopic();
|
||||
|
||||
protected:
|
||||
void updateTaskListModel();
|
||||
|
||||
protected:
|
||||
ros::Subscriber task_monitor_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
|
||||
moveit::tools::JobQueue mainloop_jobs_;
|
||||
|
||||
@ -107,7 +107,6 @@ protected:
|
||||
|
||||
// Properties
|
||||
rviz::StringProperty* robot_description_property_;
|
||||
rviz::RosTopicProperty* task_monitor_topic_property_;
|
||||
rviz::RosTopicProperty* task_solution_topic_property_;
|
||||
};
|
||||
|
||||
|
||||
@ -254,10 +254,9 @@ bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent)
|
||||
return false;
|
||||
}
|
||||
|
||||
// process a task monitoring message:
|
||||
// update existing RemoteTask, create a new one,
|
||||
// or (if msg.stages is empty) delete an existing one
|
||||
void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg)
|
||||
// process a task description message:
|
||||
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
|
||||
void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor::TaskDescription &msg)
|
||||
{
|
||||
Q_D(TaskListModel);
|
||||
|
||||
@ -266,8 +265,8 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg)
|
||||
bool created = it_inserted.second;
|
||||
RemoteTaskModel*& remote_task = it_inserted.first->second;
|
||||
|
||||
// empty stages list indicates, that this remote task is not available anymore
|
||||
if (msg.stages.empty()) {
|
||||
// empty list indicates, that this remote task is not available anymore
|
||||
if (msg.description.empty()) {
|
||||
if (!remote_task) { // task was already deleted locally
|
||||
// we can now remove it from remote_tasks_
|
||||
d->remote_tasks_.erase(it_inserted.first);
|
||||
@ -287,7 +286,8 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg)
|
||||
if (!remote_task)
|
||||
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
|
||||
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)
|
||||
{
|
||||
// TODO
|
||||
|
||||
@ -37,7 +37,8 @@
|
||||
#pragma once
|
||||
|
||||
#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 <QAbstractItemModel>
|
||||
@ -89,8 +90,10 @@ public:
|
||||
|
||||
bool removeRows(int row, int count, const QModelIndex &parent) override;
|
||||
|
||||
/// process an incoming task message - only call in Qt's main loop
|
||||
void processTaskMessage(const moveit_task_constructor::Task &msg);
|
||||
/// process an incoming task description message - only call in Qt's main loop
|
||||
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
|
||||
void processSolutionMessage(const moveit_task_constructor::Solution &msg);
|
||||
|
||||
|
||||
@ -49,14 +49,13 @@ TaskListModelCache &TaskListModelCache::instance()
|
||||
return instance_;
|
||||
}
|
||||
|
||||
TaskListModelPtr TaskListModelCache::getModel(const QString &task_monitor_topic,
|
||||
const QString &task_solution_topic)
|
||||
TaskListModelPtr TaskListModelCache::getModel(const std::string& ns)
|
||||
{
|
||||
if (task_monitor_topic.isEmpty() || task_solution_topic.isEmpty()) {
|
||||
if (ns.empty()) {
|
||||
return TaskListModelPtr();
|
||||
} else {
|
||||
// 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;
|
||||
|
||||
if (model.expired()) {
|
||||
|
||||
@ -45,7 +45,7 @@ namespace moveit_rviz_plugin {
|
||||
*
|
||||
* 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.
|
||||
* 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.
|
||||
*/
|
||||
@ -53,7 +53,7 @@ class TaskListModelCache : public QObject {
|
||||
Q_OBJECT
|
||||
|
||||
TaskListModelPtr global_model_;
|
||||
std::map<std::pair<QString, QString>, TaskListModelWeakPtr> cache_;
|
||||
std::map<std::string, TaskListModelWeakPtr> cache_;
|
||||
|
||||
/// class is singleton
|
||||
TaskListModelCache();
|
||||
@ -68,8 +68,7 @@ public:
|
||||
static TaskListModelCache& instance();
|
||||
|
||||
/// get TaskListModel for a TaskDisplay
|
||||
TaskListModelPtr getModel(const QString& task_monitor_topic,
|
||||
const QString& task_solution_topic);
|
||||
TaskListModelPtr getModel(const std::__cxx11::string &ns);
|
||||
|
||||
/// get global TaskListModel instance used for panels
|
||||
TaskListModelPtr getGlobalModel();
|
||||
|
||||
@ -431,8 +431,9 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::So
|
||||
return;
|
||||
}
|
||||
|
||||
if (!msg.model_id.empty() && msg.model_id != robot_model_->getName())
|
||||
ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", msg.model_id.c_str(),
|
||||
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.start_scene.robot_model_name .c_str(),
|
||||
robot_model_->getName().c_str());
|
||||
|
||||
trajectory_message_to_display_.reset();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user