mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Rename TaskPrivate::id -> TaskPrivate::ns
TaskPrivate's id_ actually served as a namespace parameter.
This commit is contained in:
parent
be270cb574
commit
b071a059f9
@ -78,14 +78,12 @@ public:
|
||||
createPlanner(const moveit::core::RobotModelConstPtr& model, const std::string& ns = "move_group",
|
||||
const std::string& planning_plugin_param_name = "planning_plugin",
|
||||
const std::string& adapter_plugins_param_name = "request_adapters");
|
||||
Task(const std::string& id = "", bool introspection = true,
|
||||
Task(const std::string& ns = "", bool introspection = true,
|
||||
ContainerBase::pointer&& container = std::make_unique<SerialContainer>("task pipeline"));
|
||||
Task(Task&& other); // NOLINT(performance-noexcept-move-constructor)
|
||||
Task& operator=(Task&& other); // NOLINT(performance-noexcept-move-constructor)
|
||||
~Task() override;
|
||||
|
||||
const std::string& id() const;
|
||||
|
||||
const moveit::core::RobotModelConstPtr& getRobotModel() const;
|
||||
/// setting the robot model also resets the task
|
||||
void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
|
||||
|
||||
@ -53,15 +53,15 @@ class TaskPrivate : public WrapperBasePrivate
|
||||
friend class Task;
|
||||
|
||||
public:
|
||||
TaskPrivate(Task* me, const std::string& id);
|
||||
const std::string& id() const { return id_; }
|
||||
TaskPrivate(Task* me, const std::string& ns);
|
||||
const std::string& ns() const { return ns_; }
|
||||
const ContainerBase* stages() const;
|
||||
|
||||
protected:
|
||||
static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
|
||||
|
||||
private:
|
||||
std::string id_;
|
||||
std::string ns_;
|
||||
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
|
||||
moveit::core::RobotModelConstPtr robot_model_;
|
||||
bool preempt_requested_;
|
||||
|
||||
@ -63,7 +63,7 @@ class IntrospectionPrivate
|
||||
{
|
||||
public:
|
||||
IntrospectionPrivate(const TaskPrivate* task)
|
||||
: nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace
|
||||
: nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace
|
||||
, task_(task)
|
||||
, process_id_(getProcessId()) {
|
||||
task_description_publisher_ =
|
||||
@ -82,7 +82,6 @@ public:
|
||||
// send empty task description message to indicate reset
|
||||
::moveit_task_constructor_msgs::TaskDescription msg;
|
||||
msg.process_id = process_id_;
|
||||
msg.id = task_->id();
|
||||
task_description_publisher_.publish(msg);
|
||||
}
|
||||
|
||||
@ -144,7 +143,6 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, co
|
||||
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
|
||||
|
||||
msg.process_id = impl->process_id_;
|
||||
msg.task_id = impl->task_->id();
|
||||
}
|
||||
|
||||
void Introspection::publishSolution(const SolutionBase& s) {
|
||||
@ -242,7 +240,6 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription
|
||||
msg.stages.clear();
|
||||
impl->task_->stages()->traverseRecursively(stage_processor);
|
||||
|
||||
msg.id = impl->task_->id();
|
||||
msg.process_id = impl->process_id_;
|
||||
return msg;
|
||||
}
|
||||
@ -263,7 +260,6 @@ Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics&
|
||||
msg.stages.clear();
|
||||
impl->task_->stages()->traverseRecursively(stage_processor);
|
||||
|
||||
msg.id = impl->task_->id();
|
||||
msg.process_id = impl->process_id_;
|
||||
return msg;
|
||||
}
|
||||
|
||||
@ -71,8 +71,8 @@ std::string rosNormalizeName(const std::string& name) {
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
TaskPrivate::TaskPrivate(Task* me, const std::string& id)
|
||||
: WrapperBasePrivate(me, std::string()), id_(rosNormalizeName(id)), preempt_requested_(false) {}
|
||||
TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
|
||||
: WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {}
|
||||
|
||||
void swap(StagePrivate*& lhs, StagePrivate*& rhs) {
|
||||
// It only makes sense to swap pimpl instances of a Task!
|
||||
@ -104,11 +104,8 @@ const ContainerBase* TaskPrivate::stages() const {
|
||||
return children().empty() ? nullptr : static_cast<ContainerBase*>(children().front().get());
|
||||
}
|
||||
|
||||
Task::Task(const std::string& id, bool introspection, ContainerBase::pointer&& container)
|
||||
: WrapperBase(new TaskPrivate(this, id), std::move(container)) {
|
||||
if (!id.empty())
|
||||
stages()->setName(id);
|
||||
|
||||
Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& container)
|
||||
: WrapperBase(new TaskPrivate(this, ns), std::move(container)) {
|
||||
setTimeout(std::numeric_limits<double>::max());
|
||||
|
||||
// monitor state on commandline
|
||||
@ -360,10 +357,6 @@ void Task::setProperty(const std::string& name, const boost::any& value) {
|
||||
wrapped()->setProperty(name, value);
|
||||
}
|
||||
|
||||
const std::string& Task::id() const {
|
||||
return pimpl()->id();
|
||||
}
|
||||
|
||||
const core::RobotModelConstPtr& Task::getRobotModel() const {
|
||||
auto impl = pimpl();
|
||||
return impl->robot_model_;
|
||||
|
||||
@ -1,9 +1,6 @@
|
||||
# id of generating process
|
||||
# id of generating task
|
||||
string process_id
|
||||
|
||||
# id of associated task
|
||||
string task_id
|
||||
|
||||
# planning scene of start state
|
||||
moveit_msgs/PlanningScene start_scene
|
||||
|
||||
|
||||
@ -1,8 +1,5 @@
|
||||
# id of generating process
|
||||
# unique id of this task
|
||||
string process_id
|
||||
|
||||
# unique ID of this task
|
||||
string id
|
||||
|
||||
# list of all stages, including the task stage itself
|
||||
StageDescription[] stages
|
||||
|
||||
@ -1,8 +1,5 @@
|
||||
# id of generating process
|
||||
# unique id of generating task
|
||||
string process_id
|
||||
|
||||
# unique of this task
|
||||
string id
|
||||
|
||||
# list of all stages, including the task stage itself
|
||||
StageStatistics[] stages
|
||||
|
||||
@ -188,20 +188,10 @@ void TaskDisplay::changedRobotDescription() {
|
||||
loadRobotModel();
|
||||
}
|
||||
|
||||
inline std::string getUniqueId(const std::string& process_id, const std::string& task_id) {
|
||||
std::string id{ process_id };
|
||||
if (!task_id.empty()) {
|
||||
id += "/";
|
||||
id += task_id;
|
||||
}
|
||||
return id;
|
||||
}
|
||||
|
||||
void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) {
|
||||
const std::string id = getUniqueId(msg->process_id, msg->id);
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
mainloop_jobs_.addJob([this, msg]() {
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||
task_list_model_->processTaskDescriptionMessage(id, *msg);
|
||||
task_list_model_->processTaskDescriptionMessage(*msg);
|
||||
|
||||
// Start listening to other topics if this is the first description
|
||||
// Waiting for the description ensures we do not receive data that cannot be interpreted yet
|
||||
@ -215,18 +205,16 @@ void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDesc
|
||||
}
|
||||
|
||||
void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg) {
|
||||
const std::string id = getUniqueId(msg->process_id, msg->id);
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
mainloop_jobs_.addJob([this, msg]() {
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||
task_list_model_->processTaskStatisticsMessage(id, *msg);
|
||||
task_list_model_->processTaskStatisticsMessage(*msg);
|
||||
});
|
||||
}
|
||||
|
||||
void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg) {
|
||||
const std::string id = getUniqueId(msg->process_id, msg->task_id);
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
mainloop_jobs_.addJob([this, msg]() {
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
|
||||
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg);
|
||||
if (s)
|
||||
trajectory_visual_->showTrajectory(s, false);
|
||||
return;
|
||||
|
||||
@ -240,10 +240,9 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const {
|
||||
|
||||
// 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 std::string& id,
|
||||
const moveit_task_constructor_msgs::TaskDescription& msg) {
|
||||
// retrieve existing or insert new remote task for given id
|
||||
auto it_inserted = remote_tasks_.insert(std::make_pair(id, nullptr));
|
||||
void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg) {
|
||||
// retrieve existing or insert new remote task for given task id
|
||||
auto it_inserted = remote_tasks_.insert(std::make_pair(msg.process_id, nullptr));
|
||||
bool created = it_inserted.second;
|
||||
const auto& task_it = it_inserted.first;
|
||||
RemoteTaskModel*& remote_task = task_it->second;
|
||||
@ -275,17 +274,16 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
|
||||
|
||||
// insert newly created model into this' model instance
|
||||
if (created) {
|
||||
ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.id.c_str());
|
||||
ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.process_id.c_str());
|
||||
insertModel(remote_task, -1);
|
||||
}
|
||||
}
|
||||
|
||||
// process a task statistics message
|
||||
void TaskListModel::processTaskStatisticsMessage(const std::string& id,
|
||||
const moveit_task_constructor_msgs::TaskStatistics& msg) {
|
||||
auto it = remote_tasks_.find(id);
|
||||
void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg) {
|
||||
auto it = remote_tasks_.find(msg.process_id);
|
||||
if (it == remote_tasks_.cend()) {
|
||||
ROS_WARN("unknown task: %s", id.c_str());
|
||||
ROS_WARN("unknown task: %s", msg.process_id.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
@ -296,9 +294,8 @@ void TaskListModel::processTaskStatisticsMessage(const std::string& id,
|
||||
remote_task->processStageStatistics(msg.stages);
|
||||
}
|
||||
|
||||
DisplaySolutionPtr TaskListModel::processSolutionMessage(const std::string& id,
|
||||
const moveit_task_constructor_msgs::Solution& msg) {
|
||||
auto it = remote_tasks_.find(id);
|
||||
DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) {
|
||||
auto it = remote_tasks_.find(msg.process_id);
|
||||
if (it == remote_tasks_.cend())
|
||||
return DisplaySolutionPtr(); // unkown task
|
||||
|
||||
|
||||
@ -157,11 +157,11 @@ public:
|
||||
QVariant data(const QModelIndex& index, int role) const override;
|
||||
|
||||
/// process an incoming task description message - only call in Qt's main loop
|
||||
void processTaskDescriptionMessage(const std::string& id, const moveit_task_constructor_msgs::TaskDescription& msg);
|
||||
void processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg);
|
||||
/// process an incoming task description message - only call in Qt's main loop
|
||||
void processTaskStatisticsMessage(const std::string& id, const moveit_task_constructor_msgs::TaskStatistics& msg);
|
||||
void processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg);
|
||||
/// process an incoming solution message - only call in Qt's main loop
|
||||
DisplaySolutionPtr processSolutionMessage(const std::string& id, const moveit_task_constructor_msgs::Solution& msg);
|
||||
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg);
|
||||
|
||||
/// insert a TaskModel, pos is relative to modelCount()
|
||||
bool insertModel(BaseTaskModel* model, int pos = -1);
|
||||
|
||||
@ -55,10 +55,11 @@ protected:
|
||||
int num_inserts = 0;
|
||||
int num_updates = 0;
|
||||
|
||||
moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name) {
|
||||
moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name,
|
||||
const std::string& process_id = std::string()) {
|
||||
moveit_task_constructor_msgs::TaskDescription t;
|
||||
t.id = name;
|
||||
uint id = 0, root_id;
|
||||
t.process_id = process_id.empty() ? name : process_id;
|
||||
|
||||
moveit_task_constructor_msgs::StageDescription desc;
|
||||
desc.parent_id = id;
|
||||
@ -127,7 +128,7 @@ protected:
|
||||
SCOPED_TRACE("first i=" + std::to_string(i));
|
||||
num_inserts = 0;
|
||||
num_updates = 0;
|
||||
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||
model.processTaskDescriptionMessage(genMsg("first"));
|
||||
|
||||
if (i == 0)
|
||||
EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task
|
||||
@ -141,7 +142,7 @@ protected:
|
||||
SCOPED_TRACE("second i=" + std::to_string(i));
|
||||
num_inserts = 0;
|
||||
num_updates = 0;
|
||||
model.processTaskDescriptionMessage("2", genMsg("second")); // 1 notify for inserted task
|
||||
model.processTaskDescriptionMessage(genMsg("second")); // 1 notify for inserted task
|
||||
|
||||
if (i == 0)
|
||||
EXPECT_EQ(num_inserts, 1);
|
||||
@ -200,13 +201,13 @@ TEST_F(TaskListModelTest, threeChildren) {
|
||||
TEST_F(TaskListModelTest, visitedPopulate) {
|
||||
// first population without children
|
||||
children = 0;
|
||||
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||
model.processTaskDescriptionMessage(genMsg("first"));
|
||||
validate(model, { "first" }); // validation visits root node
|
||||
EXPECT_EQ(num_inserts, 1);
|
||||
|
||||
children = 3;
|
||||
num_inserts = 0;
|
||||
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||
model.processTaskDescriptionMessage(genMsg("first"));
|
||||
validate(model, { "first" });
|
||||
// second population with children should emit insert notifies for them
|
||||
EXPECT_EQ(num_inserts, 3);
|
||||
@ -215,7 +216,7 @@ TEST_F(TaskListModelTest, visitedPopulate) {
|
||||
|
||||
TEST_F(TaskListModelTest, deletion) {
|
||||
children = 3;
|
||||
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||
model.processTaskDescriptionMessage(genMsg("first"));
|
||||
auto m = model.getModel(model.index(0, 0)).first;
|
||||
int num_deletes = 0;
|
||||
QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; });
|
||||
|
||||
Loading…
Reference in New Issue
Block a user