Rename TaskPrivate::id -> TaskPrivate::ns

TaskPrivate's id_ actually served as a namespace parameter.
This commit is contained in:
Robert Haschke 2020-10-22 18:14:27 +02:00
parent be270cb574
commit b071a059f9
11 changed files with 38 additions and 74 deletions

View File

@ -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);

View File

@ -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_;

View File

@ -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;
}

View File

@ -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_;

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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;

View File

@ -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

View File

@ -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);

View File

@ -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; });