[MSA] Simplify loading of new SRDF (#1102)

moveit · May 31, 2022 · 5da2ead · 5da2ead
1 parent 37ecd0b
commit 5da2ead
Show file tree

Hide file tree

Showing 8 changed files with 5 additions and 59 deletions.
diff --git a/moveit_setup_assistant/moveit_setup_assistant/src/setup_assistant_widget.cpp b/moveit_setup_assistant/moveit_setup_assistant/src/setup_assistant_widget.cpp
@@ -243,7 +243,7 @@ void SetupAssistantWidget::moveToScreen(const int index)
 }
 
 // ******************************************************************************************
-// Ping ROS on internval
+// Ping ROS on interval
 // ******************************************************************************************
 void SetupAssistantWidget::updateTimer()
 {

diff --git a/...up_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp b/...up_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp
@@ -53,16 +53,13 @@ class StartScreen : public moveit_setup_framework::SetupStep
   std::string getURDFPath();
   std::string getXacroArgs();
   std::string getPackagePath();
-  std::string getURDFName();
 
   bool isXacroFile();
 
   void loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args);
 
   void loadExisting(const std::string& package_path);
 
-  void setSRDFFile(const std::string& srdf_string);
-
 protected:
   std::shared_ptr<moveit_setup_framework::PackageSettingsConfig> package_settings_;
   std::shared_ptr<moveit_setup_framework::SRDFConfig> srdf_config_;

diff --git a/...stant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.hpp b/...stant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.hpp
@@ -132,9 +132,6 @@ private Q_SLOTS:
 
   /// Load existing package files
   bool loadExistingFiles();
-
-  /// Loads sensors_3d yaml file
-  bool load3DSensorsFile();
 };
 
 // ******************************************************************************************

diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen.cpp
@@ -48,16 +48,7 @@ void StartScreen::onInit()
 void StartScreen::loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args)
 {
   urdf_config_->loadFromPath(urdf_file_path, xacro_args);
-}
-
-void StartScreen::setSRDFFile(const std::string& srdf_string)
-{
-  srdf_config_->loadSRDFString(srdf_string);
-}
-
-std::string StartScreen::getURDFName()
-{
-  return urdf_config_->getName();
+  srdf_config_->updateRobotModel();
 }
 
 std::string StartScreen::getURDFPath()

diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp
@@ -281,8 +281,6 @@ void StartScreenWidget::loadFilesClick()
   if (create_new_package_)
   {
     result = loadNewFiles();
-    // Load 3d_sensors config file
-    // load3DSensorsFile();
   }
   else
   {
@@ -416,24 +414,6 @@ bool StartScreenWidget::loadNewFiles()
   progress_bar_->setValue(50);
   QApplication::processEvents();
 
-  // Create blank SRDF file
-  const std::string blank_srdf = "<?xml version='1.0'?><robot name='" + setup_step_.getURDFName() +
-                                 "'></"
-                                 "robot>";
-  try
-  {
-    setup_step_.setSRDFFile(blank_srdf);
-  }
-  catch (const std::runtime_error& e)
-  {
-    QMessageBox::warning(this, "Error Loading SRDF to parameter server", QString(e.what()));
-    return false;
-  }
-
-  // Progress Indicator
-  progress_bar_->setValue(60);
-  QApplication::processEvents();
-
   // DONE LOADING --------------------------------------------------------------------------
 
   // Call a function that enables navigation

diff --git a/...etup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp b/...etup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp
@@ -70,9 +70,6 @@ class SRDFConfig : public SetupConfig
   void loadPrevious(const std::string& package_path, const YAML::Node& node) override;
   YAML::Node saveToYaml() const override;
 
-  /// Load SRDF String
-  void loadSRDFString(const std::string& srdf_string);
-
   /// Load SRDF File
   void loadSRDFFile(const std::string& package_path, const std::string& relative_path);
   void loadSRDFFile(const std::string& srdf_file_path);

diff --git a/moveit_setup_assistant/moveit_setup_framework/src/package_settings_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/package_settings_config.cpp
@@ -144,7 +144,7 @@ void PackageSettingsConfig::loadExisting(const std::string& package_path)
       config_data_->get(name)->loadPrevious(config_pkg_path_, title_node[yaml_key]);
     }
   }
-  catch (YAML::ParserException& e)  // Catch errors
+  catch (YAML::ParserException& e)  // Catch errors, translate to runtime_error
   {
     throw std::runtime_error("Error parsing " + config_path + ": " + e.what());
   }

diff --git a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp
@@ -69,24 +69,8 @@ void SRDFConfig::loadURDFModel()
 
   auto urdf_config = config_data_->get<URDFConfig>("urdf");
   urdf_model_ = urdf_config->getModelPtr();
-}
-
-void SRDFConfig::loadSRDFString(const std::string& srdf_string)
-{
-  loadURDFModel();
-
-  // Verify that file is in correct format / not an XACRO by loading into robot model
-  if (!srdf_.initString(*urdf_model_, srdf_string))
-  {
-    throw std::runtime_error("SRDF file not a valid semantic robot description model.");
-  }
-
-  // Set parameter
-  parent_node_->set_parameter(rclcpp::Parameter("robot_description_semantic", srdf_string));
-
-  updateRobotModel();
-
-  RCLCPP_INFO_STREAM((*logger_), "Robot semantic model successfully loaded.");
+  srdf_.robot_name_ = urdf_model_->getName();
+  parent_node_->set_parameter(rclcpp::Parameter("robot_description_semantic", srdf_.getSRDFString()));
 }
 
 void SRDFConfig::loadSRDFFile(const std::string& package_path, const std::string& relative_path)