mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Rename process_id -> task_id
This commit is contained in:
parent
b071a059f9
commit
c471879b08
@ -46,16 +46,19 @@
|
||||
#include <ros/service.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <sstream>
|
||||
#include <boost/bimap.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
namespace {
|
||||
std::string getProcessId() {
|
||||
std::string getTaskId(const TaskPrivate* task) {
|
||||
std::ostringstream oss;
|
||||
char our_hostname[256] = { 0 };
|
||||
gethostname(our_hostname, sizeof(our_hostname) - 1);
|
||||
return std::to_string(getpid()) + "@" + our_hostname;
|
||||
oss << our_hostname << "_" << getpid() << "_" << reinterpret_cast<std::size_t>(task);
|
||||
return oss.str();
|
||||
}
|
||||
} // namespace
|
||||
|
||||
@ -65,7 +68,7 @@ public:
|
||||
IntrospectionPrivate(const TaskPrivate* task)
|
||||
: nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace
|
||||
, task_(task)
|
||||
, process_id_(getProcessId()) {
|
||||
, task_id_(getTaskId(task)) {
|
||||
task_description_publisher_ =
|
||||
nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 2, true);
|
||||
// send reset message as early as possible to give subscribers time to see it
|
||||
@ -81,7 +84,7 @@ public:
|
||||
void indicateReset() {
|
||||
// send empty task description message to indicate reset
|
||||
::moveit_task_constructor_msgs::TaskDescription msg;
|
||||
msg.process_id = process_id_;
|
||||
msg.task_id = task_id_;
|
||||
task_description_publisher_.publish(msg);
|
||||
}
|
||||
|
||||
@ -96,7 +99,7 @@ public:
|
||||
ros::NodeHandle nh_;
|
||||
/// associated task
|
||||
const TaskPrivate* task_;
|
||||
const std::string process_id_;
|
||||
const std::string task_id_;
|
||||
|
||||
/// publish task detailed description and current state
|
||||
ros::Publisher task_description_publisher_;
|
||||
@ -142,7 +145,7 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, co
|
||||
s.fillMessage(msg, this);
|
||||
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) {
|
||||
@ -240,7 +243,7 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription
|
||||
msg.stages.clear();
|
||||
impl->task_->stages()->traverseRecursively(stage_processor);
|
||||
|
||||
msg.process_id = impl->process_id_;
|
||||
msg.task_id = impl->task_id_;
|
||||
return msg;
|
||||
}
|
||||
|
||||
@ -260,7 +263,7 @@ Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics&
|
||||
msg.stages.clear();
|
||||
impl->task_->stages()->traverseRecursively(stage_processor);
|
||||
|
||||
msg.process_id = impl->process_id_;
|
||||
msg.task_id = impl->task_id_;
|
||||
return msg;
|
||||
}
|
||||
} // namespace task_constructor
|
||||
|
@ -1,5 +1,5 @@
|
||||
# id of generating task
|
||||
string process_id
|
||||
string task_id
|
||||
|
||||
# planning scene of start state
|
||||
moveit_msgs/PlanningScene start_scene
|
||||
|
@ -1,5 +1,5 @@
|
||||
# unique id of this task
|
||||
string process_id
|
||||
string task_id
|
||||
|
||||
# list of all stages, including the task stage itself
|
||||
StageDescription[] stages
|
||||
|
@ -1,5 +1,5 @@
|
||||
# unique id of generating task
|
||||
string process_id
|
||||
string task_id
|
||||
|
||||
# list of all stages, including the task stage itself
|
||||
StageStatistics[] stages
|
||||
|
@ -242,7 +242,7 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const {
|
||||
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
|
||||
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));
|
||||
auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr));
|
||||
bool created = it_inserted.second;
|
||||
const auto& task_it = it_inserted.first;
|
||||
RemoteTaskModel*& remote_task = task_it->second;
|
||||
@ -274,16 +274,16 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_
|
||||
|
||||
// insert newly created model into this' model instance
|
||||
if (created) {
|
||||
ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.process_id.c_str());
|
||||
ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.task_id.c_str());
|
||||
insertModel(remote_task, -1);
|
||||
}
|
||||
}
|
||||
|
||||
// process a task statistics message
|
||||
void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg) {
|
||||
auto it = remote_tasks_.find(msg.process_id);
|
||||
auto it = remote_tasks_.find(msg.task_id);
|
||||
if (it == remote_tasks_.cend()) {
|
||||
ROS_WARN("unknown task: %s", msg.process_id.c_str());
|
||||
ROS_WARN("unknown task: %s", msg.task_id.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
@ -295,7 +295,7 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_m
|
||||
}
|
||||
|
||||
DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) {
|
||||
auto it = remote_tasks_.find(msg.process_id);
|
||||
auto it = remote_tasks_.find(msg.task_id);
|
||||
if (it == remote_tasks_.cend())
|
||||
return DisplaySolutionPtr(); // unkown task
|
||||
|
||||
|
@ -56,10 +56,10 @@ protected:
|
||||
int num_updates = 0;
|
||||
|
||||
moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name,
|
||||
const std::string& process_id = std::string()) {
|
||||
const std::string& task_id = std::string()) {
|
||||
moveit_task_constructor_msgs::TaskDescription t;
|
||||
uint id = 0, root_id;
|
||||
t.process_id = process_id.empty() ? name : process_id;
|
||||
t.task_id = task_id.empty() ? name : task_id;
|
||||
|
||||
moveit_task_constructor_msgs::StageDescription desc;
|
||||
desc.parent_id = id;
|
||||
|
Loading…
Reference in New Issue
Block a user