Upgrade to tesseract version 0.20.0

tesseract-robotics · Sep 30, 2023 · 47a5362 · 47a5362
1 parent e4577f1
commit 47a5362
Show file tree

Hide file tree

Showing 6 changed files with 23 additions and 66 deletions.
diff --git a/dependencies.rosinstall b/dependencies.rosinstall
@@ -5,19 +5,19 @@
 - git:
     local-name: tesseract
     uri: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.19.1
+    version: 0.20.0
 - git:
     local-name: trajopt
     uri: https://github.com/tesseract-robotics/trajopt.git
     version: 0.6.1
 - git:
     local-name: tesseract_planning
     uri: https://github.com/tesseract-robotics/tesseract_planning.git
-    version: 0.19.0
+    version: 0.20.0
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.19.0
+    version: 0.20.0
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git

diff --git a/dependencies_with_ext.rosinstall b/dependencies_with_ext.rosinstall
@@ -9,19 +9,19 @@
 - git:
     local-name: tesseract
     uri: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.19.1
+    version: 0.20.0
 - git:
     local-name: trajopt
     uri: https://github.com/tesseract-robotics/trajopt_ros.git
-    version: 0.6.0
+    version: 0.6.1
 - git:
     local-name: tesseract_planning
     uri: https://github.com/tesseract-robotics/tesseract_planning.git
-    version: 0.19.0
+    version: 0.20.0
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.19.0
+    version: 0.20.0
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git

diff --git a/tesseract_planning_server/include/tesseract_planning_server/tesseract_planning_server.h b/tesseract_planning_server/include/tesseract_planning_server/tesseract_planning_server.h
@@ -51,15 +51,9 @@ class TesseractPlanningServer
 
   static const std::string DEFAULT_GET_MOTION_PLAN_ACTION;  // "/tesseract_get_motion_plan"
 
-  TesseractPlanningServer(const std::string& robot_description,
-                          std::string input_key,
-                          std::string output_key,
-                          std::string name);
+  TesseractPlanningServer(const std::string& robot_description, std::string name);
 
-  TesseractPlanningServer(tesseract_environment::Environment::UPtr env,
-                          std::string input_key,
-                          std::string output_key,
-                          std::string name);
+  TesseractPlanningServer(tesseract_environment::Environment::UPtr env, std::string name);
 
   ~TesseractPlanningServer() = default;
   TesseractPlanningServer(const TesseractPlanningServer&) = delete;
@@ -96,12 +90,6 @@ class TesseractPlanningServer
   /** @brief The task planning server */
   tesseract_planning::TaskComposerServer::UPtr planning_server_;
 
-  /** @brief The input key */
-  std::string input_key_;
-
-  /** @brief The output key */
-  std::string output_key_;
-
   /** @brief The motion planning action server */
   actionlib::SimpleActionServer<tesseract_msgs::GetMotionPlanAction> motion_plan_server_;
 

diff --git a/tesseract_planning_server/launch/planning_server.launch b/tesseract_planning_server/launch/planning_server.launch
@@ -9,8 +9,6 @@
   <arg name="cache_size" default="5"/>
   <arg name="cache_refresh_rate" default="0.1"/>
   <arg name="task_composer_config" default="$(find tesseract_task_composer)/config/task_composer_plugins.yaml"/>
-  <arg name="input_key" default="input_data"/>
-  <arg name="output_key" default="output_data"/>
 
   <node pkg="tesseract_planning_server" type="tesseract_planning_server_node" name="tesseract_planning_server">
     <param name="robot_description" type="string" value="$(arg robot_description)"/>
@@ -22,8 +20,6 @@
     <param name="cache_size" type="int" value="$(arg cache_size)"/>
     <param name="cache_refresh_rate" type="double" value="$(arg cache_refresh_rate)"/>
     <param name="task_composer_config" type="string" value="$(arg task_composer_config)"/>
-    <param name="input_key" type="string" value="$(arg input_key)"/>
-    <param name="output_key" type="string" value="$(arg output_key)"/>
   </node>
 
 </launch>
diff --git a/tesseract_planning_server/src/tesseract_planning_server.cpp b/tesseract_planning_server/src/tesseract_planning_server.cpp
@@ -67,6 +67,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_common/timer.h>
 
 using tesseract_common::Serialization;
+using tesseract_planning::InstructionPoly;
+using tesseract_planning::PlanningTaskComposerProblem;
+using tesseract_planning::TaskComposerDataStorage;
 using tesseract_rosutils::processMsg;
 
 static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask";
@@ -79,17 +82,12 @@ namespace tesseract_planning_server
 {
 const std::string TesseractPlanningServer::DEFAULT_GET_MOTION_PLAN_ACTION = "tesseract_get_motion_plan";
 
-TesseractPlanningServer::TesseractPlanningServer(const std::string& robot_description,
-                                                 std::string input_key,
-                                                 std::string output_key,
-                                                 std::string name)
+TesseractPlanningServer::TesseractPlanningServer(const std::string& robot_description, std::string name)
   : nh_("~")
   , monitor_(std::make_shared<tesseract_monitoring::ROSEnvironmentMonitor>(robot_description, name))
   , environment_cache_(std::make_shared<tesseract_environment::DefaultEnvironmentCache>(monitor_->getEnvironment()))
   , profiles_(std::make_shared<tesseract_planning::ProfileDictionary>())
   , planning_server_(std::make_unique<tesseract_planning::TaskComposerServer>())
-  , input_key_(std::move(input_key))
-  , output_key_(std::move(output_key))
   , motion_plan_server_(nh_,
                         DEFAULT_GET_MOTION_PLAN_ACTION,
                         boost::bind(&TesseractPlanningServer::onMotionPlanningCallback, this, _1),
@@ -100,17 +98,12 @@ TesseractPlanningServer::TesseractPlanningServer(const std::string& robot_descri
   ctor();
 }
 
-TesseractPlanningServer::TesseractPlanningServer(tesseract_environment::Environment::UPtr env,
-                                                 std::string input_key,
-                                                 std::string output_key,
-                                                 std::string name)
+TesseractPlanningServer::TesseractPlanningServer(tesseract_environment::Environment::UPtr env, std::string name)
   : nh_("~")
   , monitor_(std::make_shared<tesseract_monitoring::ROSEnvironmentMonitor>(std::move(env), name))
   , environment_cache_(std::make_shared<tesseract_environment::DefaultEnvironmentCache>(monitor_->getEnvironment()))
   , profiles_(std::make_shared<tesseract_planning::ProfileDictionary>())
   , planning_server_(std::make_unique<tesseract_planning::TaskComposerServer>())
-  , input_key_(std::move(input_key))
-  , output_key_(std::move(output_key))
   , motion_plan_server_(nh_,
                         DEFAULT_GET_MOTION_PLAN_ACTION,
                         boost::bind(&TesseractPlanningServer::onMotionPlanningCallback, this, _1),
@@ -191,21 +184,17 @@ void TesseractPlanningServer::onMotionPlanningCallback(const tesseract_msgs::Get
   }
 
   auto problem = std::make_unique<tesseract_planning::PlanningTaskComposerProblem>(goal->request.name);
-  auto data_storage = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
-
   try
   {
-    auto ci = Serialization::fromArchiveStringXML<tesseract_planning::InstructionPoly>(goal->request.instructions)
-                  .as<tesseract_planning::CompositeInstruction>();
-    data_storage->setData(input_key_, ci);
+    problem->input_instruction =
+        Serialization::fromArchiveStringXML<tesseract_planning::InstructionPoly>(goal->request.instructions);
   }
   catch (const std::exception& e)
   {
     result.response.successful = false;
     std::ostringstream oss;
     oss << "Failed to deserialize program instruction with error: '" << e.what() << "'!" << std::endl;
-    oss << "   Make sure the program was serialized from an Instruction type and not a CompositeInstruction type."
-        << std::endl;
+    oss << "   Make sure the program was serialized from an InstructionPoly type." << std::endl;
     ROS_ERROR_STREAM(oss.str());
     motion_plan_server_.setSucceeded(result);
     return;
@@ -234,7 +223,7 @@ void TesseractPlanningServer::onMotionPlanningCallback(const tesseract_msgs::Get
   tesseract_common::Timer timer;
   timer.start();
   tesseract_planning::TaskComposerFuture::UPtr plan_future =
-      planning_server_->run(std::move(problem), std::move(data_storage), executor_name);
+      planning_server_->run(std::move(problem), std::make_unique<TaskComposerDataStorage>(), executor_name);
   plan_future->wait();  // Wait for results
   timer.stop();
 
@@ -243,10 +232,8 @@ void TesseractPlanningServer::onMotionPlanningCallback(const tesseract_msgs::Get
   {
     try
     {
-      // Get Task
-      const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->problem->name);
-
       // Save dot graph
+      const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->problem->name);
       std::stringstream dotgraph;
       task.dump(dotgraph, nullptr, plan_future->context->task_infos.getInfoMap());
       result.response.dotgraph = dotgraph.str();
@@ -261,7 +248,8 @@ void TesseractPlanningServer::onMotionPlanningCallback(const tesseract_msgs::Get
 
   try
   {
-    tesseract_common::AnyPoly results = plan_future->context->data_storage->getData(output_key_);
+    const tesseract_planning::TaskComposerNode& task = planning_server_->getTask(plan_future->context->problem->name);
+    tesseract_common::AnyPoly results = plan_future->context->data_storage->getData(task.getOutputKeys().front());
     result.response.results = Serialization::toArchiveStringXML<tesseract_planning::InstructionPoly>(
         results.as<tesseract_planning::CompositeInstruction>());
   }

diff --git a/tesseract_planning_server/src/tesseract_planning_server_node.cpp b/tesseract_planning_server/src/tesseract_planning_server_node.cpp
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_planning_server/tesseract_planning_server.h>
 
 using namespace tesseract_environment;
+using tesseract_planning_server::TesseractPlanningServer;
 
 const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; /**< Default ROS parameter for robot description */
 static std::shared_ptr<tesseract_planning_server::TesseractPlanningServer> planning_server;
@@ -51,8 +52,6 @@ int main(int argc, char** argv)
   std::string monitor_namespace;
   std::string monitored_namespace;
   std::string task_composer_config;
-  std::string input_key;
-  std::string output_key;
   bool publish_environment{ false };
   int cache_size{ 5 };
   double cache_refresh_rate{ 0.1 };
@@ -63,28 +62,14 @@ int main(int argc, char** argv)
     return 1;
   }
 
-  if (!pnh.getParam("input_key", input_key))
-  {
-    ROS_ERROR("Missing required parameter input_key!");
-    return 1;
-  }
-
-  if (!pnh.getParam("output_key", output_key))
-  {
-    ROS_ERROR("Missing required parameter output_key!");
-    return 1;
-  }
-
   pnh.param<std::string>("monitored_namespace", monitored_namespace, "");
   pnh.param<std::string>("robot_description", robot_description, ROBOT_DESCRIPTION_PARAM);
   pnh.param<bool>("publish_environment", publish_environment, publish_environment);
   pnh.param<int>("cache_size", cache_size, cache_size);
   pnh.param<double>("cache_refresh_rate", cache_refresh_rate, cache_refresh_rate);
   pnh.param<std::string>("task_composer_config", task_composer_config, task_composer_config);
 
-  planning_server = std::make_shared<tesseract_planning_server::TesseractPlanningServer>(
-      robot_description, input_key, output_key, monitor_namespace);
-
+  planning_server = std::make_shared<TesseractPlanningServer>(robot_description, monitor_namespace);
   planning_server->getEnvironmentCache().setCacheSize(cache_size);
 
   if (publish_environment)