From 5da2ead3a78da462b948d3e85fcf1f59b0fb7447 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Tue, 15 Mar 2022 09:16:25 -0400 Subject: [PATCH] [MSA] Simplify loading of new SRDF (#1102) --- .../src/setup_assistant_widget.cpp | 2 +- .../start_screen.hpp | 3 --- .../start_screen_widget.hpp | 3 --- .../src/start_screen.cpp | 11 +--------- .../src/start_screen_widget.cpp | 20 ------------------- .../data/srdf_config.hpp | 3 --- .../src/package_settings_config.cpp | 2 +- .../src/srdf_config.cpp | 20 ++----------------- 8 files changed, 5 insertions(+), 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 index b4fe09caff..acaf9ff029 100644 --- 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/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp index 78cd612830..6b4061172f 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp @@ -53,7 +53,6 @@ class StartScreen : public moveit_setup_framework::SetupStep std::string getURDFPath(); std::string getXacroArgs(); std::string getPackagePath(); - std::string getURDFName(); bool isXacroFile(); @@ -61,8 +60,6 @@ class StartScreen : public moveit_setup_framework::SetupStep void loadExisting(const std::string& package_path); - void setSRDFFile(const std::string& srdf_string); - protected: std::shared_ptr package_settings_; std::shared_ptr srdf_config_; diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.hpp b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.hpp index 873edd7cdc..9d6fe11857 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.hpp +++ b/moveit_setup_assistant/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 index db799ade57..1ad93bc24b 100644 --- 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 index b6a921c4f7..14c965b100 100644 --- 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 = ""; - 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/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp index 8e68a4cec9..9241a750b8 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp +++ b/moveit_setup_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 index 4734de3e9a..2dc8e203db 100644 --- 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 index c61af23367..14d585413d 100644 --- 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("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)