From 4bf5456d068ca81d8ce1e271c3d11c34eac3a2e3 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Wed, 29 Jul 2020 18:11:47 +0900 Subject: [PATCH 1/2] Add creator name to sub-trajectory --- core/src/storage.cpp | 1 + msgs/msg/SubTrajectory.msg | 3 +++ 2 files changed, 4 insertions(+) diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 2db0d9a50..5062ee25e 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -160,6 +160,7 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Int if (trajectory()) trajectory()->getRobotTrajectoryMsg(t.trajectory); + t.creator_name = this->creator()->name(); this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff); } diff --git a/msgs/msg/SubTrajectory.msg b/msgs/msg/SubTrajectory.msg index 10e3fde8a..704c745d4 100644 --- a/msgs/msg/SubTrajectory.msg +++ b/msgs/msg/SubTrajectory.msg @@ -6,3 +6,6 @@ moveit_msgs/RobotTrajectory trajectory # planning scene of end state as diff w.r.t. start state moveit_msgs/PlanningScene scene_diff + +# Name of the stage that created this trajectory +string creator_name From a7fb545efe623148a69a064bdf4c16cd1f9dab9f Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Wed, 29 Jul 2020 21:53:56 +0900 Subject: [PATCH 2/2] Move to SolutionInfo --- core/src/storage.cpp | 2 +- msgs/msg/SolutionInfo.msg | 3 +++ msgs/msg/SubTrajectory.msg | 3 --- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 5062ee25e..16f76df31 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -146,6 +146,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In info.comment = this->comment(); const Introspection* ci = introspection; info.stage_id = ci ? ci->stageId(this->creator()->me()) : 0; + info.creator_name = this->creator()->name(); const auto& markers = this->markers(); info.markers.resize(markers.size()); @@ -160,7 +161,6 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Int if (trajectory()) trajectory()->getRobotTrajectoryMsg(t.trajectory); - t.creator_name = this->creator()->name(); this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff); } diff --git a/msgs/msg/SolutionInfo.msg b/msgs/msg/SolutionInfo.msg index c3346ab53..62303c80f 100644 --- a/msgs/msg/SolutionInfo.msg +++ b/msgs/msg/SolutionInfo.msg @@ -10,5 +10,8 @@ string comment # id of stage that created this trajectory uint32 stage_id +# name of the stage that created this trajectory +string creator_name + # markers, e.g. providing additional hints or illustrating failure visualization_msgs/Marker[] markers diff --git a/msgs/msg/SubTrajectory.msg b/msgs/msg/SubTrajectory.msg index 704c745d4..10e3fde8a 100644 --- a/msgs/msg/SubTrajectory.msg +++ b/msgs/msg/SubTrajectory.msg @@ -6,6 +6,3 @@ moveit_msgs/RobotTrajectory trajectory # planning scene of end state as diff w.r.t. start state moveit_msgs/PlanningScene scene_diff - -# Name of the stage that created this trajectory -string creator_name