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:
Robert Haschke 2018-01-11 16:01:44 +01:00
parent e9363919e8
commit f8952bb712
7 changed files with 47 additions and 19 deletions

View File

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

View File

@ -1,3 +1,6 @@
# id of generating process
string process_id
# id of associated task
string task_id

View File

@ -1,3 +1,6 @@
# id of generating process
string process_id
# unique ID of this task
string id

View File

@ -1,3 +1,6 @@
# id of generating process
string process_id
# unique of this task
string id

View File

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

View File

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

View File

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