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
|
# 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})
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
|
|||||||
@ -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
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
|
# 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
|
||||||
|
|
||||||
@ -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
|
||||||
|
|||||||
@ -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
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
|
string id
|
||||||
|
|
||||||
# list of all stages, including the task stage itself
|
# list of all stages, including the task stage itself
|
||||||
Stage[] stages
|
StageStatistics[] stages
|
||||||
@ -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)
|
||||||
{
|
{
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
92
src/task.cpp
92
src/task.cpp
@ -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,27 +224,14 @@ 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;
|
|
||||||
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();
|
const StagePrivate *simpl = stage.pimpl();
|
||||||
|
|
||||||
moveit_task_constructor::Stage s; // create new Stage msg
|
fillInterfaceList(s.received_starts, simpl->starts());
|
||||||
s.id = stage_to_id_map.size();
|
fillInterfaceList(s.received_ends, simpl->ends());
|
||||||
stage_to_id_map[&stage] = s.id;
|
fillInterfaceList(s.generated_starts, simpl->nextStarts());
|
||||||
s.name = stage.name();
|
fillInterfaceList(s.generated_ends, simpl->prevEnds());
|
||||||
s.flags = stage.pimpl()->interfaceFlags();
|
|
||||||
auto it = stage_to_id_map.find(simpl->parent());
|
|
||||||
assert (it != stage_to_id_map.cend());
|
|
||||||
s.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());
|
|
||||||
|
|
||||||
// insert solution IDs
|
// insert solution IDs
|
||||||
Stage::SolutionProcessor solutionProcessor = [&s](const SolutionBase& solution) {
|
Stage::SolutionProcessor solutionProcessor = [&s](const SolutionBase& solution) {
|
||||||
@ -258,16 +245,73 @@ moveit_task_constructor::Task& Task::fillMessage(moveit_task_constructor::Task &
|
|||||||
return true;
|
return true;
|
||||||
};
|
};
|
||||||
stage.processFailures(solutionProcessor);
|
stage.processFailures(solutionProcessor);
|
||||||
|
}
|
||||||
|
|
||||||
// finally store in msg.stages
|
moveit_task_constructor::TaskDescription& Task::fillTaskDescription(moveit_task_constructor::TaskDescription &msg) const
|
||||||
msg.stages.push_back(std::move(s));
|
{
|
||||||
|
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::StageDescription desc;
|
||||||
|
moveit_task_constructor::StageStatistics stat;
|
||||||
|
desc.id = stat.id = stage_to_id_map.size();
|
||||||
|
stage_to_id_map[&stage] = stat.id;
|
||||||
|
|
||||||
|
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());
|
||||||
|
desc.parent_id = stat.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));
|
||||||
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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());
|
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)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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);
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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); });
|
||||||
} 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);
|
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 {
|
||||||
|
setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel");
|
||||||
|
}
|
||||||
|
|
||||||
|
// listen to task solutions
|
||||||
boost::function<void(const moveit_task_constructor::SolutionConstPtr &)> solCB
|
boost::function<void(const moveit_task_constructor::SolutionConstPtr &)> solCB
|
||||||
([this](const moveit_task_constructor::SolutionConstPtr &msg){
|
([this](const moveit_task_constructor::SolutionConstPtr &msg){
|
||||||
mainloop_jobs_.addJob([this, msg]() { task_model_->processSolutionMessage(*msg); });
|
mainloop_jobs_.addJob([this, msg]() {
|
||||||
|
if (task_model_) task_model_->processSolutionMessage(*msg);
|
||||||
// TODO: use already processed trajectory (e.g. by ID)
|
// TODO: use already processed trajectory (e.g. by ID)
|
||||||
mainloop_jobs_.addJob([this, msg]() {trajectory_visual_->showTrajectory(*msg);});
|
trajectory_visual_->showTrajectory(*msg);
|
||||||
});
|
});
|
||||||
task_solution_sub = update_nh_.subscribe(task_solution_topic_property_->getStdString(), 2, solCB);
|
});
|
||||||
|
task_solution_sub = update_nh_.subscribe(solution_topic, 2, solCB);
|
||||||
|
|
||||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
|
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void TaskDisplay::changedTaskMonitorTopic()
|
|
||||||
{
|
|
||||||
task_monitor_sub.shutdown();
|
|
||||||
updateTaskListModel();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskDisplay::changedTaskSolutionTopic()
|
void TaskDisplay::changedTaskSolutionTopic()
|
||||||
{
|
{
|
||||||
|
task_description_sub.shutdown();
|
||||||
|
task_statistics_sub.shutdown();
|
||||||
task_solution_sub.shutdown();
|
task_solution_sub.shutdown();
|
||||||
updateTaskListModel();
|
updateTaskListModel();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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()) {
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user