mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
introducing process_id
The task id was not unique enough to distinguish different tasks. When a task publisher is killed and restarted, it usually comes up with the same task id. However, visualization doesn't notice this change and get confused / crashes when receiving task statistics and solutions.
This commit is contained in:
parent
e9363919e8
commit
f8952bb712
@ -48,11 +48,20 @@
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
namespace {
|
||||
std::string getProcessId() {
|
||||
char our_hostname[256] = {0};
|
||||
gethostname(our_hostname, sizeof(our_hostname)-1);
|
||||
return std::to_string(getpid()) + "@" + our_hostname;
|
||||
}
|
||||
}
|
||||
|
||||
class IntrospectionPrivate {
|
||||
public:
|
||||
IntrospectionPrivate(const Task &task)
|
||||
: nh_(std::string("~/") + task.id()) // topics + services are advertised in private namespace
|
||||
, task_(task)
|
||||
, process_id_(getProcessId())
|
||||
{
|
||||
resetMaps();
|
||||
task_description_publisher_ = nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
|
||||
@ -70,6 +79,7 @@ public:
|
||||
ros::NodeHandle nh_;
|
||||
/// associated task
|
||||
const Task &task_;
|
||||
const std::string process_id_;
|
||||
|
||||
/// publish task detailed description and current state
|
||||
ros::Publisher task_description_publisher_;
|
||||
@ -111,6 +121,7 @@ void Introspection::reset()
|
||||
{
|
||||
// send empty task description message to indicate reset
|
||||
::moveit_task_constructor_msgs::TaskDescription msg;
|
||||
msg.process_id = impl->process_id_;
|
||||
msg.id = impl->task_.id();
|
||||
impl->task_description_publisher_.publish(msg);
|
||||
|
||||
@ -121,6 +132,7 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution &msg,
|
||||
const SolutionBase &s)
|
||||
{
|
||||
s.fillMessage(msg, this);
|
||||
msg.process_id = impl->process_id_;
|
||||
msg.task_id = impl->task_.id();
|
||||
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
|
||||
}
|
||||
@ -140,9 +152,7 @@ void Introspection::publishAllSolutions(bool wait)
|
||||
std::cout << "publishing solution with cost: " << s.cost() << std::endl;
|
||||
msg.sub_solution.clear();
|
||||
msg.sub_trajectory.clear();
|
||||
s.fillMessage(msg, this);
|
||||
msg.task_id = impl->task_.id();
|
||||
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
|
||||
fillSolution(msg, s);
|
||||
impl->solution_publisher_.publish(msg);
|
||||
|
||||
if (wait) {
|
||||
@ -227,6 +237,7 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio
|
||||
impl->task_.stages()->traverseRecursively(stageProcessor);
|
||||
|
||||
msg.id = impl->task_.id();
|
||||
msg.process_id = impl->process_id_;
|
||||
return msg;
|
||||
}
|
||||
|
||||
@ -248,6 +259,7 @@ moveit_task_constructor_msgs::TaskStatistics& Introspection::fillTaskStatistics(
|
||||
impl->task_.stages()->traverseRecursively(stageProcessor);
|
||||
|
||||
msg.id = impl->task_.id();
|
||||
msg.process_id = impl->process_id_;
|
||||
return msg;
|
||||
}
|
||||
|
||||
|
@ -1,3 +1,6 @@
|
||||
# id of generating process
|
||||
string process_id
|
||||
|
||||
# id of associated task
|
||||
string task_id
|
||||
|
||||
|
@ -1,3 +1,6 @@
|
||||
# id of generating process
|
||||
string process_id
|
||||
|
||||
# unique ID of this task
|
||||
string id
|
||||
|
||||
|
@ -1,3 +1,6 @@
|
||||
# id of generating process
|
||||
string process_id
|
||||
|
||||
# unique of this task
|
||||
string id
|
||||
|
||||
|
@ -155,28 +155,30 @@ void TaskDisplay::changedRobotDescription()
|
||||
loadRobotModel();
|
||||
}
|
||||
|
||||
void TaskDisplay::taskDescriptionCB(const ros::MessageEvent<const moveit_task_constructor_msgs::TaskDescription> &event)
|
||||
inline std::string getUniqueId(const std::string& process_id, const std::string& task_id)
|
||||
{
|
||||
const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||
return process_id + "/" + task_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]() {
|
||||
task_list_model_->processTaskDescriptionMessage(id, *msg);
|
||||
});
|
||||
}
|
||||
|
||||
void TaskDisplay::taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor_msgs::TaskStatistics> &event)
|
||||
void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg)
|
||||
{
|
||||
const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||
const std::string id = getUniqueId(msg->process_id, msg->id);
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
task_list_model_->processTaskStatisticsMessage(id, *msg);
|
||||
});
|
||||
}
|
||||
|
||||
void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_constructor_msgs::Solution> &event)
|
||||
void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg)
|
||||
{
|
||||
const moveit_task_constructor_msgs::SolutionConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
||||
const std::string id = getUniqueId(msg->process_id, msg->task_id);
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
|
||||
trajectory_visual_->showTrajectory(s);
|
||||
|
@ -98,9 +98,9 @@ private Q_SLOTS:
|
||||
void onTasksRemoved(const QModelIndex& parent, int first, int last);
|
||||
void onTaskDataChanged(const QModelIndex &topLeft, const QModelIndex &bottomRight);
|
||||
|
||||
void taskDescriptionCB(const ros::MessageEvent<moveit_task_constructor_msgs::TaskDescription const>& event);
|
||||
void taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor_msgs::TaskStatistics> &event);
|
||||
void taskSolutionCB(const ros::MessageEvent<moveit_task_constructor_msgs::Solution const>& event);
|
||||
void taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg);
|
||||
void taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg);
|
||||
void taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg);
|
||||
|
||||
protected:
|
||||
ros::Subscriber task_solution_sub;
|
||||
|
@ -213,8 +213,11 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
|
||||
bool created = it_inserted.second;
|
||||
RemoteTaskModel*& remote_task = it_inserted.first->second;
|
||||
|
||||
if (!msg.stages.empty() && remote_task && remote_task->taskFlags() & BaseTaskModel::IS_DESTROYED)
|
||||
if (!msg.stages.empty() && remote_task && (remote_task->taskFlags() & BaseTaskModel::IS_DESTROYED)) {
|
||||
removeModel(remote_task);
|
||||
delete remote_task;
|
||||
created = true; // re-create remote task after it was destroyed beforehand
|
||||
}
|
||||
|
||||
// empty list indicates, that this remote task is not available anymore
|
||||
if (msg.stages.empty()) {
|
||||
@ -245,11 +248,13 @@ void TaskListModel::processTaskStatisticsMessage(const std::string &id,
|
||||
const moveit_task_constructor_msgs::TaskStatistics &msg)
|
||||
{
|
||||
auto it = remote_tasks_.find(id);
|
||||
if (it == remote_tasks_.cend())
|
||||
return; // unkown task
|
||||
if (it == remote_tasks_.cend()) {
|
||||
ROS_WARN("unknown task: %s", id.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
RemoteTaskModel* remote_task = it->second;
|
||||
if (!remote_task)
|
||||
if (!remote_task || (remote_task->taskFlags() & RemoteTaskModel::IS_DESTROYED))
|
||||
return; // task is not in use anymore
|
||||
|
||||
remote_task->processStageStatistics(msg.stages);
|
||||
|
Loading…
Reference in New Issue
Block a user