From f14312336716bec4b3c6a11cc0bcf21fd18cbf9b Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Wed, 2 Mar 2022 14:04:30 -0500 Subject: [PATCH] [MSA] SRDF Setup (#1057) --- moveit_setup_assistant/MIGRATION.md | 11 +- .../extra_generated_files_code.txt | 8 - .../extra_initialization_code.txt | 25 +- .../extra_template_variables.txt | 18 - .../src/perception_widget.cpp | 2 +- .../src/setup_assistant_widget.cpp | 4 + .../moveit_setup_framework/CMakeLists.txt | 2 + .../data/srdf_config.hpp | 51 +- .../qt/double_list_widget.hpp | 25 +- .../src/double_list_widget.cpp | 22 +- .../src/srdf_config.cpp | 38 +- .../moveit_setup_srdf_plugins/CMakeLists.txt | 17 + .../collision_matrix_model.hpp | 2 +- .../default_collisions.hpp | 20 +- .../end_effectors.hpp | 90 +++ .../end_effectors_widget.hpp | 36 +- .../group_edit_widget.hpp | 24 +- .../group_meta_config.hpp | 123 ++++ .../kinematic_chain_widget.hpp | 40 +- .../passive_joints.hpp | 67 ++ .../passive_joints_widget.hpp | 27 +- .../planning_groups.hpp | 150 +++++ .../planning_groups_widget.hpp | 55 +- .../moveit_setup_srdf_plugins/robot_poses.hpp | 117 ++++ .../robot_poses_widget.hpp | 62 +- .../moveit_setup_srdf_plugins/srdf_step.hpp | 170 +++++ .../virtual_joints.hpp | 34 +- .../moveit_setup_framework_plugins.xml | 15 + .../src/default_collisions.cpp | 29 +- .../src/default_collisions_widget.cpp | 2 +- .../src/end_effectors.cpp | 63 ++ .../src/end_effectors_widget.cpp | 167 ++--- .../src/group_edit_widget.cpp | 68 +- .../src/group_meta_config.cpp | 199 ++++++ .../src/kinematic_chain_widget.cpp | 82 +-- .../src/passive_joints.cpp | 82 +++ .../src/passive_joints_widget.cpp | 68 +- .../src/planning_groups.cpp | 445 +++++++++++++ .../src/planning_groups_widget.cpp | 618 ++++-------------- .../src/robot_poses.cpp | 157 +++++ .../src/robot_poses_widget.cpp | 238 ++----- .../src/virtual_joints.cpp | 64 +- .../src/virtual_joints_widget.cpp | 9 +- .../tools/moveit_config_data.hpp | 49 -- .../src/tools/moveit_config_data.cpp | 119 ---- 45 files changed, 2242 insertions(+), 1472 deletions(-) create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors.hpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_meta_config.hpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.hpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses.hpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/srdf_step.hpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors.cpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_meta_config.cpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints.cpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp create mode 100644 moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp diff --git a/moveit_setup_assistant/MIGRATION.md b/moveit_setup_assistant/MIGRATION.md index 8453ce83e5..646458b601 100644 --- a/moveit_setup_assistant/MIGRATION.md +++ b/moveit_setup_assistant/MIGRATION.md @@ -10,10 +10,13 @@ This file should be deleted once all tasks are complete. Also delete `SETUP_STEP * `moveit_setup_srdf_plugins` * [x] Self-Collisions * [x] Virtual Joints - * [ ] Planning Groups - * [ ] Robot Poses - * [ ] End Effectors - * [ ] Passive Joints + * [x] Planning Groups + * BUG: Creating a new group does not properly create a new JointModelGroup + * TODO: Need method for getting a list of OMPL Planners + * [x] Robot Poses + * TODO: Need to re-add sleeping code + * [x] End Effectors + * [x] Passive Joints * `moveit_setup_app_plugins` * [ ] ROS Control * [ ] Simulation diff --git a/moveit_setup_assistant/extra_generated_files_code.txt b/moveit_setup_assistant/extra_generated_files_code.txt index 3baafbbc3b..70d389464d 100644 --- a/moveit_setup_assistant/extra_generated_files_code.txt +++ b/moveit_setup_assistant/extra_generated_files_code.txt @@ -19,14 +19,6 @@ file.gen_func_ = boost::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, _1); file.write_on_changes = MoveItConfigData::GROUPS; // need to double check if this is actually correct! - // kinematics.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "kinematics.yaml"; - file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); - file.description_ = "Specifies which kinematic solver plugin to use for each planning group in the SRDF, as well as " - "the kinematic solver search resolution."; - file.gen_func_ = boost::bind(&MoveItConfigData::outputKinematicsYAML, config_data_, _1); - file.write_on_changes = MoveItConfigData::GROUPS | MoveItConfigData::GROUP_KINEMATICS; - // joint_limits.yaml -------------------------------------------------------------------------------------- file.file_name_ = "joint_limits.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); diff --git a/moveit_setup_assistant/extra_initialization_code.txt b/moveit_setup_assistant/extra_initialization_code.txt index f8a875fc16..934ea4ff7f 100644 --- a/moveit_setup_assistant/extra_initialization_code.txt +++ b/moveit_setup_assistant/extra_initialization_code.txt @@ -1,25 +1,6 @@ - // Load the allowed collision matrix - config_data_->loadAllowedCollisionMatrix(); - - // Load kinematics yaml file if available -------------------------------------------------- - fs::path kinematics_yaml_path = config_data_->config_pkg_path_; - kinematics_yaml_path /= "config/kinematics.yaml"; - - if (!config_data_->inputKinematicsYAML(kinematics_yaml_path.make_preferred().string())) - { - QMessageBox::warning(this, "No Kinematic YAML File", - QString("Failed to parse kinematics yaml file. This file is not critical but any previous " - "kinematic solver settings have been lost. To re-populate this file edit each " - "existing planning group and choose a solver, then save each change. \n\nFile error " - "at location ") - .append(kinematics_yaml_path.make_preferred().string().c_str())); - } - else - { - fs::path planning_context_launch_path = config_data_->config_pkg_path_; - planning_context_launch_path /= "launch/planning_context.launch"; - config_data_->inputPlanningContextLaunch(planning_context_launch_path.make_preferred().string()); - } + fs::path planning_context_launch_path = config_data_->config_pkg_path_; + planning_context_launch_path /= "launch/planning_context.launch"; + config_data_->inputPlanningContextLaunch(planning_context_launch_path.make_preferred().string()); // Load ros controllers yaml file if available----------------------------------------------- fs::path ros_controllers_yaml_path = config_data_->config_pkg_path_; diff --git a/moveit_setup_assistant/extra_template_variables.txt b/moveit_setup_assistant/extra_template_variables.txt index ee33d4e682..02572be124 100644 --- a/moveit_setup_assistant/extra_template_variables.txt +++ b/moveit_setup_assistant/extra_template_variables.txt @@ -14,21 +14,3 @@ } variables.push_back(TemplateVariable("ROS_CONTROLLERS", controllers.str())); } - - // Pair 10 - Add parameter files for the kinematics solvers that should be loaded - // in addition to kinematics.yaml by planning_context.launch - std::string kinematics_parameters_files_block; - for (const auto& groups : config_data_->group_meta_data_) - { - if (groups.second.kinematics_parameters_file_.empty()) - continue; - - // add a linebreak if we have more than one entry - if (!kinematics_parameters_files_block.empty()) - kinematics_parameters_files_block += "\n"; - - std::string line = " "; - kinematics_parameters_files_block += line; - } - variables.push_back(TemplateVariable("KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK", kinematics_parameters_files_block)); diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp index 1d2cd7646a..01a32d6d3a 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp @@ -283,7 +283,7 @@ void PerceptionWidget::loadSensorPluginsComboBox() sensor_plugin_field_->addItem("Depth Map"); // Load default config, or use the one in the config package if exists - auto sensors_vec_map = setup_step_.getSensorPluginConfig(); + auto& sensors_vec_map = setup_step_.getSensorPluginConfig(); for (auto& sensor_plugin_config : sensors_vec_map) { if (sensor_plugin_config["sensor_plugin"] == std::string("occupancy_map_monitor/PointCloudOctomapUpdater")) 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 898f07ab21..b4fe09caff 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 @@ -92,6 +92,10 @@ SetupAssistantWidget::SetupAssistantWidget(rviz_common::ros_integration::RosNode setup_steps.push_back("moveit_setup_core_plugins::StartScreenWidget"); setup_steps.push_back("moveit_setup_srdf_plugins::DefaultCollisionsWidget"); setup_steps.push_back("moveit_setup_srdf_plugins::VirtualJointsWidget"); + setup_steps.push_back("moveit_setup_srdf_plugins::PlanningGroupsWidget"); + setup_steps.push_back("moveit_setup_srdf_plugins::RobotPosesWidget"); + setup_steps.push_back("moveit_setup_srdf_plugins::EndEffectorsWidget"); + setup_steps.push_back("moveit_setup_srdf_plugins::PassiveJointsWidget"); setup_steps.push_back("moveit_setup_app_plugins::PerceptionWidget"); setup_steps.push_back("moveit_setup_core_plugins::AuthorInformationWidget"); setup_steps.push_back("moveit_setup_core_plugins::ConfigurationFilesWidget"); diff --git a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt index 15ce90d4d3..c7503616b4 100644 --- a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt @@ -26,10 +26,12 @@ qt5_wrap_cpp(MOC_FILES include/moveit_setup_framework/qt/helper_widgets.hpp include/moveit_setup_framework/qt/setup_step_widget.hpp include/moveit_setup_framework/qt/rviz_panel.hpp + include/moveit_setup_framework/qt/double_list_widget.hpp ) add_library(${PROJECT_NAME} src/helper_widgets.cpp + src/double_list_widget.cpp src/urdf_config.cpp src/package_settings_config.cpp src/srdf_config.cpp 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 74c2a785fb..8e68a4cec9 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 @@ -43,6 +43,20 @@ namespace moveit_setup_framework { +// bits of information that can be changed in the SRDF +enum InformationFields +{ + NONE = 0, + COLLISIONS = 1 << 1, + VIRTUAL_JOINTS = 1 << 2, + GROUPS = 1 << 3, + GROUP_CONTENTS = 1 << 4, + POSES = 1 << 5, + END_EFFECTORS = 1 << 6, + PASSIVE_JOINTS = 1 << 7, + OTHER = 1 << 8, +}; + class SRDFConfig : public SetupConfig { public: @@ -71,7 +85,9 @@ class SRDFConfig : public SetupConfig /// Provide a shared planning scene planning_scene::PlanningScenePtr getPlanningScene(); - void updateRobotModel(bool mark_as_changed = false); + /// Update the robot model with the new SRDF, AND mark the changes that have been made to the model + /// changed_information should be composed of InformationFields + void updateRobotModel(long changed_information = 0L); std::vector getLinkNames() const; @@ -90,11 +106,39 @@ class SRDFConfig : public SetupConfig return srdf_.groups_; } + std::vector getGroupNames() const + { + std::vector group_names; + for (const srdf::Model::Group& group : srdf_.groups_) + { + group_names.push_back(group.name_); + } + return group_names; + } + + std::vector& getGroupStates() + { + return srdf_.group_states_; + } + std::vector& getVirtualJoints() { return srdf_.virtual_joints_; } + std::vector& getPassiveJoints() + { + return srdf_.passive_joints_; + } + + /** + * @brief Return the name of the child link of a joint + * @return empty string if joint is not found + */ + std::string getChildOfJoint(const std::string& joint_name) const; + + void removePoseByName(const std::string& pose_name, const std::string& group_name); + class GeneratedSRDF : public GeneratedFile { public: @@ -118,7 +162,7 @@ class SRDFConfig : public SetupConfig bool hasChanges() const override { - return parent_.has_changes_; + return parent_.changes_ > 0; } bool write() override @@ -173,6 +217,7 @@ class SRDFConfig : public SetupConfig /// Shared planning scene planning_scene::PlanningScenePtr planning_scene_; - bool has_changes_; + // bitfield of changes (composed of InformationFields) + unsigned long changes_; }; } // namespace moveit_setup_framework diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.hpp index 61fffc8ec2..03ae871667 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.hpp @@ -37,16 +37,12 @@ #pragma once #include -class QLabel; -class QTableWidget; -class QTableWidgetItem; -class QItemSelection; +#include +#include +#include +#include -#ifndef Q_MOC_RUN -#include -#endif - -namespace moveit_setup_assistant +namespace moveit_setup_framework { class DoubleListWidget : public QWidget { @@ -61,8 +57,7 @@ class DoubleListWidget : public QWidget // ****************************************************************************************** /// Constructor - DoubleListWidget(QWidget* parent, const MoveItConfigDataPtr& config_data, const QString& long_name, - const QString& short_name, bool add_ok_cancel = true); + DoubleListWidget(QWidget* parent, const QString& long_name, const QString& short_name, bool add_ok_cancel = true); /// Loads the available data list void setAvailable(const std::vector& items); @@ -78,6 +73,9 @@ class DoubleListWidget : public QWidget /// Set the names of the two columns in the widget void setColumnNames(const QString& col1, const QString& col2); + /// Return all the values that are in the "selected" subset + std::vector getSelectedValues() const; + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -130,9 +128,6 @@ private Q_SLOTS: // Variables // ****************************************************************************************** - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; - // ****************************************************************************************** // Private Functions // ****************************************************************************************** @@ -141,4 +136,4 @@ private Q_SLOTS: void previewSelected(const QList& selected); }; -} // namespace moveit_setup_assistant +} // namespace moveit_setup_framework diff --git a/moveit_setup_assistant/moveit_setup_framework/src/double_list_widget.cpp b/moveit_setup_assistant/moveit_setup_framework/src/double_list_widget.cpp index 583e9e2ed4..e1cf8434fd 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/double_list_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/double_list_widget.cpp @@ -45,16 +45,16 @@ #include #include #include -#include "double_list_widget.h" +#include -namespace moveit_setup_assistant +namespace moveit_setup_framework { // ****************************************************************************************** // // ****************************************************************************************** -DoubleListWidget::DoubleListWidget(QWidget* parent, const MoveItConfigDataPtr& config_data, const QString& long_name, - const QString& short_name, bool add_ok_cancel) - : QWidget(parent), long_name_(long_name), short_name_(short_name), config_data_(config_data) +DoubleListWidget::DoubleListWidget(QWidget* parent, const QString& long_name, const QString& short_name, + bool add_ok_cancel) + : QWidget(parent), long_name_(long_name), short_name_(short_name) { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -334,4 +334,14 @@ void DoubleListWidget::previewSelected(const QList& selected) Q_EMIT(previewSelected(selected_vector)); } -} // namespace moveit_setup_assistant +std::vector DoubleListWidget::getSelectedValues() const +{ + std::vector values(selected_data_table_->rowCount()); + for (int i = 0; i < selected_data_table_->rowCount(); ++i) + { + values.push_back(selected_data_table_->item(i, 0)->text().toStdString()); + } + return values; +} + +} // namespace moveit_setup_framework 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 b24d71e3d9..751b1a5b56 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp @@ -41,7 +41,7 @@ namespace moveit_setup_framework void SRDFConfig::onInit() { parent_node_->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); - has_changes_ = false; + changes_ = 0L; } void SRDFConfig::loadPrevious(const std::string& package_path, const YAML::Node& node) @@ -135,19 +135,24 @@ void SRDFConfig::getRelativePath() srdf_pkg_relative_path_ = appendPaths("config", urdf_model_->getName() + ".srdf"); } -void SRDFConfig::updateRobotModel(bool mark_as_changed) +void SRDFConfig::updateRobotModel(long changed_information) { // Initialize with a URDF Model Interface and a SRDF Model loadURDFModel(); + if (changed_information > 0) + { + srdf_.updateSRDFModel(*urdf_model_); + } + robot_model_ = std::make_shared(urdf_model_, srdf_.srdf_model_); if (srdf_pkg_relative_path_.empty()) { srdf_pkg_relative_path_ = appendPaths("config", urdf_model_->getName() + ".srdf"); srdf_.robot_name_ = urdf_model_->getName(); - has_changes_ = true; + changes_ |= OTHER; } - has_changes_ |= mark_as_changed; + changes_ |= changed_information; // Reset the planning scene planning_scene_.reset(); @@ -179,6 +184,31 @@ std::vector SRDFConfig::getLinkNames() const return names; } +std::string SRDFConfig::getChildOfJoint(const std::string& joint_name) const +{ + const moveit::core::JointModel* joint_model = getRobotModel()->getJointModel(joint_name); + // Check that a joint model was found + if (!joint_model) + { + return ""; + } + return joint_model->getChildLinkModel()->getName(); +} + +void SRDFConfig::removePoseByName(const std::string& pose_name, const std::string& group_name) +{ + for (std::vector::iterator pose_it = srdf_.group_states_.begin(); + pose_it != srdf_.group_states_.end(); ++pose_it) + { + if (pose_it->name_ == pose_name && pose_it->group_ == group_name) + { + srdf_.group_states_.erase(pose_it); + updateRobotModel(moveit_setup_framework::POSES); + return; + } + } +} + void SRDFConfig::collectVariables(std::vector& variables) { variables.push_back(TemplateVariable("ROBOT_NAME", srdf_.robot_name_)); diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt index 1789f8abc8..822211001e 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt @@ -14,6 +14,12 @@ qt5_wrap_cpp(MOC_FILES include/moveit_setup_srdf_plugins/collision_linear_model.hpp include/moveit_setup_srdf_plugins/collision_matrix_model.hpp include/moveit_setup_srdf_plugins/default_collisions_widget.hpp + include/moveit_setup_srdf_plugins/end_effectors_widget.hpp + include/moveit_setup_srdf_plugins/group_edit_widget.hpp + include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp + include/moveit_setup_srdf_plugins/passive_joints_widget.hpp + include/moveit_setup_srdf_plugins/planning_groups_widget.hpp + include/moveit_setup_srdf_plugins/robot_poses_widget.hpp include/moveit_setup_srdf_plugins/rotated_header_view.hpp include/moveit_setup_srdf_plugins/virtual_joints_widget.hpp ) @@ -24,6 +30,17 @@ add_library(${PROJECT_NAME} src/compute_default_collisions.cpp src/default_collisions.cpp src/default_collisions_widget.cpp + src/end_effectors.cpp + src/end_effectors_widget.cpp + src/group_edit_widget.cpp + src/group_meta_config.cpp + src/kinematic_chain_widget.cpp + src/passive_joints.cpp + src/passive_joints_widget.cpp + src/planning_groups.cpp + src/planning_groups_widget.cpp + src/robot_poses.cpp + src/robot_poses_widget.cpp src/rotated_header_view.cpp src/virtual_joints.cpp src/virtual_joints_widget.cpp diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.hpp index ece1f8214d..9fc0f873cd 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.hpp @@ -42,7 +42,7 @@ #include #endif -class QItemSelection; +#include class CollisionMatrixModel : public QAbstractTableModel { diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp index d98f1cc1f0..0fcee13474 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp @@ -35,14 +35,13 @@ /* Author: David V. Lu!! */ #pragma once -#include -#include +#include #include #include namespace moveit_setup_srdf_plugins { -class DefaultCollisions : public moveit_setup_framework::SetupStep +class DefaultCollisions : public SRDFStep { public: std::string getName() const override @@ -50,13 +49,6 @@ class DefaultCollisions : public moveit_setup_framework::SetupStep return "Self-Collisions"; } - void onInit() override; - - bool isReady() const override - { - return srdf_config_->isConfigured(); - } - std::vector getCollidingLinks(); /** @@ -75,9 +67,6 @@ class DefaultCollisions : public moveit_setup_framework::SetupStep */ void linkPairsFromSRDF(); - /// Load the allowed collision matrix from the SRDF's list of link pairs - void loadAllowedCollisionMatrix(); - LinkPairMap& getLinkPairs() { return link_pairs_; @@ -92,14 +81,9 @@ class DefaultCollisions : public moveit_setup_framework::SetupStep protected: void generateCollisionTable(unsigned int num_trials, double min_frac, bool verbose); - std::shared_ptr srdf_config_; - /// main storage of link pair data LinkPairMap link_pairs_; - /// Allowed collision matrix for robot poses - collision_detection::AllowedCollisionMatrix allowed_collision_matrix_; - // For threaded operations boost::thread worker_; unsigned int progress_; diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors.hpp new file mode 100644 index 0000000000..30ed06c1a1 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ +#pragma once + +#include +#include + +namespace moveit_setup_srdf_plugins +{ +class EndEffectors : public SuperSRDFStep +{ +public: + std::string getName() const override + { + return "End Effectors"; + } + + std::vector& getContainer() override + { + return srdf_config_->getEndEffectors(); + } + + moveit_setup_framework::InformationFields getInfoField() const override + { + return moveit_setup_framework::END_EFFECTORS; + } + + void onInit() override; + + bool isReady() const override + { + return hasGroups(); + } + + std::vector getGroupNames() const + { + return srdf_config_->getGroupNames(); + } + + std::vector& getEndEffectors() + { + return srdf_config_->getEndEffectors(); + } + + std::vector getLinkNames() const + { + return srdf_config_->getLinkNames(); + } + + bool isLinkInGroup(const std::string& link, const std::string& group) const; + void setProperties(srdf::Model::EndEffector* eef, const std::string& parent_link, const std::string& component_group, + const std::string& parent_group); + +protected: + std::shared_ptr urdf_config_; +}; +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.hpp index 6b776d6e57..aa7ad5abce 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.hpp @@ -37,22 +37,19 @@ #pragma once // Qt -class QComboBox; -class QLineEdit; -class QPushButton; -class QStackedWidget; -class QTableWidget; +#include +#include +#include +#include +#include // SA -#ifndef Q_MOC_RUN -#include -#endif +#include +#include -#include "setup_screen_widget.h" // a base class for screens in the setup assistant - -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { -class EndEffectorsWidget : public SetupScreenWidget +class EndEffectorsWidget : public moveit_setup_framework::SetupStepWidget { Q_OBJECT @@ -61,11 +58,16 @@ class EndEffectorsWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - EndEffectorsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; /// Received when this widget is chosen from the navigation menu void focusGiven() override; + moveit_setup_framework::SetupStep& getSetupStep() override + { + return setup_step_; + } + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -116,9 +118,7 @@ private Q_SLOTS: // ****************************************************************************************** // Variables // ****************************************************************************************** - - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + EndEffectors setup_step_; /// Original name of effector currently being edited. This is used to find the element in the vector std::string current_edit_effector_; @@ -133,7 +133,7 @@ private Q_SLOTS: * @param name - name of data to find in datastructure * @return pointer to data in datastructure */ - srdf::Model::EndEffector* findEffectorByName(const std::string& name); + srdf::Model::EndEffector* getEndEffector(const std::string& name); /** * Create the main list view of effectors for robot @@ -175,4 +175,4 @@ private Q_SLOTS: void edit(const std::string& name); }; -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.hpp index 0cf37207c7..fd226770fd 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.hpp @@ -37,16 +37,14 @@ #pragma once #include -class QComboBox; -class QLabel; -class QLineEdit; -class QPushButton; +#include +#include +#include +#include -#ifndef Q_MOC_RUN -#include -#endif +#include -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { class GroupEditWidget : public QWidget { @@ -58,10 +56,10 @@ class GroupEditWidget : public QWidget // ****************************************************************************************** /// Constructor - GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + GroupEditWidget(QWidget* parent, PlanningGroups& setup_step); /// Set the previous data - void setSelected(const std::string& group_name); + void setSelected(const std::string& group_name, const GroupMetaData& meta_data); /// Populate the combo dropdown box with kinematic planners void loadKinematicPlannersComboBox(); @@ -121,12 +119,10 @@ private Q_SLOTS: // ****************************************************************************************** // Variables // ****************************************************************************************** - - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + PlanningGroups& setup_step_; // ****************************************************************************************** // Private Functions // ****************************************************************************************** }; -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_meta_config.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_meta_config.hpp new file mode 100644 index 0000000000..2d821b18fa --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_meta_config.hpp @@ -0,0 +1,123 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Metro Robots nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ +#pragma once + +#include +#include +#include + +namespace moveit_setup_srdf_plugins +{ +// Default kin solver values +static const double DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION = 0.005; +static const double DEFAULT_KIN_SOLVER_TIMEOUT = 0.005; + +/** + * Planning groups extra data not found in srdf but used in config files + */ +struct GroupMetaData +{ + std::string kinematics_solver_; // Name of kinematics plugin to use + double kinematics_solver_search_resolution_{ DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION }; // resolution to use with solver + double kinematics_solver_timeout_{ DEFAULT_KIN_SOLVER_TIMEOUT }; // solver timeout + std::string kinematics_parameters_file_; // file for additional kinematics parameters + std::string default_planner_; // Name of the default planner to use +}; + +static const std::string KINEMATICS_FILE = "config/kinematics.yaml"; + +class GroupMetaConfig : public moveit_setup_framework::SetupConfig +{ +public: + bool isConfigured() const override; + void loadPrevious(const std::string& package_path, const YAML::Node& node) override; + + void deleteGroup(const std::string& group_name); + void renameGroup(const std::string& old_group_name, const std::string& new_group_name); + + const GroupMetaData& getMetaData(const std::string& group_name) const; + void setMetaData(const std::string& group_name, const GroupMetaData& meta_data); + + class GeneratedGroupMetaConfig : public moveit_setup_framework::YamlGeneratedFile + { + public: + GeneratedGroupMetaConfig(const std::string& package_path, const std::time_t& last_gen_time, GroupMetaConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + std::string getRelativePath() const override + { + return KINEMATICS_FILE; + } + + std::string getDescription() const override + { + return "Specifies which kinematic solver plugin to use for each planning group in the SRDF, as well as " + "the kinematic solver search resolution."; + } + + bool hasChanges() const override + { + return parent_.changed_; + } + + bool writeYaml(YAML::Emitter& emitter) override; + + protected: + GroupMetaConfig& parent_; + }; + + void collectFiles(const std::string& package_path, const std::time_t& last_gen_time, + std::vector& files) override + { + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + } + + void collectVariables(std::vector& variables) override; + +protected: + // Helper method with old name that conveniently returns a bool + bool inputKinematicsYAML(const std::string& file_path); + + /// Planning groups extra data not found in srdf but used in config files + std::map group_meta_data_; + + GroupMetaData default_values_; + + bool changed_{ false }; +}; +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp index 03e3a90cac..1f6e09295d 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp @@ -37,42 +37,34 @@ #pragma once #include -class QLabel; -class QLineEdit; -class QTreeWidget; -class QTreeWidgetItem; +#include +#include +#include +#include -#ifndef Q_MOC_RUN -#include -#endif +#include +#include -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { class KinematicChainWidget : public QWidget { Q_OBJECT - - // ****************************************************************************************** - // Reusable double list widget for selecting and deselecting a subset from a set - // ****************************************************************************************** public: // ****************************************************************************************** // Public Functions // ****************************************************************************************** /// Constructor - KinematicChainWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + KinematicChainWidget(QWidget* parent, moveit_setup_framework::RVizPanel* rviz_panel); /// Loads the available data list - void setAvailable(); + void setAvailable(const LinkNameTree& link_name_tree); /// Set the link field with previous value void setSelected(const std::string& base_link, const std::string& tip_link); - void addLinktoTreeRecursive(const moveit::core::LinkModel* link, const moveit::core::LinkModel* parent); - - bool addLinkChildRecursive(QTreeWidgetItem* parent, const moveit::core::LinkModel* link, - const std::string& parent_name); + QTreeWidgetItem* addLinkChildRecursive(QTreeWidgetItem* parent, const LinkNameTree& link); // ****************************************************************************************** // Qt Components @@ -113,25 +105,17 @@ private Q_SLOTS: /// Event sent when user presses cancel button void cancelEditing(); - /// Event for telling rviz to highlight a link of the robot - void highlightLink(const std::string& name, const QColor& /*_t2*/); - - /// Event for telling rviz to unhighlight all links of the robot - void unhighlightAll(); - private: // ****************************************************************************************** // Variables // ****************************************************************************************** - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; - /// Remember if the chain tree has been loaded bool kinematic_chain_loaded_; + moveit_setup_framework::RVizPanel* rviz_panel_; // ****************************************************************************************** // Private Functions // ****************************************************************************************** }; -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.hpp new file mode 100644 index 0000000000..dc734f8115 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.hpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ +#pragma once + +#include + +namespace moveit_setup_srdf_plugins +{ +class PassiveJoints : public SRDFStep +{ +public: + std::string getName() const override + { + return "Passive Joints"; + } + + /** + * @brief Return all active (non-fixed) joint names + */ + std::vector getActiveJoints() const; + + /** + * @brief Return all passive joint names (according to srdf) + */ + std::vector getPassiveJoints() const; + + std::string getChildOfJoint(const std::string& joint_name) const + { + return srdf_config_->getChildOfJoint(joint_name); + } + + void setPassiveJoints(const std::vector& passive_joints); +}; +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.hpp index 1eced6e34b..e5b58c24e3 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.hpp @@ -37,16 +37,13 @@ #pragma once // SA -#ifndef Q_MOC_RUN -#include -#endif +#include +#include +#include -#include "setup_screen_widget.h" // a base class for screens in the setup assistant - -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { -class DoubleListWidget; -class PassiveJointsWidget : public SetupScreenWidget +class PassiveJointsWidget : public moveit_setup_framework::SetupStepWidget { Q_OBJECT @@ -55,16 +52,21 @@ class PassiveJointsWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - PassiveJointsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; /// Received when this widget is chosen from the navigation menu void focusGiven() override; + moveit_setup_framework::SetupStep& getSetupStep() override + { + return setup_step_; + } + // ****************************************************************************************** // Qt Components // ****************************************************************************************** - DoubleListWidget* joints_widget_; + moveit_setup_framework::DoubleListWidget* joints_widget_; private Q_SLOTS: @@ -81,11 +83,10 @@ private Q_SLOTS: // Variables // ****************************************************************************************** - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + PassiveJoints setup_step_; /// Original name of vjoint currently being edited. This is used to find the element in the vector std::string current_edit_vjoint_; }; -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp new file mode 100644 index 0000000000..529f64b588 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp @@ -0,0 +1,150 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ +#pragma once + +#include +#include +#include +#include + +namespace moveit_setup_srdf_plugins +{ +class LinkNameTree +{ +public: + std::string data; + std::vector children; +}; + +inline LinkNameTree buildLinkNameTree(const moveit::core::LinkModel* link) +{ + LinkNameTree node; + node.data = link->getName(); + + for (const auto& child_joint : link->getChildJointModels()) + { + node.children.push_back(buildLinkNameTree(child_joint->getChildLinkModel())); + } + return node; +} + +class PlanningGroups : public SuperSRDFStep +{ +public: + std::string getName() const override + { + return "Planning Groups"; + } + + std::vector& getContainer() override + { + return srdf_config_->getGroups(); + } + + moveit_setup_framework::InformationFields getInfoField() const override + { + return moveit_setup_framework::GROUPS; + } + + void onInit() override; + + std::vector& getGroups() + { + return srdf_config_->getGroups(); + } + + void renameGroup(const std::string& old_group_name, const std::string& new_group_name); + void deleteGroup(const std::string& group_name); + + /** + * @brief Set the specified group's joint names + */ + void setJoints(const std::string& group_name, const std::vector& joint_names); + + /** + * @brief Set the specified group's link names + */ + void setLinks(const std::string& group_name, const std::vector& link_names); + + /** + * @brief Set the specified group's kinematic chain + * @throws runtime_error If base/tip are invalid + */ + void setChain(const std::string& group_name, const std::string& base, const std::string& tip); + + /** + * @brief Set the specified group's subgroups + * @throws runtime_error If subgroup would result in a cycle + */ + void setSubgroups(const std::string& selected_group_name, const std::vector& subgroups); + + const GroupMetaData& getMetaData(const std::string& group_name) const + { + return group_meta_config_->getMetaData(group_name); + } + + void setMetaData(const std::string& group_name, const GroupMetaData& meta_data) + { + group_meta_config_->setMetaData(group_name, meta_data); + } + + std::vector getGroupNames() const + { + return srdf_config_->getGroupNames(); + } + + const std::vector& getJointNames() const + { + return srdf_config_->getRobotModel()->getJointModelNames(); + } + + const std::vector& getLinkNames() const + { + return srdf_config_->getRobotModel()->getLinkModelNames(); + } + + std::string getChildOfJoint(const std::string& joint_name) const; + std::string getJointType(const std::string& joint_name) const; + LinkNameTree getLinkNameTree() const; + std::vector getPosesByGroup(const std::string& group_name) const; + std::vector getEndEffectorsByGroup(const std::string& group_name) const; + std::vector getKinematicPlanners() const; + std::vector getOMPLPlanners() const; + +protected: + std::shared_ptr group_meta_config_; +}; +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.hpp index ef860eddbd..f65db1a04a 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.hpp @@ -37,27 +37,23 @@ #pragma once // Qt -class QPushButton; -class QStackedWidget; -class QTreeWidget; -class QTreeWidgetItem; - -// Setup Asst -#ifndef Q_MOC_RUN -#include -#endif - -#include "setup_screen_widget.h" // a base class for screens in the setup assistant +#include +#include +#include +#include + +// SA +#include +#include +#include +#include // for kinematic chain page +#include // for group rename page // Forward Declaration (outside of namespace for Qt) class PlanGroupType; -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { -class DoubleListWidget; -class KinematicChainWidget; -class GroupEditWidget; - // Custom Type enum GroupType { @@ -73,7 +69,7 @@ enum GroupType // CLASS // ****************************************************************************************** // ****************************************************************************************** -class PlanningGroupsWidget : public SetupScreenWidget +class PlanningGroupsWidget : public moveit_setup_framework::SetupStepWidget { Q_OBJECT @@ -82,13 +78,18 @@ class PlanningGroupsWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - PlanningGroupsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; void changeScreen(int index); /// Received when this widget is chosen from the navigation menu void focusGiven() override; + moveit_setup_framework::SetupStep& getSetupStep() override + { + return setup_step_; + } + private Q_SLOTS: // ****************************************************************************************** @@ -156,9 +157,9 @@ private Q_SLOTS: // Stacked Layout SUBPAGES ------------------------------------------- QWidget* groups_tree_widget_; - DoubleListWidget* joints_widget_; - DoubleListWidget* links_widget_; - DoubleListWidget* subgroups_widget_; + moveit_setup_framework::DoubleListWidget* joints_widget_; + moveit_setup_framework::DoubleListWidget* links_widget_; + moveit_setup_framework::DoubleListWidget* subgroups_widget_; KinematicChainWidget* chain_widget_; GroupEditWidget* group_edit_widget_; @@ -166,8 +167,7 @@ private Q_SLOTS: // Variables // ****************************************************************************************** - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + PlanningGroups setup_step_; /// Remember what group we are editing when an edit screen is being shown std::string current_edit_group_; @@ -188,9 +188,6 @@ private Q_SLOTS: /// Recursively build the SRDF tree void loadGroupsTreeRecursive(srdf::Model::Group& group_it, QTreeWidgetItem* parent); - // Convenience function for getting a group pointer - srdf::Model::Group* findGroupByName(const std::string& name); - // Load edit screen void loadJointsScreen(srdf::Model::Group* this_group); void loadLinksScreen(srdf::Model::Group* this_group); @@ -204,7 +201,7 @@ private Q_SLOTS: /// Switch to current groups view void showMainScreen(); }; -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins // ****************************************************************************************** // ****************************************************************************************** @@ -219,7 +216,7 @@ class PlanGroupType PlanGroupType() { } - PlanGroupType(srdf::Model::Group* group, const moveit_setup_assistant::GroupType type); + PlanGroupType(srdf::Model::Group* group, const moveit_setup_srdf_plugins::GroupType type); virtual ~PlanGroupType() { ; @@ -227,7 +224,7 @@ class PlanGroupType srdf::Model::Group* group_; - moveit_setup_assistant::GroupType type_; + moveit_setup_srdf_plugins::GroupType type_; }; Q_DECLARE_METATYPE(PlanGroupType); diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses.hpp new file mode 100644 index 0000000000..36ed03d30f --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses.hpp @@ -0,0 +1,117 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ +#pragma once + +#include +#include + +namespace moveit_setup_srdf_plugins +{ +// Note: Does not derive from SuperSRDFStep because we always need to check the name AND the group name for each pose +class RobotPoses : public SRDFStep +{ +public: + std::string getName() const override + { + return "Robot Poses"; + } + + void onInit() override; + + bool isReady() const override + { + return hasGroups(); + } + + std::vector getGroupNames() const + { + return srdf_config_->getGroupNames(); + } + + /** + * Find the associated data by name + * + * @param name - name of data to find in datastructure + * @return pointer to data in datastructure + */ + srdf::Model::GroupState* findPoseByName(const std::string& name, const std::string& group); + + std::vector& getGroupStates() + { + return srdf_config_->getGroupStates(); + } + + moveit::core::RobotState& getState() + { + return srdf_config_->getPlanningScene()->getCurrentStateNonConst(); + } + + /** + * @brief Publish the given state on the moveit_robot_state topic and return true if it is in collision + */ + bool publishState(const moveit::core::RobotState& robot_state); + + /** + * @brief Returns a vector of joint models for the given group name + * @throws runtime_error if the group does not exist + * + * Note: "Simple" means we exclude Passive/Mimic/MultiDOF joints + */ + std::vector getSimpleJointModels(const std::string& group_name) const; + + void removePoseByName(const std::string& pose_name, const std::string& group_name) + { + srdf_config_->removePoseByName(pose_name, group_name); + } + + void setToCurrentValues(srdf::Model::GroupState& group_state); + + /// Load the allowed collision matrix from the SRDF's list of link pairs + void loadAllowedCollisionMatrix(); + +protected: + /// Remember the publisher for quick publishing later + rclcpp::Publisher::SharedPtr pub_robot_state_; + + // ****************************************************************************************** + // Collision Variables + // ****************************************************************************************** + collision_detection::CollisionRequest request; + + /// Allowed collision matrix for robot poses + collision_detection::AllowedCollisionMatrix allowed_collision_matrix_; +}; +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.hpp index dcc8377be4..333c0fed24 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.hpp @@ -37,26 +37,23 @@ #pragma once // Qt -class QComboBox; -class QLabel; -class QLineEdit; -class QPushButton; -class QScrollArea; -class QSlider; -class QStackedWidget; -class QTableWidget; -class QVBoxLayout; +#include +#include +#include +#include +#include +#include +#include +#include +#include // SA -#ifndef Q_MOC_RUN -#include -#endif +#include +#include -#include "setup_screen_widget.h" // a base class for screens in the setup assistant - -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { -class RobotPosesWidget : public SetupScreenWidget +class RobotPosesWidget : public moveit_setup_framework::SetupStepWidget { Q_OBJECT @@ -65,11 +62,16 @@ class RobotPosesWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; /// Received when this widget is chosen from the navigation menu void focusGiven() override; + moveit_setup_framework::SetupStep& getSetupStep() override + { + return setup_step_; + } + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -133,40 +135,20 @@ private Q_SLOTS: */ void updateRobotModel(const std::string& name, double value); - /// Publishes a joint state message based on all the slider locations in a planning group, to rviz - void publishJoints(); - private: // ****************************************************************************************** // Variables // ****************************************************************************************** - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + RobotPoses setup_step_; /// Pointer to currently edited group state srdf::Model::GroupState* current_edit_pose_; - /// Remember the publisher for quick publishing later - ros::Publisher pub_robot_state_; - - // ****************************************************************************************** - // Collision Variables - // ****************************************************************************************** - collision_detection::CollisionRequest request; - // ****************************************************************************************** // Private Functions // ****************************************************************************************** - /** - * Find the associated data by name - * - * @param name - name of data to find in datastructure - * @return pointer to data in datastructure - */ - srdf::Model::GroupState* findPoseByName(const std::string& name, const std::string& group); - /** * Create the main list view of poses for robot * @@ -203,7 +185,7 @@ private Q_SLOTS: /** * Show the robot in the current pose */ - void showPose(srdf::Model::GroupState* pose); + void showPose(const srdf::Model::GroupState& pose); }; // ****************************************************************************************** @@ -279,7 +261,7 @@ private Q_SLOTS: // ****************************************************************************************** }; -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins // Declare std::string as metatype so we can use it in a signal Q_DECLARE_METATYPE(std::string) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/srdf_step.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/srdf_step.hpp new file mode 100644 index 0000000000..f234d02f57 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/srdf_step.hpp @@ -0,0 +1,170 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Metro Robots nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ +#pragma once + +#include +#include + +namespace moveit_setup_srdf_plugins +{ +/** + * @brief Setup Step that contains the SRDFConfig + */ +class SRDFStep : public moveit_setup_framework::SetupStep +{ +public: + void onInit() override + { + srdf_config_ = config_data_->get("srdf"); + } + + bool isReady() const override + { + return srdf_config_->isConfigured(); + } + + bool hasGroups() const + { + return !srdf_config_->getGroups().empty(); + } + +protected: + std::shared_ptr srdf_config_; +}; + +/** + * @brief This class provides a number of standard operations based on srdf's vector members + */ +template +class SuperSRDFStep : public SRDFStep +{ +public: + /** + * @brief Returns the reference to the vector in the SRDF + */ + virtual std::vector& getContainer() = 0; + + /** + * @brief Returns the info field associated with this part of the SRDF + */ + virtual moveit_setup_framework::InformationFields getInfoField() const = 0; + + /** + * @brief Return a pointer to an item with the given name if it exists, otherwise null + */ + T* find(const std::string& name) + { + // Note: method is not const because otherwise we cannot return a non-const pointer + for (T& item : getContainer()) + { + if (item.name_ == name) + { + return &item; + } + } + return nullptr; + } + + /** + * @brief Create an item with the given name and return the pointer + * @note: Does not check if an item with the given name exists + */ + T* create(const std::string& name) + { + T new_item; + new_item.name_ = name; + getContainer().push_back(new_item); + srdf_config_->updateRobotModel(getInfoField()); + return &getContainer().back(); + } + + /** + * @brief Renames an item and returns a pointer to the item + * @throws runtime_error If an item exists with the new name + */ + T* rename(const std::string& old_name, const std::string& new_name) + { + T* item = find(old_name); + T* existing_item = find(new_name); + if (existing_item) + { + throw std::runtime_error("An item already exists with that name!"); + } + item->name_ = new_name; + srdf_config_->updateRobotModel(getInfoField()); + return item; + } + + /** + * @brief Delete an item with the given name from the list + * @return true if item was found + */ + bool remove(const std::string& name) + { + auto& container = getContainer(); + for (auto it = container.begin(); it != container.end(); ++it) + { + if (it->name_ == name) // string match + { + container.erase(it); + srdf_config_->updateRobotModel(getInfoField()); + return true; + } + } + return false; + } + + /** + * @brief Get a pointer to an item with the given name, creating if necessary. + * If old_name is provided (and is different) will rename the given item. + */ + T* get(const std::string& name, const std::string& old_name = "") + { + if (name == old_name) + { + return find(name); + } + else if (old_name.empty()) + { + return create(name); + } + else + { + return rename(old_name, name); + } + } +}; +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints.hpp index 020efd451c..1129b0d49e 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints.hpp @@ -35,13 +35,12 @@ /* Author: David V. Lu!! */ #pragma once -#include +#include #include -#include namespace moveit_setup_srdf_plugins { -class VirtualJoints : public moveit_setup_framework::SetupStep +class VirtualJoints : public SuperSRDFStep { public: std::string getName() const override @@ -49,38 +48,27 @@ class VirtualJoints : public moveit_setup_framework::SetupStep return "Virtual Joints"; } - void onInit() override; - - bool isReady() const override + std::vector& getContainer() override { - return urdf_config_->isConfigured(); + return srdf_config_->getVirtualJoints(); } - /** - * Find the associated data by name - * - * @param name - name of data to find in datastructure - * @return pointer to data in datastructure - */ - srdf::Model::VirtualJoint* findVJointByName(const std::string& name); + moveit_setup_framework::InformationFields getInfoField() const override + { + return moveit_setup_framework::VIRTUAL_JOINTS; + } - bool deleteByName(const std::string& name); + void onInit() override; - void create(const std::string& old_name, const std::string& joint_name, const std::string& parent_name, - const std::string& child_name, const std::string& joint_type); + void setProperties(srdf::Model::VirtualJoint* vj, const std::string& parent_name, const std::string& child_name, + const std::string& joint_type); std::vector getLinkNames() const { return srdf_config_->getLinkNames(); } - std::vector& getVirtualJoints() - { - return srdf_config_->getVirtualJoints(); - } - protected: - std::shared_ptr srdf_config_; std::shared_ptr urdf_config_; }; } // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/moveit_setup_framework_plugins.xml b/moveit_setup_assistant/moveit_setup_srdf_plugins/moveit_setup_framework_plugins.xml index fe44e53683..3194be7d52 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/moveit_setup_framework_plugins.xml +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/moveit_setup_framework_plugins.xml @@ -5,4 +5,19 @@ Step to ignore self-colisions + + Step to configure planning groups + + + Step to configure robot poses + + + Step to configure end effectors + + + Step to configure passive joints (if any) + + + Extra data associated with planning groups not found in srdf but used in config files + diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions.cpp index 0011243e1f..148be4bb72 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions.cpp @@ -38,11 +38,6 @@ namespace moveit_setup_srdf_plugins { -void DefaultCollisions::onInit() -{ - srdf_config_ = config_data_->get("srdf"); -} - std::vector DefaultCollisions::getCollidingLinks() { return srdf_config_->getPlanningScene()->getRobotModel()->getLinkModelNamesWithCollisionGeometry(); @@ -53,7 +48,7 @@ std::vector DefaultCollisions::getCollidingLinks() void DefaultCollisions::linkPairsToSRDF() { // reset the data in the SRDF Writer class - auto disabled_list = srdf_config_->getDisabledCollisions(); + auto& disabled_list = srdf_config_->getDisabledCollisions(); disabled_list.clear(); // Create temp disabled collision @@ -71,9 +66,7 @@ void DefaultCollisions::linkPairsToSRDF() disabled_list.push_back(dc); } } - - // Update collision_matrix for robot pose's use - loadAllowedCollisionMatrix(); + srdf_config_->updateRobotModel(moveit_setup_framework::COLLISIONS); // mark as changed } // ****************************************************************************************** @@ -103,7 +96,7 @@ class SortableDisabledCollision void DefaultCollisions::linkPairsToSRDFSorted(size_t skip_mask) { - auto disabled_list = srdf_config_->getDisabledCollisions(); + auto& disabled_list = srdf_config_->getDisabledCollisions(); // Create temp disabled collision srdf::Model::DisabledCollision dc; @@ -166,21 +159,6 @@ void DefaultCollisions::linkPairsFromSRDF() } } -// ****************************************************************************************** -// Load the allowed collision matrix from the SRDF's list of link pairs -// ****************************************************************************************** -void DefaultCollisions::loadAllowedCollisionMatrix() -{ - // Clear the allowed collision matrix - allowed_collision_matrix_.clear(); - - // Update the allowed collision matrix, in case there has been a change - for (const auto& disabled_collision : srdf_config_->getDisabledCollisions()) - { - allowed_collision_matrix_.setEntry(disabled_collision.link1_, disabled_collision.link2_, true); - } -} - void DefaultCollisions::startGenerationThread(unsigned int num_trials, double min_frac, bool verbose) { progress_ = 0; @@ -216,7 +194,6 @@ void DefaultCollisions::cancelGenerationThread() void DefaultCollisions::joinGenerationThread() { - srdf_config_->updateRobotModel(true); // mark as changed worker_.join(); } diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions_widget.cpp index 943c89a539..25595fe58d 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions_widget.cpp @@ -713,7 +713,7 @@ bool DefaultCollisionsWidget::focusLost() worker_->wait(); } - // Copy changes to srdf_writer object and config_data_->allowed_collision_matrix_ + // Copy changes to srdf_writer object setup_step_.linkPairsToSRDF(); return true; } diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors.cpp new file mode 100644 index 0000000000..4d06a301b4 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors.cpp @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ + +#include + +namespace moveit_setup_srdf_plugins +{ +void EndEffectors::onInit() +{ + SuperSRDFStep::onInit(); + urdf_config_ = config_data_->get("urdf"); +} + +bool EndEffectors::isLinkInGroup(const std::string& link, const std::string& group) const +{ + const auto rm = srdf_config_->getRobotModel(); + const moveit::core::JointModelGroup* jmg = rm->getJointModelGroup(group); + return jmg->hasLinkModel(link); +} + +void EndEffectors::setProperties(srdf::Model::EndEffector* eef, const std::string& parent_link, + const std::string& component_group, const std::string& parent_group) +{ + eef->parent_link_ = parent_link; + eef->component_group_ = component_group; + eef->parent_group_ = parent_group; + srdf_config_->updateRobotModel(moveit_setup_framework::END_EFFECTORS); +} + +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors_widget.cpp index 8366b42d74..964de7b885 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors_widget.cpp @@ -35,8 +35,8 @@ /* Author: Dave Coleman */ // SA -#include "end_effectors_widget.h" -#include "header_widget.h" +#include +#include // Qt #include @@ -55,25 +55,25 @@ #include #include -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { // ****************************************************************************************** // Constructor // ****************************************************************************************** -EndEffectorsWidget::EndEffectorsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +void EndEffectorsWidget::onInit() { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); // Top Header Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget("Define End Effectors", - "Setup your robot's end effectors. These are planning groups " - "corresponding to grippers or tools, attached to a parent " - "planning group (an arm). The specified parent link is used as the " - "reference frame for IK attempts.", - this); + auto header = + new moveit_setup_framework::HeaderWidget("Define End Effectors", + "Setup your robot's end effectors. These are planning groups " + "corresponding to grippers or tools, attached to a parent " + "planning group (an arm). The specified parent link is used as the " + "reference frame for IK attempts.", + this); layout->addWidget(header); // Create contents screens --------------------------------------- @@ -246,7 +246,7 @@ void EndEffectorsWidget::showNewScreen() stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode - Q_EMIT isModal(true); + Q_EMIT setModalMode(true); } // ****************************************************************************************** @@ -270,13 +270,13 @@ void EndEffectorsWidget::previewClicked(int /*row*/, int /*column*/) return; // Find the selected in datastructure - srdf::Model::EndEffector* effector = findEffectorByName(selected[0]->text().toStdString()); + srdf::Model::EndEffector* effector = getEndEffector(selected[0]->text().toStdString()); // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); // Highlight group - Q_EMIT highlightGroup(effector->component_group_); + rviz_panel_->highlightGroup(effector->component_group_); } // ****************************************************************************************** @@ -289,10 +289,10 @@ void EndEffectorsWidget::previewClickedString(const QString& name) return; // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); // Highlight group - Q_EMIT highlightGroup(name.toStdString()); + rviz_panel_->highlightGroup(name.toStdString()); } // ****************************************************************************************** @@ -320,7 +320,7 @@ void EndEffectorsWidget::edit(const std::string& name) current_edit_effector_ = name; // Find the selected in datastruture - srdf::Model::EndEffector* effector = findEffectorByName(name); + srdf::Model::EndEffector* effector = getEndEffector(name); // Set effector name effector_name_field_->setText(effector->name_.c_str()); @@ -356,7 +356,7 @@ void EndEffectorsWidget::edit(const std::string& name) stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode - Q_EMIT isModal(true); + Q_EMIT setModalMode(true); } // ****************************************************************************************** @@ -370,10 +370,10 @@ void EndEffectorsWidget::loadGroupsComboBox() parent_group_name_field_->addItem(""); // optional setting // Add all group names to combo box - for (srdf::Model::Group& group : config_data_->srdf_->groups_) + for (const std::string& group_name : setup_step_.getGroupNames()) { - group_name_field_->addItem(group.name_.c_str()); - parent_group_name_field_->addItem(group.name_.c_str()); + group_name_field_->addItem(group_name.c_str()); + parent_group_name_field_->addItem(group_name.c_str()); } } @@ -386,32 +386,19 @@ void EndEffectorsWidget::loadParentComboBox() parent_name_field_->clear(); // Get all links in robot model - std::vector link_models = config_data_->getRobotModel()->getLinkModels(); - // Add all links to combo box - for (std::vector::const_iterator link_it = link_models.begin(); - link_it < link_models.end(); ++link_it) + for (const std::string& link_name : setup_step_.getLinkNames()) { - parent_name_field_->addItem((*link_it)->getName().c_str()); + parent_name_field_->addItem(link_name.c_str()); } } // ****************************************************************************************** // Find the associated data by name // ****************************************************************************************** -srdf::Model::EndEffector* EndEffectorsWidget::findEffectorByName(const std::string& name) +srdf::Model::EndEffector* EndEffectorsWidget::getEndEffector(const std::string& name) { - // Find the group state we are editing based on the effector name - srdf::Model::EndEffector* searched_group = nullptr; // used for holding our search results - - for (srdf::Model::EndEffector& end_effector : config_data_->srdf_->end_effectors_) - { - if (end_effector.name_ == name) // string match - { - searched_group = &end_effector; // convert to pointer from iterator - break; // we are done searching - } - } + srdf::Model::EndEffector* searched_group = setup_step_.find(name); // Check if effector was found if (searched_group == nullptr) // not found @@ -448,21 +435,10 @@ void EndEffectorsWidget::deleteSelected() return; } - // Delete effector from vector - for (std::vector::iterator effector_it = config_data_->srdf_->end_effectors_.begin(); - effector_it != config_data_->srdf_->end_effectors_.end(); ++effector_it) - { - // check if this is the group we want to delete - if (effector_it->name_ == current_edit_effector_) // string match - { - config_data_->srdf_->end_effectors_.erase(effector_it); - break; - } - } + setup_step_.remove(current_edit_effector_); // Reload main screen table loadDataTable(); - config_data_->changes |= MoveItConfigData::END_EFFECTORS; } // ****************************************************************************************** @@ -473,9 +449,6 @@ void EndEffectorsWidget::doneEditing() // Get a reference to the supplied strings const std::string effector_name = effector_name_field_->text().toStdString(); - // Used for editing existing groups - srdf::Model::EndEffector* searched_data = nullptr; - // Check that name field is not empty if (effector_name.empty()) { @@ -483,29 +456,6 @@ void EndEffectorsWidget::doneEditing() return; } - // Check if this is an existing group - if (!current_edit_effector_.empty()) - { - // Find the group we are editing based on the goup name string - searched_data = findEffectorByName(current_edit_effector_); - } - - // Check that the effector name is unique - for (const auto& eef : config_data_->srdf_->end_effectors_) - { - if (eef.name_ == effector_name) - { - // is this our existing effector? check if effector pointers are same - if (&eef != searched_data) - { - QMessageBox::warning( - this, "Error Saving", - QString("An end-effector named '").append(effector_name.c_str()).append("'already exists!")); - return; - } - } - } - // Check that a group was selected if (group_name_field_->currentText().isEmpty()) { @@ -520,22 +470,23 @@ void EndEffectorsWidget::doneEditing() return; } - const moveit::core::JointModelGroup* jmg = - config_data_->getRobotModel()->getJointModelGroup(group_name_field_->currentText().toStdString()); /* - if (jmg->hasLinkModel(parent_name_field_->currentText().toStdString())) + if (!setup_step_.isLinkInGroup(parent_name_field_->currentText().toStdString(), + group_name_field_->currentText().toStdString())) { - QMessageBox::warning( this, "Error Saving", QString::fromStdString("Group " + - group_name_field_->currentText().toStdString() + " contains the link " + - parent_name_field_->currentText().toStdString() + ". However, the parent link of the end-effector should not belong to - the group for the end-effector itself.")); + QMessageBox::warning(this, "Error Saving", + QString::fromStdString("Group " + group_name_field_->currentText().toStdString() + + " contains the link " + parent_name_field_->currentText().toStdString() + + ". However, the parent link of the end-effector should not belong to " + "the group for the end-effector itself.")); return; } */ + if (!parent_group_name_field_->currentText().isEmpty()) { - jmg = config_data_->getRobotModel()->getJointModelGroup(parent_group_name_field_->currentText().toStdString()); - if (!jmg->hasLinkModel(parent_name_field_->currentText().toStdString())) + if (!setup_step_.isLinkInGroup(parent_name_field_->currentText().toStdString(), + parent_group_name_field_->currentText().toStdString())) { QMessageBox::warning(this, "Error Saving", QString::fromStdString("The specified parent group '" + @@ -546,29 +497,20 @@ void EndEffectorsWidget::doneEditing() } } - config_data_->changes |= MoveItConfigData::END_EFFECTORS; - // Save the new effector name or create the new effector ---------------------------- - bool is_new = false; - if (searched_data == nullptr) // create new + try { - is_new = true; - - searched_data = new srdf::Model::EndEffector(); + srdf::Model::EndEffector* searched_data = setup_step_.get(effector_name, current_edit_effector_); + // Copy name data ---------------------------------------------------- + setup_step_.setProperties(searched_data, parent_name_field_->currentText().toStdString(), + group_name_field_->currentText().toStdString(), + parent_group_name_field_->currentText().toStdString()); } - - // Copy name data ---------------------------------------------------- - searched_data->name_ = effector_name; - searched_data->parent_link_ = parent_name_field_->currentText().toStdString(); - searched_data->component_group_ = group_name_field_->currentText().toStdString(); - searched_data->parent_group_ = parent_group_name_field_->currentText().toStdString(); - - // Insert new effectors into group state vector -------------------------- - if (is_new) + catch (const std::runtime_error& e) { - config_data_->srdf_->end_effectors_.push_back(*searched_data); - delete searched_data; + QMessageBox::warning(this, "Error Saving", e.what()); + return; } // Finish up ------------------------------------------------------ @@ -580,7 +522,7 @@ void EndEffectorsWidget::doneEditing() stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode - Q_EMIT isModal(false); + Q_EMIT setModalMode(false); } // ****************************************************************************************** @@ -595,7 +537,7 @@ void EndEffectorsWidget::cancelEditing() previewClicked(0, 0); // the number parameters are actually meaningless // Announce that this widget is not in modal mode - Q_EMIT isModal(false); + Q_EMIT setModalMode(false); } // ****************************************************************************************** @@ -608,12 +550,14 @@ void EndEffectorsWidget::loadDataTable() data_table_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called data_table_->clearContents(); + const auto& end_effectors = setup_step_.getEndEffectors(); + // Set size of datatable - data_table_->setRowCount(config_data_->srdf_->end_effectors_.size()); + data_table_->setRowCount(end_effectors.size()); // Loop through every end effector int row = 0; - for (const auto& eef : config_data_->srdf_->end_effectors_) + for (const auto& eef : end_effectors) { // Create row elements QTableWidgetItem* data_name = new QTableWidgetItem(eef.name_.c_str()); @@ -646,7 +590,7 @@ void EndEffectorsWidget::loadDataTable() data_table_->resizeColumnToContents(3); // Show edit button if applicable - if (!config_data_->srdf_->end_effectors_.empty()) + if (!end_effectors.empty()) btn_edit_->show(); } @@ -666,4 +610,7 @@ void EndEffectorsWidget::focusGiven() loadParentComboBox(); } -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup_srdf_plugins::EndEffectorsWidget, moveit_setup_framework::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_edit_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_edit_widget.cpp index 86944edcef..30274b04bf 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_edit_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_edit_widget.cpp @@ -46,16 +46,14 @@ #include #include -#include "group_edit_widget.h" -#include // for loading all avail kinematic planners +#include -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { // ****************************************************************************************** // // ****************************************************************************************** -GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : QWidget(parent), config_data_(config_data) +GroupEditWidget::GroupEditWidget(QWidget* parent, PlanningGroups& setup_step) : QWidget(parent), setup_step_(setup_step) { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -225,32 +223,20 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con // ****************************************************************************************** // Set the link field with previous value // ****************************************************************************************** -void GroupEditWidget::setSelected(const std::string& group_name) +void GroupEditWidget::setSelected(const std::string& group_name, const GroupMetaData& meta_data) { group_name_field_->setText(QString(group_name.c_str())); // Load properties from moveit_config_data.cpp ---------------------------------------------- // Load resolution - double* resolution = &config_data_->group_meta_data_[group_name].kinematics_solver_search_resolution_; - if (*resolution == 0) - { - // Set default value - *resolution = DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION; - } - kinematics_resolution_field_->setText(QString::number(*resolution)); + kinematics_resolution_field_->setText(QString::number(meta_data.kinematics_solver_search_resolution_)); // Load timeout - double* timeout = &config_data_->group_meta_data_[group_name].kinematics_solver_timeout_; - if (*timeout == 0) - { - // Set default value - *timeout = DEFAULT_KIN_SOLVER_TIMEOUT; - } - kinematics_timeout_field_->setText(QString::number(*timeout)); + kinematics_timeout_field_->setText(QString::number(meta_data.kinematics_solver_timeout_)); // Set kin solver - std::string kin_solver = config_data_->group_meta_data_[group_name].kinematics_solver_; + std::string kin_solver = meta_data.kinematics_solver_; // If this group doesn't have a solver, reset it to 'None' if (kin_solver.empty()) @@ -274,11 +260,10 @@ void GroupEditWidget::setSelected(const std::string& group_name) kinematics_solver_field_->setCurrentIndex(index); } - kinematics_parameters_file_field_->setText( - config_data_->group_meta_data_[group_name].kinematics_parameters_file_.c_str()); + kinematics_parameters_file_field_->setText(meta_data.kinematics_parameters_file_.c_str()); // Set default planner - std::string default_planner = config_data_->group_meta_data_[group_name].default_planner_; + std::string default_planner = meta_data.default_planner_; // If this group doesn't have a solver, reset it to 'None' if (default_planner.empty()) @@ -317,32 +302,15 @@ void GroupEditWidget::loadKinematicPlannersComboBox() kinematics_solver_field_->addItem("None"); default_planner_field_->addItem("None"); - // load all avail kin planners - std::unique_ptr> loader; + std::vector classes; try { - loader = std::make_unique>("moveit_core", - "kinematics::KinematicsBase"); + classes = setup_step_.getKinematicPlanners(); } - catch (pluginlib::PluginlibException& ex) + catch (const std::runtime_error& ex) { - QMessageBox::warning(this, "Missing Kinematic Solvers", - "Exception while creating class loader for kinematic " - "solver plugins"); - ROS_ERROR_STREAM(ex.what()); - return; - } - - // Get classes - const std::vector& classes = loader->getDeclaredClasses(); - - // Warn if no plugins are found - if (classes.empty()) - { - QMessageBox::warning(this, "Missing Kinematic Solvers", - "No MoveIt-compatible kinematics solvers found. Try " - "installing moveit_kinematics (sudo apt-get install " - "ros-${ROS_DISTRO}-moveit-kinematics)"); + QMessageBox::warning(this, "Missing Kinematic Solvers", QString(ex.what())); + RCLCPP_ERROR_STREAM(setup_step_.getLogger(), ex.what()); return; } @@ -352,10 +320,8 @@ void GroupEditWidget::loadKinematicPlannersComboBox() kinematics_solver_field_->addItem(kinematics_plugin_name.c_str()); } - std::vector planners = config_data_->getOMPLPlanners(); - for (OMPLPlannerDescription& planner : planners) + for (const std::string& planner_name : setup_step_.getOMPLPlanners()) { - std::string planner_name = planner.name_; default_planner_field_->addItem(planner_name.c_str()); } } @@ -372,7 +338,7 @@ void GroupEditWidget::selectKinematicsFile() std::string package_name; std::string relative_filename; bool package_found = - config_data_->extractPackageNameFromPath(filename.toStdString(), package_name, relative_filename); + moveit_setup_framework::extractPackageNameFromPath(filename.toStdString(), package_name, relative_filename); QString lookup_path = filename; if (package_found) @@ -382,4 +348,4 @@ void GroupEditWidget::selectKinematicsFile() kinematics_parameters_file_field_->setText(lookup_path); } -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_meta_config.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_meta_config.cpp new file mode 100644 index 0000000000..0fa1aa70b1 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_meta_config.cpp @@ -0,0 +1,199 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Metro Robots nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ + +#include + +namespace moveit_setup_srdf_plugins +{ +using moveit_setup_framework::getYamlProperty; + +bool GroupMetaConfig::isConfigured() const +{ + return !group_meta_data_.empty(); +} + +// ****************************************************************************************** +// Input kinematics.yaml file +// ****************************************************************************************** +void GroupMetaConfig::loadPrevious(const std::string& package_path, const YAML::Node& /* node */) +{ + std::string file_path = package_path + "/" + KINEMATICS_FILE; + if (!inputKinematicsYAML(file_path)) + { + throw std::runtime_error("Failed to parse kinematics yaml file. This file is not critical but any previous " + "kinematic solver settings have been lost. To re-populate this file edit each " + "existing planning group and choose a solver, then save each change."); + } +} + +void GroupMetaConfig::deleteGroup(const std::string& group_name) +{ + group_meta_data_.erase(group_name); + changed_ = true; +} + +void GroupMetaConfig::renameGroup(const std::string& old_group_name, const std::string& new_group_name) +{ + group_meta_data_[new_group_name] = group_meta_data_[old_group_name]; + group_meta_data_.erase(old_group_name); + changed_ = true; +} + +const GroupMetaData& GroupMetaConfig::getMetaData(const std::string& group_name) const +{ + const auto& match = group_meta_data_.find(group_name); + if (match != group_meta_data_.end()) + { + return match->second; + } + else + { + return default_values_; + } +} + +void GroupMetaConfig::setMetaData(const std::string& group_name, const GroupMetaData& meta_data) +{ + group_meta_data_[group_name] = meta_data; + changed_ = true; +} + +bool GroupMetaConfig::inputKinematicsYAML(const std::string& file_path) +{ + // Load file + std::ifstream input_stream(file_path.c_str()); + if (!input_stream.good()) + { + return false; + } + + // Begin parsing + try + { + YAML::Node doc = YAML::Load(input_stream); + + // Loop through all groups + for (YAML::const_iterator group_it = doc.begin(); group_it != doc.end(); ++group_it) + { + const std::string& group_name = group_it->first.as(); + const YAML::Node& group = group_it->second; + + // Create new meta data + GroupMetaData meta_data; + + getYamlProperty(group, "kinematics_solver", meta_data.kinematics_solver_); + getYamlProperty(group, "kinematics_solver_search_resolution", meta_data.kinematics_solver_search_resolution_, + DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION); + getYamlProperty(group, "kinematics_solver_timeout", meta_data.kinematics_solver_timeout_, + DEFAULT_KIN_SOLVER_TIMEOUT); + + // Assign meta data to vector + group_meta_data_[group_name] = meta_data; + } + + return true; + } + catch (YAML::ParserException& e) // Catch errors + { + return false; + } +} + +// ****************************************************************************************** +// Output kinematic config files +// ****************************************************************************************** +bool GroupMetaConfig::GeneratedGroupMetaConfig::writeYaml(YAML::Emitter& emitter) +{ + emitter << YAML::BeginMap; + + // Output every group and the kinematic solver it can use ---------------------------------- + for (const auto& meta_pair : parent_.group_meta_data_) + { + const std::string& group_name = meta_pair.first; + const GroupMetaData& meta_data = meta_pair.second; + + // Only save kinematic data if the solver is not "None" + if (meta_data.kinematics_solver_.empty() || meta_data.kinematics_solver_ == "None") + continue; + + emitter << YAML::Key << group_name; + emitter << YAML::Value << YAML::BeginMap; + + // Kinematic Solver + emitter << YAML::Key << "kinematics_solver"; + emitter << YAML::Value << meta_data.kinematics_solver_; + + // Search Resolution + emitter << YAML::Key << "kinematics_solver_search_resolution"; + emitter << YAML::Value << meta_data.kinematics_solver_search_resolution_; + + // Solver Timeout + emitter << YAML::Key << "kinematics_solver_timeout"; + emitter << YAML::Value << meta_data.kinematics_solver_timeout_; + + emitter << YAML::EndMap; + } + + emitter << YAML::EndMap; + return true; +} + +void GroupMetaConfig::collectVariables(std::vector& variables) +{ + // Add parameter files for the kinematics solvers that should be loaded + // in addition to kinematics.yaml by planning_context.launch + std::string kinematics_parameters_files_block; + for (const auto& groups : group_meta_data_) + { + if (groups.second.kinematics_parameters_file_.empty()) + continue; + + // add a linebreak if we have more than one entry + if (!kinematics_parameters_files_block.empty()) + kinematics_parameters_files_block += "\n"; + + std::string line = " "; + kinematics_parameters_files_block += line; + } + variables.push_back(moveit_setup_framework::TemplateVariable("KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK", + kinematics_parameters_files_block)); +} + +} // namespace moveit_setup_srdf_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup_srdf_plugins::GroupMetaConfig, moveit_setup_framework::SetupConfig) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/kinematic_chain_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/kinematic_chain_widget.cpp index 4b6b348cc0..da5fb80d19 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/kinematic_chain_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/kinematic_chain_widget.cpp @@ -43,15 +43,15 @@ #include #include #include -#include "kinematic_chain_widget.h" +#include -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { // ****************************************************************************************** // Constructor // ****************************************************************************************** -KinematicChainWidget::KinematicChainWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : QWidget(parent), config_data_(config_data) +KinematicChainWidget::KinematicChainWidget(QWidget* parent, moveit_setup_framework::RVizPanel* rviz_panel) + : QWidget(parent), rviz_panel_(rviz_panel) { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -138,19 +138,14 @@ KinematicChainWidget::KinematicChainWidget(QWidget* parent, const MoveItConfigDa // ****************************************************************************************** // Load robot links into tree // ****************************************************************************************** -void KinematicChainWidget::setAvailable() +void KinematicChainWidget::setAvailable(const LinkNameTree& link_name_tree) { // Only load the kinematic chain once if (kinematic_chain_loaded_) return; - // Retrieve pointer to the shared kinematic model - const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); - - // Get the root joint - const moveit::core::JointModel* root_joint = model->getRootJoint(); - - addLinktoTreeRecursive(root_joint->getChildLinkModel(), nullptr); + QTreeWidgetItem* root_item = addLinkChildRecursive(nullptr, link_name_tree); + link_tree_->addTopLevelItem(root_item); // Remember that we have loaded the chain kinematic_chain_loaded_ = true; @@ -159,60 +154,17 @@ void KinematicChainWidget::setAvailable() // ****************************************************************************************** // // ****************************************************************************************** -void KinematicChainWidget::addLinktoTreeRecursive(const moveit::core::LinkModel* link, - const moveit::core::LinkModel* parent) -{ - // Create new tree item - QTreeWidgetItem* new_item = new QTreeWidgetItem(link_tree_); - - // Add item to tree - if (parent == nullptr) - { - new_item->setText(0, link->getName().c_str()); - link_tree_->addTopLevelItem(new_item); - } - else - { - for (int i = 0; i < link_tree_->topLevelItemCount(); ++i) - { - if (addLinkChildRecursive(link_tree_->topLevelItem(i), link, parent->getName())) - { - break; - } - } - } - for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i) - { - addLinktoTreeRecursive(link->getChildJointModels()[i]->getChildLinkModel(), link); - } -} - -// ****************************************************************************************** -// -// ****************************************************************************************** -bool KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const moveit::core::LinkModel* link, - const std::string& parent_name) +QTreeWidgetItem* KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const LinkNameTree& link) { - if (parent->text(0).toStdString() == parent_name) - { - QTreeWidgetItem* new_item = new QTreeWidgetItem(parent); - new_item->setText(0, link->getName().c_str()); + QTreeWidgetItem* new_item = new QTreeWidgetItem(parent); + new_item->setText(0, link.data.c_str()); - parent->addChild(new_item); - return true; - } - else + for (const LinkNameTree& child : link.children) { - for (int i = 0; i < parent->childCount(); ++i) - { - if (addLinkChildRecursive(parent->child(i), link, parent_name)) - { - return true; - } - } + QTreeWidgetItem* new_child = addLinkChildRecursive(new_item, child); + new_item->addChild(new_child); } - - return false; + return new_item; } // ****************************************************************************************** @@ -267,7 +219,7 @@ void KinematicChainWidget::itemSelected() QTreeWidgetItem* item = link_tree_->currentItem(); if (item != nullptr) { - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); std::string name = item->text(0).toStdString(); @@ -276,7 +228,7 @@ void KinematicChainWidget::itemSelected() return; // Check that item is not empty - Q_EMIT highlightLink(item->text(0).toStdString(), QColor(255, 0, 0)); + rviz_panel_->highlightLink(item->text(0).toStdString(), QColor(255, 0, 0)); } } -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints.cpp new file mode 100644 index 0000000000..3be7e76099 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints.cpp @@ -0,0 +1,82 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ + +#include + +namespace moveit_setup_srdf_plugins +{ +std::vector PassiveJoints::getActiveJoints() const +{ + std::vector active_joints; + + // Retrieve pointer to the shared kinematic model + const moveit::core::RobotModelConstPtr& model = srdf_config_->getRobotModel(); + + // Get the names of the all joints + for (const std::string& joint : model->getJointModelNames()) + { + if (model->getJointModel(joint)->getVariableCount() > 0) + { + active_joints.push_back(joint); + } + } + return active_joints; +} + +std::vector PassiveJoints::getPassiveJoints() const +{ + std::vector passive_joints; + for (const srdf::Model::PassiveJoint& passive_joint : srdf_config_->getPassiveJoints()) + { + passive_joints.push_back(passive_joint.name_); + } + return passive_joints; +} + +void PassiveJoints::setPassiveJoints(const std::vector& passive_joint_names) +{ + std::vector& passive_joints = srdf_config_->getPassiveJoints(); + passive_joints.clear(); + for (const std::string& passive_joint : passive_joint_names) + { + srdf::Model::PassiveJoint pj; + pj.name_ = passive_joint; + passive_joints.push_back(pj); + } + srdf_config_->updateRobotModel(moveit_setup_framework::PASSIVE_JOINTS); +} + +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints_widget.cpp index 85610b4404..63495c9cb5 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints_widget.cpp @@ -35,9 +35,8 @@ /* Author: Dave Coleman */ // SA -#include "passive_joints_widget.h" -#include "header_widget.h" -#include "double_list_widget.h" +#include +#include // Qt #include @@ -45,27 +44,26 @@ #include #include -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { // ****************************************************************************************** // Constructor // ****************************************************************************************** -PassiveJointsWidget::PassiveJointsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +void PassiveJointsWidget::onInit() { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); // Top Header Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget("Define Passive Joints", - "Specify the set of passive joints (not actuated). Joint " - "state is not expected to be published for these joints.", - this); + auto header = new moveit_setup_framework::HeaderWidget("Define Passive Joints", + "Specify the set of passive joints (not actuated). Joint " + "state is not expected to be published for these joints.", + this); layout->addWidget(header); // Joints edit widget - joints_widget_ = new DoubleListWidget(this, config_data_, "Joint Collection", "Joint", false); + joints_widget_ = new moveit_setup_framework::DoubleListWidget(this, "Joint Collection", "Joint", false); connect(joints_widget_, SIGNAL(selectionUpdated()), this, SLOT(selectionUpdated())); connect(joints_widget_, SIGNAL(previewSelected(std::vector)), this, SLOT(previewSelectedJoints(std::vector))); @@ -88,29 +86,18 @@ void PassiveJointsWidget::focusGiven() { joints_widget_->clearContents(); - // Retrieve pointer to the shared kinematic model - const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); + std::vector active_joints = setup_step_.getActiveJoints(); - // Get the names of the all joints - const std::vector& joints = model->getJointModelNames(); - - if (joints.empty()) + if (active_joints.empty()) { QMessageBox::critical(this, "Error Loading", "No joints found for robot model"); return; } - std::vector active_joints; - for (const std::string& joint : joints) - if (model->getJointModel(joint)->getVariableCount() > 0) - active_joints.push_back(joint); // Set the available joints (left box) joints_widget_->setAvailable(active_joints); - std::vector passive_joints; - for (srdf::Model::PassiveJoint& passive_joint : config_data_->srdf_->passive_joints_) - passive_joints.push_back(passive_joint.name_); - joints_widget_->setSelected(passive_joints); + joints_widget_->setSelected(setup_step_.getPassiveJoints()); } // ****************************************************************************************** @@ -118,14 +105,7 @@ void PassiveJointsWidget::focusGiven() // ****************************************************************************************** void PassiveJointsWidget::selectionUpdated() { - config_data_->srdf_->passive_joints_.clear(); - for (int i = 0; i < joints_widget_->selected_data_table_->rowCount(); ++i) - { - srdf::Model::PassiveJoint pj; - pj.name_ = joints_widget_->selected_data_table_->item(i, 0)->text().toStdString(); - config_data_->srdf_->passive_joints_.push_back(pj); - } - config_data_->changes |= MoveItConfigData::PASSIVE_JOINTS; + setup_step_.setPassiveJoints(joints_widget_->getSelectedValues()); } // ****************************************************************************************** @@ -134,29 +114,23 @@ void PassiveJointsWidget::selectionUpdated() void PassiveJointsWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); for (const std::string& joint : joints) { - const moveit::core::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); - - // Check that a joint model was found - if (!joint_model) - { - continue; - } - - // Get the name of the link - const std::string link = joint_model->getChildLinkModel()->getName(); - + std::string link = setup_step_.getChildOfJoint(joint); + // Check that a joint model/link was found if (link.empty()) { continue; } // Highlight link - Q_EMIT highlightLink(link, QColor(255, 0, 0)); + rviz_panel_->highlightLink(link, QColor(255, 0, 0)); } } -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup_srdf_plugins::PassiveJointsWidget, moveit_setup_framework::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp new file mode 100644 index 0000000000..64896b143c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp @@ -0,0 +1,445 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ + +#include +#include +#include // for loading all avail kinematic planners + +//// Cycle checking +#include +#include + +namespace moveit_setup_srdf_plugins +{ +// Used for checking for cycles in a subgroup hierarchy +struct CycleDetector : public boost::dfs_visitor<> +{ + CycleDetector(bool& has_cycle) : m_has_cycle(has_cycle) + { + } + + template + void backEdge(Edge /*unused*/, Graph& /*unused*/) + { + m_has_cycle = true; + } + +protected: + bool& m_has_cycle; +}; + +void PlanningGroups::onInit() +{ + SuperSRDFStep::onInit(); + config_data_->registerType("group_meta", "moveit_setup_srdf_plugins::GroupMetaConfig"); + group_meta_config_ = config_data_->get("group_meta"); +} + +void PlanningGroups::renameGroup(const std::string& old_group_name, const std::string& new_group_name) +{ + // Rename the actual group + rename(old_group_name, new_group_name); + + long changes = 0L; + + // Change all references to this group name in other subgroups + // Loop through every group + for (srdf::Model::Group& group : srdf_config_->getGroups()) + { + // Loop through every subgroup + for (std::string& subgroup : group.subgroups_) + { + // Check if that subgroup references old group name. if so, update it + if (subgroup == old_group_name) // same name + { + subgroup.assign(new_group_name); // updated + changes |= moveit_setup_framework::GROUP_CONTENTS; + } + } + } + + // Change all references to this group name in the end effectors screen + for (srdf::Model::EndEffector& eef : srdf_config_->getEndEffectors()) + { + // Check if this eef's parent group references old group name. if so, update it + if (eef.parent_group_ == old_group_name) // same name + { + RCLCPP_DEBUG_STREAM((*logger_), "Changed eef '" << eef.name_ << "' to new parent group name " << new_group_name); + eef.parent_group_ = new_group_name; // updated + changes |= moveit_setup_framework::END_EFFECTORS; + } + + // Check if this eef's group references old group name. if so, update it + if (eef.component_group_.compare(old_group_name) == 0) // same name + { + RCLCPP_DEBUG_STREAM((*logger_), "Changed eef '" << eef.name_ << "' to new group name " << new_group_name); + eef.component_group_ = new_group_name; // updated + changes |= moveit_setup_framework::END_EFFECTORS; + } + } + + // Change all references to this group name in the robot poses screen + for (srdf::Model::GroupState& gs : srdf_config_->getGroupStates()) + { + // Check if this eef's parent group references old group name. if so, update it + if (gs.group_ == old_group_name) // same name + { + RCLCPP_DEBUG_STREAM((*logger_), "Changed group state group '" << gs.group_ << "' to new parent group name " + << new_group_name); + gs.group_ = new_group_name; // updated + changes |= moveit_setup_framework::POSES; + } + } + + group_meta_config_->renameGroup(old_group_name, new_group_name); + changes |= moveit_setup_framework::GROUPS; + + // Now update the robot model based on our changed to the SRDF + srdf_config_->updateRobotModel(changes); +} + +void PlanningGroups::deleteGroup(const std::string& group_name) +{ + long changes = 0L; + + // Remove poses in this group + for (const std::string& pose_name : getPosesByGroup(group_name)) + { + srdf_config_->removePoseByName(pose_name, group_name); + } + + // Remove end effectors in this group + auto& eefs = srdf_config_->getEndEffectors(); + auto it = eefs.begin(); + while (it != eefs.end()) + { + if (it->component_group_ == group_name) + { + it = eefs.erase(it); + changes |= moveit_setup_framework::END_EFFECTORS; + } + else + { + it++; + } + } + + // delete actual group + remove(group_name); + + // Delete references in subgroups + for (srdf::Model::Group& group : srdf_config_->getGroups()) + { + auto& subgroups = group.subgroups_; + std::vector::iterator subgroup_it = std::find(subgroups.begin(), subgroups.end(), group_name); + while (subgroup_it != subgroups.end()) + { + subgroups.erase(subgroup_it); + changes |= moveit_setup_framework::GROUP_CONTENTS; + subgroup_it = std::find(subgroups.begin(), subgroups.end(), group_name); + } + } + + group_meta_config_->deleteGroup(group_name); + + srdf_config_->updateRobotModel(changes); +} // namespace moveit_setup_srdf_plugins + +void PlanningGroups::setJoints(const std::string& group_name, const std::vector& joint_names) +{ + // Find the group we are editing based on the group name string + srdf::Model::Group* searched_group = find(group_name); + + // save the data + searched_group->joints_ = joint_names; + + // Update the kinematic model with changes + srdf_config_->updateRobotModel(moveit_setup_framework::GROUP_CONTENTS); +} + +void PlanningGroups::setLinks(const std::string& group_name, const std::vector& link_names) +{ + // Find the group we are editing based on the group name string + srdf::Model::Group* searched_group = find(group_name); + + // save the data + searched_group->links_ = link_names; + + // Update the kinematic model with changes + srdf_config_->updateRobotModel(moveit_setup_framework::GROUP_CONTENTS); +} + +void PlanningGroups::setChain(const std::string& group_name, const std::string& base, const std::string& tip) +{ + // Check that box the tip and base, or neither, have text + if ((!tip.empty() && base.empty()) || (tip.empty() && !base.empty())) + { + throw std::runtime_error("You must specify a link for both the base and tip, or leave both " + "blank."); + } + + // Check that both given links are valid links, unless they are both blank + if (!tip.empty() && !base.empty()) + { + // Check that they are not the same link + if (tip.compare(base) == 0) // they are same + { + throw std::runtime_error("Tip and base link cannot be the same link."); + } + + bool found_tip = false; + bool found_base = false; + const std::vector& links = getLinkNames(); + + for (const std::string& link : links) + { + // Check if string matches either of user specified links + if (link.compare(tip) == 0) // they are same + found_tip = true; + else if (link.compare(base) == 0) // they are same + found_base = true; + + // Check if we are done searching + if (found_tip && found_base) + break; + } + + // Check if we found both links + if (!found_tip || !found_base) + { + throw std::runtime_error("Tip or base link(s) were not found in kinematic chain."); + } + } + + // Find the group we are editing based on the group name string + srdf::Model::Group* searched_group = find(group_name); + + // clear the old data + searched_group->chains_.clear(); + + // Save the data if there is data to save + if (!tip.empty() && !base.empty()) + { + searched_group->chains_.push_back(std::pair(base, tip)); + } + + // Update the kinematic model with changes + srdf_config_->updateRobotModel(moveit_setup_framework::GROUP_CONTENTS); +} + +void PlanningGroups::setSubgroups(const std::string& selected_group_name, const std::vector& subgroups) +{ + // Check for cycles ------------------------------- + + // Create vector index of all nodes + std::map group_nodes; + + // Create vector of all nodes for use as id's + int node_id = 0; + for (const std::string& group_name : getGroupNames()) + { + // Add string to vector + group_nodes.insert(std::pair(group_name, node_id)); + ++node_id; + } + + // Create the empty graph + typedef boost::adjacency_list Graph; + Graph g(group_nodes.size()); + + // Traverse the group list again, this time inserting subgroups into graph + int from_id = 0; // track the outer node we are on to reduce searches performed + for (srdf::Model::Group& group : srdf_config_->getGroups()) + { + // Check if group_it is same as current group + if (group.name_ == selected_group_name) // yes, same group + { + // add new subgroup list from widget, not old one. this way we can check for new cycles + for (const std::string& to_string : subgroups) + { + // convert subgroup string to associated id + int to_id = group_nodes[to_string]; + + // Add edge from from_id to to_id + add_edge(from_id, to_id, g); + } + } + else // this group is not the group we are editing, so just add subgroups from memory + { + // add new subgroup list from widget, not old one. this way we can check for new cycles + for (const std::string& to_string : group.subgroups_) + { + // Get std::string of subgroup + // convert subgroup string to associated id + int to_id = group_nodes[to_string]; + + // Add edge from from_id to to_id + add_edge(from_id, to_id, g); + } + } + + ++from_id; + } + + // Check for cycles + bool has_cycle = false; + CycleDetector vis(has_cycle); + boost::depth_first_search(g, visitor(vis)); + + if (has_cycle) + { + throw std::runtime_error("Depth first search reveals a cycle in the subgroups"); + } + + // Find the group we are editing based on the group name string + srdf::Model::Group* searched_group = find(selected_group_name); + + // save the data + searched_group->subgroups_ = subgroups; + + // Update the kinematic model with changes + srdf_config_->updateRobotModel(moveit_setup_framework::GROUP_CONTENTS); +} + +std::string PlanningGroups::getChildOfJoint(const std::string& joint_name) const +{ + return srdf_config_->getChildOfJoint(joint_name); +} + +std::string PlanningGroups::getJointType(const std::string& joint_name) const +{ + const moveit::core::JointModel* joint_model = srdf_config_->getRobotModel()->getJointModel(joint_name); + if (!joint_model) + { + return ""; + } + return joint_model->getTypeName(); +} + +LinkNameTree PlanningGroups::getLinkNameTree() const +{ + const moveit::core::JointModel* root_joint = srdf_config_->getRobotModel()->getRootJoint(); + return buildLinkNameTree(root_joint->getChildLinkModel()); +} + +std::vector PlanningGroups::getPosesByGroup(const std::string& group_name) const +{ + std::vector pose_names; + for (const srdf::Model::GroupState& pose : srdf_config_->getGroupStates()) + { + if (pose.group_ == group_name) + { + pose_names.push_back(pose.name_); + } + } + return pose_names; +} + +std::vector PlanningGroups::getEndEffectorsByGroup(const std::string& group_name) const +{ + std::vector eef_names; + for (const srdf::Model::EndEffector& eef : srdf_config_->getEndEffectors()) + { + if (eef.component_group_ == group_name) + { + eef_names.push_back(eef.name_); + } + } + return eef_names; +} + +std::vector PlanningGroups::getKinematicPlanners() const +{ + // load all avail kin planners + std::unique_ptr> loader; + try + { + loader = std::make_unique>("moveit_core", + "kinematics::KinematicsBase"); + } + catch (pluginlib::PluginlibException& ex) + { + throw std::runtime_error(std::string("Exception while creating class loader for kinematic " + "solver plugins: ") + + ex.what()); + } + + std::vector planners(loader->getDeclaredClasses()); + + // Warn if no plugins are found + if (planners.empty()) + { + throw std::runtime_error("No MoveIt-compatible kinematics solvers found. Try " + "installing moveit_kinematics (sudo apt-get install " + "ros-${ROS_DISTRO}-moveit-kinematics)"); + } + return planners; +} + +std::vector PlanningGroups::getOMPLPlanners() const +{ + std::vector planner_names; + // TODO: This should call ompl_interface::PlanningContextManager::getRegisteredPlannerAllocators to load the + // names dynamically + planner_names.push_back("AnytimePathShortening"); + planner_names.push_back("SBL"); + planner_names.push_back("EST"); + planner_names.push_back("LBKPIECE"); + planner_names.push_back("BKPIECE"); + planner_names.push_back("KPIECE"); + planner_names.push_back("RRT"); + planner_names.push_back("RRTConnect"); + planner_names.push_back("RRTstar"); + planner_names.push_back("TRRT"); + planner_names.push_back("PRM"); + planner_names.push_back("PRMstar"); + planner_names.push_back("FMT"); + planner_names.push_back("BFMT"); + planner_names.push_back("PDST"); + planner_names.push_back("STRIDE"); + planner_names.push_back("BiTRRT"); + planner_names.push_back("LBTRRT"); + planner_names.push_back("BiEST"); + planner_names.push_back("ProjEST"); + planner_names.push_back("LazyPRM"); + planner_names.push_back("LazyPRMstar"); + planner_names.push_back("SPARS"); + planner_names.push_back("SPARStwo"); + + return planner_names; +} + +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp index 4fd262dae7..6a1d72f410 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp @@ -47,11 +47,10 @@ */ // ****************************************************************************************** -#include "header_widget.h" -#include "planning_groups_widget.h" -#include "double_list_widget.h" // for joints, links and subgroups pages -#include "kinematic_chain_widget.h" // for kinematic chain page -#include "group_edit_widget.h" // for group rename page +#include +#include +#include + // Qt #include #include @@ -66,43 +65,21 @@ #include #include -//// Cycle checking -#include -#include - -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { // Name of rviz topic in ROS static const std::string VIS_TOPIC_NAME = "planning_components_visualization"; -// Used for checking for cycles in a subgroup hierarchy -struct CycleDetector : public boost::dfs_visitor<> -{ - CycleDetector(bool& has_cycle) : m_has_cycle(has_cycle) - { - } - - template - void backEdge(Edge /*unused*/, Graph& /*unused*/) - { - m_has_cycle = true; - } - -protected: - bool& m_has_cycle; -}; - // ****************************************************************************************** // Constructor // ****************************************************************************************** -PlanningGroupsWidget::PlanningGroupsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +void PlanningGroupsWidget::onInit() { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); // Top Label Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget( + auto header = new moveit_setup_framework::HeaderWidget( "Define Planning Groups", "Create and edit 'joint model' groups for your robot based on joint collections, " "link collections, kinematic chains or subgroups. " @@ -118,36 +95,33 @@ PlanningGroupsWidget::PlanningGroupsWidget(QWidget* parent, const MoveItConfigDa groups_tree_widget_ = createContentsWidget(); // included in this file // Joints edit widget - joints_widget_ = new DoubleListWidget(this, config_data_, "Joint Collection", "Joint"); + joints_widget_ = new moveit_setup_framework::DoubleListWidget(this, "Joint Collection", "Joint"); connect(joints_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing())); connect(joints_widget_, SIGNAL(doneEditing()), this, SLOT(saveJointsScreen())); connect(joints_widget_, SIGNAL(previewSelected(std::vector)), this, SLOT(previewSelectedJoints(std::vector))); // Links edit widget - links_widget_ = new DoubleListWidget(this, config_data_, "Link Collection", "Link"); + links_widget_ = new moveit_setup_framework::DoubleListWidget(this, "Link Collection", "Link"); connect(links_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing())); connect(links_widget_, SIGNAL(doneEditing()), this, SLOT(saveLinksScreen())); connect(links_widget_, SIGNAL(previewSelected(std::vector)), this, SLOT(previewSelectedLink(std::vector))); // Chain Widget - chain_widget_ = new KinematicChainWidget(this, config_data); + chain_widget_ = new KinematicChainWidget(this, rviz_panel_); connect(chain_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing())); connect(chain_widget_, SIGNAL(doneEditing()), this, SLOT(saveChainScreen())); - connect(chain_widget_, SIGNAL(unhighlightAll()), this, SIGNAL(unhighlightAll())); - connect(chain_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SIGNAL(highlightLink(const std::string&, const QColor&))); // Subgroups Widget - subgroups_widget_ = new DoubleListWidget(this, config_data_, "Subgroup", "Subgroup"); + subgroups_widget_ = new moveit_setup_framework::DoubleListWidget(this, "Subgroup", "Subgroup"); connect(subgroups_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing())); connect(subgroups_widget_, SIGNAL(doneEditing()), this, SLOT(saveSubgroupsScreen())); connect(subgroups_widget_, SIGNAL(previewSelected(std::vector)), this, SLOT(previewSelectedSubgroup(std::vector))); // Group Edit Widget - group_edit_widget_ = new GroupEditWidget(this, config_data_); + group_edit_widget_ = new GroupEditWidget(this, setup_step_); connect(group_edit_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing())); connect(group_edit_widget_, SIGNAL(deleteGroup()), this, SLOT(deleteGroup())); connect(group_edit_widget_, SIGNAL(save()), this, SLOT(saveGroupScreenEdit())); @@ -253,7 +227,8 @@ void PlanningGroupsWidget::loadGroupsTree() groups_tree_->clear(); // reset the tree // Display all groups by looping through them - for (srdf::Model::Group& group : config_data_->srdf_->groups_) + std::vector& groups = setup_step_.getContainer(); + for (srdf::Model::Group& group : groups) { loadGroupsTreeRecursive(group, nullptr); } @@ -263,7 +238,7 @@ void PlanningGroupsWidget::loadGroupsTree() groups_tree_->setDisabled(false); // make sure we disable it so that the cellChanged event is not called // Show Edit button if there are things to edit - if (!config_data_->srdf_->groups_.empty()) + if (!groups.empty()) { btn_edit_->show(); btn_delete_->show(); @@ -313,9 +288,6 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, joints->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, JOINT))); group->addChild(joints); - // Retrieve pointer to the shared kinematic model - const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); - // Loop through all aval. joints for (std::vector::const_iterator joint_it = group_it.joints_.begin(); joint_it != group_it.joints_.end(); ++joint_it) @@ -325,10 +297,10 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, std::string joint_name; // Get the type of joint this is - const moveit::core::JointModel* jm = model->getJointModel(*joint_it); - if (jm) // check if joint model was found + std::string joint_type = setup_step_.getJointType(*joint_it); + if (!joint_type.empty()) { - joint_name = *joint_it + " - " + jm->getTypeName(); + joint_name = *joint_it + " - " + joint_type; } else { @@ -397,20 +369,13 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, subgroup_it != group_it.subgroups_.end(); ++subgroup_it) { // Find group with this subgroups' name + srdf::Model::Group* searched_group; - srdf::Model::Group* searched_group = nullptr; // used for holding our search results - - for (srdf::Model::Group& group : config_data_->srdf_->groups_) + try { - if (group.name_ == *subgroup_it) // this is the group we are looking for - { - searched_group = &group; // convert to pointer from iterator - break; // we are done searching - } + searched_group = setup_step_.find(*subgroup_it); } - - // Check if subgroup was found - if (searched_group == nullptr) // not found + catch (const std::runtime_error& e) { QMessageBox::critical(this, "Error Loading SRDF", QString("Subgroup '") @@ -421,8 +386,6 @@ void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, return; // TODO: something better for error handling? } - // subgroup found! - // Recurse this function for each new group loadGroupsTreeRecursive(*searched_group, subgroups); } @@ -443,10 +406,10 @@ void PlanningGroupsWidget::previewSelected() PlanGroupType plan_group = item->data(0, Qt::UserRole).value(); // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); // Highlight the group - Q_EMIT(highlightGroup(plan_group.group_->name_)); + rviz_panel_->highlightGroup(plan_group.group_->name_); } // ****************************************************************************************** @@ -495,11 +458,8 @@ void PlanningGroupsWidget::editSelected() // ****************************************************************************************** void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group) { - // Retrieve pointer to the shared kinematic model - const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); - // Get the names of the all joints - const std::vector& joints = model->getJointModelNames(); + const std::vector& joints = setup_step_.getJointNames(); if (joints.empty()) { @@ -526,11 +486,8 @@ void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group) // ****************************************************************************************** void PlanningGroupsWidget::loadLinksScreen(srdf::Model::Group* this_group) { - // Retrieve pointer to the shared kinematic model - const moveit::core::RobotModelConstPtr& model = config_data_->getRobotModel(); - // Get the names of the all links - const std::vector& links = model->getLinkModelNames(); + const std::vector& links = setup_step_.getLinkNames(); if (links.empty()) { @@ -557,8 +514,7 @@ void PlanningGroupsWidget::loadLinksScreen(srdf::Model::Group* this_group) // ****************************************************************************************** void PlanningGroupsWidget::loadChainScreen(srdf::Model::Group* this_group) { - // Tell the kin chain widget to load up the chain from a kinematic model - chain_widget_->setAvailable(); + chain_widget_->setAvailable(setup_step_.getLinkNameTree()); // Make sure there isn't more than 1 chain pair if (this_group->chains_.size() > 1) @@ -593,12 +549,12 @@ void PlanningGroupsWidget::loadSubgroupsScreen(srdf::Model::Group* this_group) std::vector subgroups; // Display all groups by looping through them - for (srdf::Model::Group& group : config_data_->srdf_->groups_) + for (const std::string& group_name : setup_step_.getGroupNames()) { - if (group.name_ != this_group->name_) // do not include current group + if (group_name != this_group->name_) // do not include current group { // add to available subgroups list - subgroups.push_back(group.name_); + subgroups.push_back(group_name); } } @@ -643,7 +599,7 @@ void PlanningGroupsWidget::loadGroupScreen(srdf::Model::Group* this_group) } // Set the data in the edit box - group_edit_widget_->setSelected(current_edit_group_); + group_edit_widget_->setSelected(current_edit_group_, setup_step_.getMetaData(current_edit_group_)); } // ****************************************************************************************** @@ -651,8 +607,8 @@ void PlanningGroupsWidget::loadGroupScreen(srdf::Model::Group* this_group) // ****************************************************************************************** void PlanningGroupsWidget::deleteGroup() { - std::string group = current_edit_group_; - if (group.empty()) + std::string group_to_delete = current_edit_group_; + if (group_to_delete.empty()) { QTreeWidgetItem* item = groups_tree_->currentItem(); // Check that something was actually selected @@ -661,20 +617,17 @@ void PlanningGroupsWidget::deleteGroup() // Get the user custom properties of the currently selected row PlanGroupType plan_group = item->data(0, Qt::UserRole).value(); if (plan_group.group_) - group = plan_group.group_->name_; + group_to_delete = plan_group.group_->name_; } else current_edit_group_.clear(); - if (group.empty()) + if (group_to_delete.empty()) return; - // Find the group we are editing based on the goup name string - srdf::Model::Group* searched_group = config_data_->findGroupByName(group); - // Confirm user wants to delete group if (QMessageBox::question(this, "Confirm Group Deletion", QString("Are you sure you want to delete the planning group '") - .append(searched_group->name_.c_str()) + .append(group_to_delete.c_str()) .append("'? This will also delete all references in subgroups, robot poses and end " "effectors."), QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) @@ -682,124 +635,36 @@ void PlanningGroupsWidget::deleteGroup() return; } - // delete all robot poses that use that group's name - bool have_confirmed_group_state_deletion = false; - bool have_deleted_group_state = true; - while (have_deleted_group_state) + // Ensure we want to delete the states + std::vector pose_names = setup_step_.getPosesByGroup(group_to_delete); + if (!pose_names.empty() && + QMessageBox::question( + this, "Confirm Group State Deletion", + QString("The group that is about to be deleted has robot poses (robot states) that depend on this " + "group. Are you sure you want to delete this group as well as all dependent robot poses?"), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) { - have_deleted_group_state = false; - for (std::vector::iterator pose_it = config_data_->srdf_->group_states_.begin(); - pose_it != config_data_->srdf_->group_states_.end(); ++pose_it) - { - // check if this group state depends on the currently being deleted group - if (pose_it->group_ == searched_group->name_) - { - if (!have_confirmed_group_state_deletion) - { - have_confirmed_group_state_deletion = true; - - // confirm the user wants to delete group states - if (QMessageBox::question( - this, "Confirm Group State Deletion", - QString("The group that is about to be deleted has robot poses (robot states) that depend on this " - "group. Are you sure you want to delete this group as well as all dependent robot poses?"), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) - { - return; - } - } - config_data_->changes |= MoveItConfigData::POSES; - - // the user has confirmed, now delete this group state - config_data_->srdf_->group_states_.erase(pose_it); - have_deleted_group_state = true; - break; // you can only delete 1 item in vector before invalidating iterator - } - } + return; } - - // delete all end effectors that use that group's name - bool have_confirmed_end_effector_deletion = false; - bool have_deleted_end_effector = true; - while (have_deleted_end_effector) + // Ensure we want to delete the end_effectors + std::vector eef_names = setup_step_.getEndEffectorsByGroup(group_to_delete); + if (!eef_names.empty() && + QMessageBox::question( + this, "Confirm End Effector Deletion", + QString("The group that is about to be deleted has end effectors (grippers) that depend on this " + "group. Are you sure you want to delete this group as well as all dependent end effectors?"), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) { - have_deleted_end_effector = false; - for (std::vector::iterator effector_it = config_data_->srdf_->end_effectors_.begin(); - effector_it != config_data_->srdf_->end_effectors_.end(); ++effector_it) - { - // check if this group state depends on the currently being deleted group - if (effector_it->component_group_ == searched_group->name_) - { - if (!have_confirmed_end_effector_deletion) - { - have_confirmed_end_effector_deletion = true; - - // confirm the user wants to delete group states - if (QMessageBox::question( - this, "Confirm End Effector Deletion", - QString("The group that is about to be deleted has end effectors (grippers) that depend on this " - "group. Are you sure you want to delete this group as well as all dependent end effectors?"), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) - { - return; - } - } - config_data_->changes |= MoveItConfigData::END_EFFECTORS; - - // the user has confirmed, now delete this group state - config_data_->srdf_->end_effectors_.erase(effector_it); - have_deleted_end_effector = true; - break; // you can only delete 1 item in vector before invalidating iterator - } - } + return; } - config_data_->changes |= MoveItConfigData::GROUPS; - - // delete actual group - for (std::vector::iterator group_it = config_data_->srdf_->groups_.begin(); - group_it != config_data_->srdf_->groups_.end(); ++group_it) - { - // check if this is the group we want to delete - if (group_it->name_ == group) // string match - { - config_data_->srdf_->groups_.erase(group_it); - break; - } - } - - // loop again to delete all subgroup references - for (srdf::Model::Group& group_it : config_data_->srdf_->groups_) - { - // delete all subgroup references - bool deleted_subgroup = true; - while (deleted_subgroup) - { - deleted_subgroup = false; - - // check if the subgroups reference our deleted group - for (std::vector::iterator subgroup_it = group_it.subgroups_.begin(); - subgroup_it != group_it.subgroups_.end(); ++subgroup_it) - { - // Check if that subgroup references the deletion group. if so, delete it - if (subgroup_it->compare(group) == 0) // same name - { - group_it.subgroups_.erase(subgroup_it); // delete - deleted_subgroup = true; - break; - } - } - } - } + setup_step_.deleteGroup(group_to_delete); // Switch to main screen showMainScreen(); // Reload main screen table loadGroupsTree(); - - // Update the kinematic model with changes - config_data_->updateRobotModel(); } // ****************************************************************************************** @@ -821,27 +686,13 @@ void PlanningGroupsWidget::addGroup() // ****************************************************************************************** void PlanningGroupsWidget::saveJointsScreen() { - // Find the group we are editing based on the goup name string - srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_); - - // clear the old data - searched_group->joints_.clear(); - - // copy the data - for (int i = 0; i < joints_widget_->selected_data_table_->rowCount(); ++i) - { - searched_group->joints_.push_back(joints_widget_->selected_data_table_->item(i, 0)->text().toStdString()); - } + setup_step_.setJoints(current_edit_group_, joints_widget_->getSelectedValues()); // Switch to main screen showMainScreen(); // Reload main screen table loadGroupsTree(); - - // Update the kinematic model with changes - config_data_->updateRobotModel(); - config_data_->changes |= MoveItConfigData::GROUP_CONTENTS; } // ****************************************************************************************** @@ -849,28 +700,13 @@ void PlanningGroupsWidget::saveJointsScreen() // ****************************************************************************************** void PlanningGroupsWidget::saveLinksScreen() { - // Find the group we are editing based on the goup name string - srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_); - - // Find the group we are editing based on the goup name string - // clear the old data - searched_group->links_.clear(); - - // copy the data - for (int i = 0; i < links_widget_->selected_data_table_->rowCount(); ++i) - { - searched_group->links_.push_back(links_widget_->selected_data_table_->item(i, 0)->text().toStdString()); - } + setup_step_.setJoints(current_edit_group_, links_widget_->getSelectedValues()); // Switch to main screen showMainScreen(); // Reload main screen table loadGroupsTree(); - - // Update the kinematic model with changes - config_data_->updateRobotModel(); - config_data_->changes |= MoveItConfigData::GROUP_CONTENTS; } // ****************************************************************************************** @@ -878,64 +714,18 @@ void PlanningGroupsWidget::saveLinksScreen() // ****************************************************************************************** void PlanningGroupsWidget::saveChainScreen() { - // Find the group we are editing based on the goup name string - srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_); - // Get a reference to the supplied strings const std::string& tip = chain_widget_->tip_link_field_->text().trimmed().toStdString(); const std::string& base = chain_widget_->base_link_field_->text().trimmed().toStdString(); - // Check that box the tip and base, or neither, have text - if ((!tip.empty() && base.empty()) || (tip.empty() && !base.empty())) + try { - QMessageBox::warning(this, "Error Saving", - "You must specify a link for both the base and tip, or leave both " - "blank."); - return; + setup_step_.setChain(current_edit_group_, base, tip); } - - // Check that both given links are valid links, unless they are both blank - if (!tip.empty() && !base.empty()) + catch (const std::runtime_error& e) { - // Check that they are not the same link - if (tip.compare(base) == 0) // they are same - { - QMessageBox::warning(this, "Error Saving", "Tip and base link cannot be the same link."); - return; - } - - bool found_tip = false; - bool found_base = false; - const std::vector& links = config_data_->getRobotModel()->getLinkModelNames(); - - for (const std::string& link : links) - { - // Check if string matches either of user specified links - if (link.compare(tip) == 0) // they are same - found_tip = true; - else if (link.compare(base) == 0) // they are same - found_base = true; - - // Check if we are done searching - if (found_tip && found_base) - break; - } - - // Check if we found both links - if (!found_tip || !found_base) - { - QMessageBox::warning(this, "Error Saving", "Tip or base link(s) were not found in kinematic chain."); - return; - } - } - - // clear the old data - searched_group->chains_.clear(); - - // Save the data if there is data to save - if (!tip.empty() && !base.empty()) - { - searched_group->chains_.push_back(std::pair(base, tip)); + QMessageBox::warning(this, "Error Saving", e.what()); + return; } // Switch to main screen @@ -943,10 +733,6 @@ void PlanningGroupsWidget::saveChainScreen() // Reload main screen table loadGroupsTree(); - - // Update the kinematic model with changes - config_data_->updateRobotModel(); - config_data_->changes |= MoveItConfigData::GROUP_CONTENTS; } // ****************************************************************************************** @@ -954,94 +740,21 @@ void PlanningGroupsWidget::saveChainScreen() // ****************************************************************************************** void PlanningGroupsWidget::saveSubgroupsScreen() { - // Find the group we are editing based on the goup name string - srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_); - - // Check for cycles ------------------------------- - - // Create vector index of all nodes - std::map group_nodes; - - // Create vector of all nodes for use as id's - int node_id = 0; - for (srdf::Model::Group& group : config_data_->srdf_->groups_) + try { - // Add string to vector - group_nodes.insert(std::pair(group.name_, node_id)); - ++node_id; + setup_step_.setSubgroups(current_edit_group_, subgroups_widget_->getSelectedValues()); } - - // Create the empty graph - typedef boost::adjacency_list Graph; - Graph g(group_nodes.size()); - - // Traverse the group list again, this time inserting subgroups into graph - int from_id = 0; // track the outer node we are on to reduce searches performed - for (srdf::Model::Group& group : config_data_->srdf_->groups_) + catch (const std::runtime_error& e) { - // Check if group_it is same as current group - if (group.name_ == searched_group->name_) // yes, same group - { - // add new subgroup list from widget, not old one. this way we can check for new cycles - for (int i = 0; i < subgroups_widget_->selected_data_table_->rowCount(); ++i) - { - // Get std::string of subgroup - const std::string to_string = subgroups_widget_->selected_data_table_->item(i, 0)->text().toStdString(); - - // convert subgroup string to associated id - int to_id = group_nodes[to_string]; - - // Add edge from from_id to to_id - add_edge(from_id, to_id, g); - } - } - else // this group is not the group we are editing, so just add subgroups from memory - { - // add new subgroup list from widget, not old one. this way we can check for new cycles - for (const std::string& to_string : group.subgroups_) - { - // Get std::string of subgroup - // convert subgroup string to associated id - int to_id = group_nodes[to_string]; - - // Add edge from from_id to to_id - add_edge(from_id, to_id, g); - } - } - - ++from_id; - } - - // Check for cycles - bool has_cycle = false; - CycleDetector vis(has_cycle); - boost::depth_first_search(g, visitor(vis)); - - if (has_cycle) - { - // report to user the error - QMessageBox::warning(this, "Error Saving", "Depth first search reveals a cycle in the subgroups"); + QMessageBox::warning(this, "Error Saving", e.what()); return; } - // clear the old data - searched_group->subgroups_.clear(); - - // copy the data - for (int i = 0; i < subgroups_widget_->selected_data_table_->rowCount(); ++i) - { - searched_group->subgroups_.push_back(subgroups_widget_->selected_data_table_->item(i, 0)->text().toStdString()); - } - // Switch to main screen showMainScreen(); // Reload main screen table loadGroupsTree(); - - // Update the kinematic model with changes - config_data_->updateRobotModel(); - config_data_->changes |= MoveItConfigData::GROUP_CONTENTS; } // ****************************************************************************************** @@ -1049,17 +762,16 @@ void PlanningGroupsWidget::saveSubgroupsScreen() // ****************************************************************************************** bool PlanningGroupsWidget::saveGroupScreen() { - // Get a reference to the supplied strings const std::string& group_name = group_edit_widget_->group_name_field_->text().trimmed().toStdString(); - const std::string& kinematics_solver = group_edit_widget_->kinematics_solver_field_->currentText().toStdString(); - const std::string& default_planner = group_edit_widget_->default_planner_field_->currentText().toStdString(); - const std::string& kinematics_resolution = group_edit_widget_->kinematics_resolution_field_->text().toStdString(); - const std::string& kinematics_timeout = group_edit_widget_->kinematics_timeout_field_->text().toStdString(); - const std::string& kinematics_parameters_file = - group_edit_widget_->kinematics_parameters_file_field_->text().toStdString(); - // Used for editing existing groups - srdf::Model::Group* searched_group = nullptr; + GroupMetaData meta_data; + meta_data.kinematics_solver_ = group_edit_widget_->kinematics_solver_field_->currentText().toStdString(); + meta_data.kinematics_parameters_file_ = group_edit_widget_->kinematics_parameters_file_field_->text().toStdString(); + meta_data.default_planner_ = group_edit_widget_->default_planner_field_->currentText().toStdString(); + if (meta_data.default_planner_ == "None") + { + meta_data.default_planner_ = ""; + } // Check that a valid group name has been given if (group_name.empty()) @@ -1068,32 +780,11 @@ bool PlanningGroupsWidget::saveGroupScreen() return false; } - // Check if this is an existing group - if (!current_edit_group_.empty()) - { - // Find the group we are editing based on the goup name string - searched_group = config_data_->findGroupByName(current_edit_group_); - } - - // Check that the group name is unique - for (const auto& group : config_data_->srdf_->groups_) - { - if (group.name_.compare(group_name) == 0) // the names are the same - { - // is this our existing group? check if group pointers are same - if (&group != searched_group) - { - QMessageBox::warning(this, "Error Saving", "A group already exists with that name!"); - return false; - } - } - } - // Check that the resolution is an double number - double kinematics_resolution_double; + const std::string& kinematics_resolution = group_edit_widget_->kinematics_resolution_field_->text().toStdString(); try { - kinematics_resolution_double = boost::lexical_cast(kinematics_resolution); + meta_data.kinematics_solver_search_resolution_ = boost::lexical_cast(kinematics_resolution); } catch (boost::bad_lexical_cast&) { @@ -1102,10 +793,10 @@ bool PlanningGroupsWidget::saveGroupScreen() } // Check that the timeout is a double number - double kinematics_timeout_double; + const std::string& kinematics_timeout = group_edit_widget_->kinematics_timeout_field_->text().toStdString(); try { - kinematics_timeout_double = boost::lexical_cast(kinematics_timeout); + meta_data.kinematics_solver_timeout_ = boost::lexical_cast(kinematics_timeout); } catch (boost::bad_lexical_cast&) { @@ -1114,102 +805,29 @@ bool PlanningGroupsWidget::saveGroupScreen() } // Check that all numbers are >0 - if (kinematics_resolution_double <= 0) + if (meta_data.kinematics_solver_search_resolution_ <= 0) { QMessageBox::warning(this, "Error Saving", "Kinematics solver search resolution must be greater than 0."); return false; } - if (kinematics_timeout_double <= 0) + if (meta_data.kinematics_solver_timeout_ <= 0) { QMessageBox::warning(this, "Error Saving", "Kinematics solver search timeout must be greater than 0."); return false; } - adding_new_group_ = false; - - // Save the new group name or create the new group - if (searched_group == nullptr) // create new + try { - srdf::Model::Group new_group; - new_group.name_ = group_name; - config_data_->srdf_->groups_.push_back(new_group); - adding_new_group_ = true; // remember this group is new - config_data_->changes |= MoveItConfigData::GROUPS; + adding_new_group_ = current_edit_group_.empty(); + setup_step_.get(group_name, current_edit_group_); } - else + catch (const std::runtime_error& e) { - // Remember old group name - const std::string old_group_name = searched_group->name_; - - // Change group name - searched_group->name_ = group_name; - - // Change all references to this group name in other subgroups - // Loop through every group - for (std::vector::iterator group_it = config_data_->srdf_->groups_.begin(); - group_it != config_data_->srdf_->groups_.end(); ++group_it) - { - // Loop through every subgroup - for (std::string& subgroup : group_it->subgroups_) - { - // Check if that subgroup references old group name. if so, update it - if (subgroup.compare(old_group_name) == 0) // same name - { - subgroup.assign(group_name); // updated - config_data_->changes |= MoveItConfigData::GROUP_CONTENTS; - } - } - } - - // Change all references to this group name in the end effectors screen - for (std::vector::iterator eef_it = config_data_->srdf_->end_effectors_.begin(); - eef_it != config_data_->srdf_->end_effectors_.end(); ++eef_it) - { - // Check if this eef's parent group references old group name. if so, update it - if (eef_it->parent_group_.compare(old_group_name) == 0) // same name - { - ROS_DEBUG_STREAM_NAMED("setup_assistant", - "Changed eef '" << eef_it->name_ << "' to new parent group name " << group_name); - eef_it->parent_group_ = group_name; // updated - config_data_->changes |= MoveItConfigData::END_EFFECTORS; - } - - // Check if this eef's group references old group name. if so, update it - if (eef_it->component_group_.compare(old_group_name) == 0) // same name - { - ROS_DEBUG_STREAM_NAMED("setup_assistant", - "Changed eef '" << eef_it->name_ << "' to new group name " << group_name); - eef_it->component_group_ = group_name; // updated - config_data_->changes |= MoveItConfigData::END_EFFECTORS; - } - } - - // Change all references to this group name in the robot poses screen - for (std::vector::iterator state_it = config_data_->srdf_->group_states_.begin(); - state_it != config_data_->srdf_->group_states_.end(); ++state_it) - { - // Check if this eef's parent group references old group name. if so, update it - if (state_it->group_.compare(old_group_name) == 0) // same name - { - ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed group state group '" << state_it->group_ - << "' to new parent group name " - << group_name); - state_it->group_ = group_name; // updated - config_data_->changes |= MoveItConfigData::POSES; - } - } - - // Now update the robot model based on our changed to the SRDF - config_data_->updateRobotModel(); + QMessageBox::warning(this, "Error Saving", e.what()); + return false; } - // Save the group meta data - config_data_->group_meta_data_[group_name].kinematics_solver_ = kinematics_solver; - config_data_->group_meta_data_[group_name].kinematics_solver_search_resolution_ = kinematics_resolution_double; - config_data_->group_meta_data_[group_name].kinematics_solver_timeout_ = kinematics_timeout_double; - config_data_->group_meta_data_[group_name].kinematics_parameters_file_ = kinematics_parameters_file; - config_data_->group_meta_data_[group_name].default_planner_ = (default_planner == "None" ? "" : default_planner); - config_data_->changes |= MoveItConfigData::GROUP_KINEMATICS; + setup_step_.setMetaData(group_name, meta_data); // Reload main screen table loadGroupsTree(); @@ -1242,8 +860,8 @@ void PlanningGroupsWidget::saveGroupScreenJoints() if (!saveGroupScreen()) return; - // Find the group we are editing based on the goup name string - loadJointsScreen(config_data_->findGroupByName(current_edit_group_)); + // Find the group we are editing based on the group name string + loadJointsScreen(setup_step_.find(current_edit_group_)); return_screen_ = GROUP; // Switch to screen @@ -1259,8 +877,8 @@ void PlanningGroupsWidget::saveGroupScreenLinks() if (!saveGroupScreen()) return; - // Find the group we are editing based on the goup name string - loadLinksScreen(config_data_->findGroupByName(current_edit_group_)); + // Find the group we are editing based on the group name string + loadLinksScreen(setup_step_.find(current_edit_group_)); return_screen_ = GROUP; // Switch to screen @@ -1276,8 +894,8 @@ void PlanningGroupsWidget::saveGroupScreenChain() if (!saveGroupScreen()) return; - // Find the group we are editing based on the goup name string - loadChainScreen(config_data_->findGroupByName(current_edit_group_)); + // Find the group we are editing based on the group name string + loadChainScreen(setup_step_.find(current_edit_group_)); return_screen_ = GROUP; // Switch to screen @@ -1293,8 +911,8 @@ void PlanningGroupsWidget::saveGroupScreenSubgroups() if (!saveGroupScreen()) return; - // Find the group we are editing based on the goup name string - loadSubgroupsScreen(config_data_->findGroupByName(current_edit_group_)); + // Find the group we are editing based on the group name string + loadSubgroupsScreen(setup_step_.find(current_edit_group_)); return_screen_ = GROUP; // Switch to screen @@ -1314,18 +932,11 @@ void PlanningGroupsWidget::cancelEditing() } if (!current_edit_group_.empty() && adding_new_group_) { - srdf::Model::Group* editing = config_data_->findGroupByName(current_edit_group_); + srdf::Model::Group* editing = setup_step_.find(current_edit_group_); if (editing && editing->joints_.empty() && editing->links_.empty() && editing->chains_.empty() && editing->subgroups_.empty()) { - config_data_->group_meta_data_.erase(editing->name_); - for (std::vector::iterator group_it = config_data_->srdf_->groups_.begin(); - group_it != config_data_->srdf_->groups_.end(); ++group_it) - if (&(*group_it) == editing) - { - config_data_->srdf_->groups_.erase(group_it); - break; - } + setup_step_.deleteGroup(editing->name_); current_edit_group_.clear(); // Load the data to the tree loadGroupsTree(); @@ -1367,7 +978,7 @@ void PlanningGroupsWidget::showMainScreen() stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode anymore - Q_EMIT isModal(false); + Q_EMIT setModalMode(false); } // ****************************************************************************************** @@ -1378,7 +989,7 @@ void PlanningGroupsWidget::changeScreen(int index) stacked_widget_->setCurrentIndex(index); // Announce this widget's mode - Q_EMIT isModal(index != 0); + Q_EMIT setModalMode(index != 0); } // ****************************************************************************************** @@ -1387,7 +998,7 @@ void PlanningGroupsWidget::changeScreen(int index) void PlanningGroupsWidget::previewSelectedLink(const std::vector& links) { // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); for (const std::string& link : links) { @@ -1397,7 +1008,7 @@ void PlanningGroupsWidget::previewSelectedLink(const std::vector& l } // Highlight link - Q_EMIT highlightLink(link, QColor(255, 0, 0)); + rviz_panel_->highlightLink(link, QColor(255, 0, 0)); } } @@ -1407,28 +1018,18 @@ void PlanningGroupsWidget::previewSelectedLink(const std::vector& l void PlanningGroupsWidget::previewSelectedJoints(const std::vector& joints) { // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); for (const std::string& joint : joints) { - const moveit::core::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joint); - - // Check that a joint model was found - if (!joint_model) - { - continue; - } - - // Get the name of the link - const std::string link = joint_model->getChildLinkModel()->getName(); - + const std::string link = setup_step_.getChildOfJoint(joint); if (link.empty()) { continue; } // Highlight link - Q_EMIT highlightLink(link, QColor(255, 0, 0)); + rviz_panel_->highlightLink(link, QColor(255, 0, 0)); } } @@ -1438,16 +1039,16 @@ void PlanningGroupsWidget::previewSelectedJoints(const std::vector& void PlanningGroupsWidget::previewSelectedSubgroup(const std::vector& groups) { // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); for (const std::string& group : groups) { // Highlight group - Q_EMIT highlightGroup(group); + rviz_panel_->highlightGroup(group); } } -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins // ****************************************************************************************** // ****************************************************************************************** @@ -1455,7 +1056,10 @@ void PlanningGroupsWidget::previewSelectedSubgroup(const std::vector // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup_srdf_plugins::PlanningGroupsWidget, moveit_setup_framework::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp new file mode 100644 index 0000000000..b10b484fc0 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: David V. Lu!! */ + +#include +#include + +namespace moveit_setup_srdf_plugins +{ +void RobotPoses::onInit() +{ + SRDFStep::onInit(); + + // Create scene publisher for later use + pub_robot_state_ = parent_node_->create_publisher("moveit_robot_state", 1); + + // Set the planning scene + // srdf_config_->getPlanningScene()->setName("MoveIt Planning Scene"); + + // Collision Detection initializtion ------------------------------- + + // Setup the request + request.contacts = true; + request.max_contacts = 1; + request.max_contacts_per_pair = 1; + request.verbose = false; +} + +// ****************************************************************************************** +// Find the associated data by name +// ****************************************************************************************** +srdf::Model::GroupState* RobotPoses::findPoseByName(const std::string& name, const std::string& group) +{ + // Find the group state we are editing based on the pose name + srdf::Model::GroupState* searched_state = nullptr; // used for holding our search results + + for (srdf::Model::GroupState& state : srdf_config_->getGroupStates()) + { + if (state.name_ == name && state.group_ == group) // match + { + searched_state = &state; + break; + } + } + + return searched_state; +} + +// ****************************************************************************************** +// Load the allowed collision matrix from the SRDF's list of link pairs +// ****************************************************************************************** +void RobotPoses::loadAllowedCollisionMatrix() +{ + // Clear the allowed collision matrix + allowed_collision_matrix_.clear(); + + // Update the allowed collision matrix, in case there has been a change + for (const auto& disabled_collision : srdf_config_->getDisabledCollisions()) + { + allowed_collision_matrix_.setEntry(disabled_collision.link1_, disabled_collision.link2_, true); + } +} + +// ****************************************************************************************** +// Publish the current RobotState to Rviz +// ****************************************************************************************** +bool RobotPoses::publishState(const moveit::core::RobotState& robot_state) +{ + // Create a planning scene message + moveit_msgs::msg::DisplayRobotState msg; + moveit::core::robotStateToRobotStateMsg(robot_state, msg.state); + + // Publish! + pub_robot_state_->publish(msg); + + // Decide if current state is in collision + collision_detection::CollisionResult result; + srdf_config_->getPlanningScene()->checkSelfCollision(request, result, robot_state, allowed_collision_matrix_); + return !result.contacts.empty(); +} + +std::vector RobotPoses::getSimpleJointModels(const std::string& group_name) const +{ + moveit::core::RobotModelPtr robot_model = srdf_config_->getRobotModel(); + if (!robot_model->hasJointModelGroup(group_name)) + { + throw std::runtime_error(std::string("Unable to find joint model group for group: ") + group_name + + ". Are you sure this group has associated joints/links?"); + } + + const moveit::core::JointModelGroup* joint_model_group = robot_model->getJointModelGroup(group_name); + + std::vector joint_models; + for (const moveit::core::JointModel* joint_model : joint_model_group->getJointModels()) + { + if (joint_model->getVariableCount() != 1 || // only consider 1-variable joints + joint_model->isPassive() || // ignore passive + joint_model->getMimic()) // and mimic joints + continue; + + joint_models.push_back(joint_model); + } + return joint_models; +} + +void RobotPoses::setToCurrentValues(srdf::Model::GroupState& group_state) +{ + // Clear the old values (if any) + group_state.joint_values_.clear(); + + const auto& robot_state = srdf_config_->getPlanningScene()->getCurrentState(); + for (const moveit::core::JointModel* joint_model : getSimpleJointModels(group_state.group_)) + { + // Create vector for new joint values + std::vector joint_values(joint_model->getVariableCount()); + const double* const first_variable = robot_state.getVariablePositions() + joint_model->getFirstVariableIndex(); + std::copy(first_variable, first_variable + joint_values.size(), joint_values.begin()); + + // Add joint vector to SRDF + group_state.joint_values_[joint_model->getName()] = std::move(joint_values); + } + srdf_config_->updateRobotModel(moveit_setup_framework::POSES); +} + +} // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp index 9ee39e252e..224e8acbce 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp @@ -35,8 +35,8 @@ /* Author: Dave Coleman */ // SA -#include "robot_poses_widget.h" -#include "header_widget.h" +#include +#include #include // Qt #include @@ -52,17 +52,12 @@ #include #include -#include -#include -#include - -namespace moveit_setup_assistant +namespace moveit_setup_srdf_plugins { // ****************************************************************************************** // Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** -RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +void RobotPosesWidget::onInit() { // Set pointer to null so later we can tell if we need to delete it joint_list_layout_ = nullptr; @@ -72,12 +67,12 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Define Robot Poses", - "Create poses for the robot. Poses are defined as sets of joint values for " - "particular planning groups. This is useful for things like home position. " - "The first listed pose will be the robot's initial pose in simulation.", - this); + auto header = new moveit_setup_framework::HeaderWidget( + "Define Robot Poses", + "Create poses for the robot. Poses are defined as sets of joint values for " + "particular planning groups. This is useful for things like home position." + "The first listed pose will be the robot's initial pose in simulation.", + this); layout->addWidget(header); // Create contents screens --------------------------------------- @@ -92,23 +87,6 @@ RobotPosesWidget::RobotPosesWidget(QWidget* parent, const MoveItConfigDataPtr& c // Finish Layout -------------------------------------------------- this->setLayout(layout); - - // Create joint publisher ----------------------------------------- - ros::NodeHandle nh; - - // Create scene publisher for later use - pub_robot_state_ = nh.advertise(MOVEIT_ROBOT_STATE, 1); - - // Set the planning scene - config_data_->getPlanningScene()->setName("MoveIt Planning Scene"); - - // Collision Detection initializtion ------------------------------- - - // Setup the request - request.contacts = true; - request.max_contacts = 1; - request.max_contacts_per_pair = 1; - request.verbose = false; } // ****************************************************************************************** @@ -305,7 +283,7 @@ void RobotPosesWidget::showNewScreen() pose_name_field_->setText(""); // Announce that this widget is in modal mode - Q_EMIT isModal(true); + Q_EMIT setModalMode(true); } // ****************************************************************************************** @@ -329,33 +307,33 @@ void RobotPosesWidget::previewClicked(int row, int /*column*/, int /*previous_ro if (name && group) { // Find the selected in datastructure - srdf::Model::GroupState* pose = findPoseByName(name->text().toStdString(), group->text().toStdString()); + srdf::Model::GroupState* pose = setup_step_.findPoseByName(name->text().toStdString(), group->text().toStdString()); - showPose(pose); + showPose(*pose); } } // ****************************************************************************************** // Show the robot in the current pose // ****************************************************************************************** -void RobotPosesWidget::showPose(srdf::Model::GroupState* pose) +void RobotPosesWidget::showPose(const srdf::Model::GroupState& pose) { // Set the joints based on the SRDF pose - moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); - for (std::map >::const_iterator value_it = pose->joint_values_.begin(); - value_it != pose->joint_values_.end(); ++value_it) + moveit::core::RobotState& robot_state = setup_step_.getState(); + for (const auto& key_value_pair : pose.joint_values_) { - robot_state.setJointPositions(value_it->first, value_it->second); + robot_state.setJointPositions(key_value_pair.first, key_value_pair.second); } // Update the joints - publishJoints(); + bool collision = setup_step_.publishState(robot_state); + collision_warning_->setHidden(!collision); // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); // Highlight group - Q_EMIT highlightGroup(pose->group_); + rviz_panel_->highlightGroup(pose.group_); } // ****************************************************************************************** @@ -363,14 +341,15 @@ void RobotPosesWidget::showPose(srdf::Model::GroupState* pose) // ****************************************************************************************** void RobotPosesWidget::showDefaultPose() { - moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); + moveit::core::RobotState& robot_state = setup_step_.getState(); robot_state.setToDefaultValues(); // Update the joints - publishJoints(); + bool collision = setup_step_.publishState(robot_state); + collision_warning_->setHidden(!collision); // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); } // ****************************************************************************************** @@ -379,14 +358,13 @@ void RobotPosesWidget::showDefaultPose() void RobotPosesWidget::playPoses() { // Loop through each pose and play them - for (std::vector::iterator pose_it = config_data_->srdf_->group_states_.begin(); - pose_it != config_data_->srdf_->group_states_.end(); ++pose_it) + for (const srdf::Model::GroupState& pose : setup_step_.getGroupStates()) { - ROS_INFO_STREAM("Showing pose " << pose_it->name_); - showPose(&(*pose_it)); - ros::Duration(0.05).sleep(); + RCLCPP_INFO_STREAM(setup_step_.getLogger(), "Showing pose " << pose.name_); + showPose(pose); + // TODO: ros::Duration(0.05).sleep(); QApplication::processEvents(); - ros::Duration(0.45).sleep(); + // TODO: ros::Duration(0.45).sleep(); } } @@ -410,7 +388,7 @@ void RobotPosesWidget::edit(int row) const std::string& group = data_table_->item(row, 1)->text().toStdString(); // Find the selected in datastruture - srdf::Model::GroupState* pose = findPoseByName(name, group); + srdf::Model::GroupState* pose = setup_step_.findPoseByName(name, group); current_edit_pose_ = pose; // Set pose name @@ -425,13 +403,13 @@ void RobotPosesWidget::edit(int row) } group_name_field_->setCurrentIndex(index); - showPose(pose); + showPose(*pose); // Switch to screen - do this before setCurrentIndex stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode - Q_EMIT isModal(true); + Q_EMIT setModalMode(true); // Manually send the load joint sliders signal loadJointSliders(QString(pose->group_.c_str())); @@ -446,9 +424,9 @@ void RobotPosesWidget::loadGroupsComboBox() group_name_field_->clear(); // Add all group names to combo box - for (srdf::Model::Group& group : config_data_->srdf_->groups_) + for (const std::string& group_name : setup_step_.getGroupNames()) { - group_name_field_->addItem(group.name_.c_str()); + group_name_field_->addItem(group_name.c_str()); } } @@ -465,13 +443,15 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) // Get group name from input const std::string group_name = selected.toStdString(); - // Check that joint model exist - if (!config_data_->getRobotModel()->hasJointModelGroup(group_name)) + std::vector joint_models; + + try { - QMessageBox::critical(this, "Error Loading", - QString("Unable to find joint model group for group: ") - .append(group_name.c_str()) - .append(" Are you sure this group has associated joints/links?")); + joint_models = setup_step_.getSimpleJointModels(group_name); + } + catch (const std::runtime_error& e) + { + QMessageBox::critical(this, "Error Loading", QString(e.what())); return; } @@ -490,19 +470,11 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) joint_list_widget_->setMinimumSize(50, 50); // w, h // Get list of associated joints - const moveit::core::JointModelGroup* joint_model_group = - config_data_->getRobotModel()->getJointModelGroup(group_name); - const auto& robot_state = config_data_->getPlanningScene()->getCurrentState(); + const auto& robot_state = setup_step_.getState(); // Iterate through the joints - int num_joints = 0; - for (const moveit::core::JointModel* joint_model : joint_model_group->getJointModels()) + for (const moveit::core::JointModel* joint_model : joint_models) { - if (joint_model->getVariableCount() != 1 || // only consider 1-variable joints - joint_model->isPassive() || // ignore passive - joint_model->getMimic()) // and mimic joints - continue; - double init_value = robot_state.getVariablePosition(joint_model->getVariableNames()[0]); // For each joint in group add slider @@ -512,39 +484,18 @@ void RobotPosesWidget::loadJointSliders(const QString& selected) // Connect value change event connect(sw, SIGNAL(jointValueChanged(const std::string&, double)), this, SLOT(updateRobotModel(const std::string&, double))); - - ++num_joints; } // Copy the width of column 2 and manually calculate height from number of joints - joint_list_widget_->resize(300, num_joints * 70); // w, h + joint_list_widget_->resize(300, joint_models.size() * 70); // w, h // Update the robot model in Rviz with newly selected joint values - publishJoints(); + bool collision = setup_step_.publishState(robot_state); + collision_warning_->setHidden(!collision); // Highlight the planning group - Q_EMIT unhighlightAll(); - Q_EMIT highlightGroup(group_name); -} - -// ****************************************************************************************** -// Find the associated data by name -// ****************************************************************************************** -srdf::Model::GroupState* RobotPosesWidget::findPoseByName(const std::string& name, const std::string& group) -{ - // Find the group state we are editing based on the pose name - srdf::Model::GroupState* searched_state = nullptr; // used for holding our search results - - for (srdf::Model::GroupState& state : config_data_->srdf_->group_states_) - { - if (state.name_ == name && state.group_ == group) // match - { - searched_state = &state; - break; - } - } - - return searched_state; + rviz_panel_->unhighlightAll(); + rviz_panel_->highlightGroup(group_name); } // ****************************************************************************************** @@ -568,21 +519,10 @@ void RobotPosesWidget::deleteSelected() return; } - // Delete pose from vector - for (std::vector::iterator pose_it = config_data_->srdf_->group_states_.begin(); - pose_it != config_data_->srdf_->group_states_.end(); ++pose_it) - { - // check if this is the group we want to delete - if (pose_it->name_ == name && pose_it->group_ == group) // match - { - config_data_->srdf_->group_states_.erase(pose_it); - break; - } - } + setup_step_.removePoseByName(name, group); // Reload main screen table loadDataTable(); - config_data_->changes |= MoveItConfigData::POSES; } // ****************************************************************************************** @@ -615,7 +555,7 @@ void RobotPosesWidget::doneEditing() // If creating a new pose, check if the (name, group) pair already exists if (!current_edit_pose_) { - searched_data = findPoseByName(name, group); + searched_data = setup_step_.findPoseByName(name, group); if (searched_data != current_edit_pose_) { if (QMessageBox::warning(this, "Warning Saving", "A pose already exists with that name! Overwrite?", @@ -626,8 +566,6 @@ void RobotPosesWidget::doneEditing() else searched_data = current_edit_pose_; // overwrite edited pose - config_data_->changes |= MoveItConfigData::POSES; - // Save the new pose name or create the new pose ---------------------------- bool is_new = false; @@ -641,34 +579,12 @@ void RobotPosesWidget::doneEditing() searched_data->name_ = name; searched_data->group_ = group; - // Copy joint positions ---------------------------------------- - - // Clear the old values - searched_data->joint_values_.clear(); - - const moveit::core::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group); - const auto& robot_state = config_data_->getPlanningScene()->getCurrentState(); - - // Iterate through the current group's joints and add them to SRDF - for (const moveit::core::JointModel* jm : joint_model_group->getJointModels()) - { - // Check that this joint only represents 1 variable. - if (jm->getVariableCount() == 1 && !jm->isPassive() && !jm->getMimic()) - { - // Create vector for new joint values - std::vector joint_values(jm->getVariableCount()); - const double* const first_variable = robot_state.getVariablePositions() + jm->getFirstVariableIndex(); - std::copy(first_variable, first_variable + joint_values.size(), joint_values.begin()); - - // Add joint vector to SRDF - searched_data->joint_values_[jm->getName()] = std::move(joint_values); - } - } + setup_step_.setToCurrentValues(*searched_data); // Insert new poses into group state vector -------------------------- if (is_new) { - config_data_->srdf_->group_states_.push_back(*searched_data); + setup_step_.getGroupStates().push_back(*searched_data); delete searched_data; } @@ -681,7 +597,7 @@ void RobotPosesWidget::doneEditing() stacked_widget_->setCurrentIndex(0); // Announce that this widget is done with modal mode - Q_EMIT isModal(false); + Q_EMIT setModalMode(false); } // ****************************************************************************************** @@ -693,7 +609,7 @@ void RobotPosesWidget::cancelEditing() stacked_widget_->setCurrentIndex(0); // Announce that this widget is done with modal mode - Q_EMIT isModal(false); + Q_EMIT setModalMode(false); } // ****************************************************************************************** @@ -706,12 +622,14 @@ void RobotPosesWidget::loadDataTable() data_table_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called data_table_->clearContents(); + std::vector& group_states = setup_step_.getGroupStates(); + // Set size of datatable - data_table_->setRowCount(config_data_->srdf_->group_states_.size()); + data_table_->setRowCount(group_states.size()); // Loop through every pose int row = 0; - for (const auto& group_state : config_data_->srdf_->group_states_) + for (const auto& group_state : group_states) { // Create row elements QTableWidgetItem* data_name = new QTableWidgetItem(group_state.name_.c_str()); @@ -736,7 +654,7 @@ void RobotPosesWidget::loadDataTable() data_table_->resizeColumnToContents(1); // Show edit button if applicable - if (!config_data_->srdf_->group_states_.empty()) + if (!group_states.empty()) btn_edit_->show(); } @@ -745,6 +663,8 @@ void RobotPosesWidget::loadDataTable() // ****************************************************************************************** void RobotPosesWidget::focusGiven() { + setup_step_.loadAllowedCollisionMatrix(); + // Show the current poses screen stacked_widget_->setCurrentIndex(0); @@ -760,33 +680,12 @@ void RobotPosesWidget::focusGiven() // ****************************************************************************************** void RobotPosesWidget::updateRobotModel(const std::string& name, double value) { - moveit::core::RobotState& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); + moveit::core::RobotState& robot_state = setup_step_.getState(); robot_state.setVariablePosition(name, value); // Update the robot model/rviz - publishJoints(); -} - -// ****************************************************************************************** -// Publish the current RobotState to Rviz -// ****************************************************************************************** -void RobotPosesWidget::publishJoints() -{ - // Update link + collision transforms - auto& robot_state = config_data_->getPlanningScene()->getCurrentStateNonConst(); - robot_state.update(); - // Create a planning scene message - moveit_msgs::msg::DisplayRobotState msg; - moveit::core::robotStateToRobotStateMsg(robot_state, msg.state); - - // Publish! - pub_robot_state_.publish(msg); - - // Decide if current state is in collision - collision_detection::CollisionResult result; - config_data_->getPlanningScene()->checkSelfCollision(request, result, robot_state, - config_data_->allowed_collision_matrix_); - collision_warning_->setHidden(result.contacts.empty()); + bool collision = setup_step_.publishState(robot_state); + collision_warning_->setHidden(!collision); } // ****************************************************************************************** @@ -904,4 +803,7 @@ void SliderWidget::changeJointSlider() Q_EMIT jointValueChanged(joint_model_->getName(), value); } -} // namespace moveit_setup_assistant +} // namespace moveit_setup_srdf_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup_srdf_plugins::RobotPosesWidget, moveit_setup_framework::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints.cpp index 6883cd6a97..746792e623 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints.cpp @@ -40,72 +40,16 @@ namespace moveit_setup_srdf_plugins { void VirtualJoints::onInit() { - srdf_config_ = config_data_->get("srdf"); + SuperSRDFStep::onInit(); urdf_config_ = config_data_->get("urdf"); } -// ****************************************************************************************** -// Find the associated data by name -// ****************************************************************************************** -srdf::Model::VirtualJoint* VirtualJoints::findVJointByName(const std::string& name) +void VirtualJoints::setProperties(srdf::Model::VirtualJoint* vj, const std::string& parent_name, + const std::string& child_name, const std::string& joint_type) { - // Find the group state we are editing based on the vjoint name - srdf::Model::VirtualJoint* searched_group = nullptr; // used for holding our search results - - for (srdf::Model::VirtualJoint& virtual_joint : srdf_config_->getVirtualJoints()) - { - if (virtual_joint.name_ == name) // string match - { - searched_group = &virtual_joint; // convert to pointer from iterator - break; // we are done searching - } - } - - return searched_group; -} - -bool VirtualJoints::deleteByName(const std::string& name) -{ - auto vjs = srdf_config_->getVirtualJoints(); - - for (std::vector::iterator vjoint_it = vjs.begin(); vjoint_it != vjs.end(); ++vjoint_it) - { - // check if this is the group we want to delete - if (vjoint_it->name_ == name) // string match - { - vjs.erase(vjoint_it); - srdf_config_->updateRobotModel(true); - return true; - } - } - return false; -} - -void VirtualJoints::create(const std::string& old_name, const std::string& joint_name, const std::string& parent_name, - const std::string& child_name, const std::string& joint_type) -{ - srdf::Model::VirtualJoint* vj = nullptr; - std::vector& vjs = srdf_config_->getVirtualJoints(); - if (old_name.empty()) - { - // Create new - vjs.push_back(srdf::Model::VirtualJoint()); - vj = &vjs.back(); - } - else - { - // Find the group we are editing based on the old name string - vj = findVJointByName(old_name); - if (old_name != joint_name && findVJointByName(joint_name)) - { - throw std::runtime_error("A virtual joint already exists with that name!"); - } - } - // Copy name data ---------------------------------------------------- - vj->name_ = joint_name; vj->parent_frame_ = parent_name; vj->child_link_ = child_name; vj->type_ = joint_type; - srdf_config_->updateRobotModel(true); + srdf_config_->updateRobotModel(moveit_setup_framework::VIRTUAL_JOINTS); } } // namespace moveit_setup_srdf_plugins diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints_widget.cpp index 2dcaae477d..7967804e07 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints_widget.cpp @@ -274,7 +274,7 @@ void VirtualJointsWidget::edit(const std::string& name) current_edit_vjoint_ = name; // Find the selected in datastruture - srdf::Model::VirtualJoint* vjoint = setup_step_.findVJointByName(name); + srdf::Model::VirtualJoint* vjoint = setup_step_.find(name); // Check if vjoint was found if (vjoint == nullptr) // not found @@ -367,7 +367,7 @@ void VirtualJointsWidget::deleteSelected() } // Delete vjoint from vector - setup_step_.deleteByName(current_edit_vjoint_); + setup_step_.remove(current_edit_vjoint_); // Reload main screen table loadDataTable(); @@ -415,7 +415,8 @@ void VirtualJointsWidget::doneEditing() try { - setup_step_.create(current_edit_vjoint_, vjoint_name, parent_name, child_link, joint_type); + srdf::Model::VirtualJoint* vj = setup_step_.get(vjoint_name, current_edit_vjoint_); + setup_step_.setProperties(vj, parent_name, child_link, joint_type); } catch (const std::runtime_error& e) { @@ -459,7 +460,7 @@ void VirtualJointsWidget::loadDataTable() data_table_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called data_table_->clearContents(); - auto virtual_joints = setup_step_.getVirtualJoints(); + const auto& virtual_joints = setup_step_.getContainer(); // Set size of datatable data_table_->setRowCount(virtual_joints.size()); diff --git a/moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp b/moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp index f7daa67978..7c03d97027 100644 --- a/moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp +++ b/moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp @@ -43,26 +43,10 @@ namespace moveit_setup_assistant { -// Default kin solver values -static const double DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION = 0.005; -static const double DEFAULT_KIN_SOLVER_TIMEOUT = 0.005; - // ****************************************************************************************** // Structs // ****************************************************************************************** -/** - * Planning groups extra data not found in srdf but used in config files - */ -struct GroupMetaData -{ - std::string kinematics_solver_; // Name of kinematics plugin to use - double kinematics_solver_search_resolution_; // resolution to use with solver - double kinematics_solver_timeout_; // solver timeout - std::string kinematics_parameters_file_; // file for additional kinematics parameters - std::string default_planner_; // Name of the default planner to use -}; - /** * Controllers settings which may be set in the config files */ @@ -132,45 +116,12 @@ class MoveItConfigData MoveItConfigData(); ~MoveItConfigData(); - // bits of information that can be entered in Setup Assistant - enum InformationFields - { - COLLISIONS = 1 << 1, - VIRTUAL_JOINTS = 1 << 2, - GROUPS = 1 << 3, - GROUP_CONTENTS = 1 << 4, - GROUP_KINEMATICS = 1 << 5, - POSES = 1 << 6, - END_EFFECTORS = 1 << 7, - PASSIVE_JOINTS = 1 << 8, - SIMULATION = 1 << 9, - AUTHOR_INFO = 1 << 10, - SENSORS_CONFIG = 1 << 11, - SRDF = COLLISIONS | VIRTUAL_JOINTS | GROUPS | GROUP_CONTENTS | POSES | END_EFFECTORS | PASSIVE_JOINTS - }; - unsigned long changes; // bitfield of changes (composed of InformationFields) - - /// Planning groups extra data not found in srdf but used in config files - std::map group_meta_data_; - /// Setup Assistants package's path for when we use its templates std::string setup_assistant_path_; /// Location that moveit_setup_assistant stores its templates std::string template_package_path_; - // ****************************************************************************************** - // Public Functions - // ****************************************************************************************** - - /** - * Find the associated group by name - * - * @param name - name of data to find in datastructure - * @return pointer to data in datastructure - */ - srdf::Model::Group* findGroupByName(const std::string& name); - // ****************************************************************************************** // Public Functions for outputting configuration and setting files // ****************************************************************************************** diff --git a/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp index a0ad83da6a..0b06d65d98 100644 --- a/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp @@ -298,55 +298,6 @@ bool MoveItConfigData::outputSTOMPPlanningYAML(const std::string& file_path) return true; // file created successfully } -// ****************************************************************************************** -// Output kinematic config files -// ****************************************************************************************** -bool MoveItConfigData::outputKinematicsYAML(const std::string& file_path) -{ - YAML::Emitter emitter; - emitter << YAML::BeginMap; - - // Output every group and the kinematic solver it can use ---------------------------------- - for (srdf::Model::Group& group : srdf_->groups_) - { - // Only save kinematic data if the solver is not "None" - if (group_meta_data_[group.name_].kinematics_solver_.empty() || - group_meta_data_[group.name_].kinematics_solver_ == "None") - continue; - - emitter << YAML::Key << group.name_; - emitter << YAML::Value << YAML::BeginMap; - - // Kinematic Solver - emitter << YAML::Key << "kinematics_solver"; - emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_; - - // Search Resolution - emitter << YAML::Key << "kinematics_solver_search_resolution"; - emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_search_resolution_; - - // Solver Timeout - emitter << YAML::Key << "kinematics_solver_timeout"; - emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_timeout_; - - emitter << YAML::EndMap; - } - - emitter << YAML::EndMap; - - std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc); - if (!output_stream.good()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to open file for writing " << file_path); - return false; - } - - output_stream << emitter.c_str(); - output_stream.close(); - - return true; // file created successfully -} - // ****************************************************************************************** // Helper function to get the controller that is controlling the joint // ****************************************************************************************** @@ -1114,51 +1065,6 @@ bool MoveItConfigData::inputOMPLYAML(const std::string& file_path) return true; } -// ****************************************************************************************** -// Input kinematics.yaml file -// ****************************************************************************************** -bool MoveItConfigData::inputKinematicsYAML(const std::string& file_path) -{ - // Load file - std::ifstream input_stream(file_path.c_str()); - if (!input_stream.good()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to open file for reading " << file_path); - return false; - } - - // Begin parsing - try - { - YAML::Node doc = YAML::Load(input_stream); - - // Loop through all groups - for (YAML::const_iterator group_it = doc.begin(); group_it != doc.end(); ++group_it) - { - const std::string& group_name = group_it->first.as(); - const YAML::Node& group = group_it->second; - - // Create new meta data - GroupMetaData meta_data; - - parse(group, "kinematics_solver", meta_data.kinematics_solver_); - parse(group, "kinematics_solver_search_resolution", meta_data.kinematics_solver_search_resolution_, - DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION); - parse(group, "kinematics_solver_timeout", meta_data.kinematics_solver_timeout_, DEFAULT_KIN_SOLVER_TIMEOUT); - - // Assign meta data to vector - group_meta_data_[group_name] = meta_data; - } - } - catch (YAML::ParserException& e) // Catch errors - { - RCLCPP_ERROR_STREAM(LOGGER, e.what()); - return false; - } - - return true; // file created successfully -} - // ****************************************************************************************** // Input planning_context.launch file // ****************************************************************************************** @@ -1393,31 +1299,6 @@ bool MoveItConfigData::createFullURDFPath() return fs::is_regular_file(urdf_path_); } -srdf::Model::Group* MoveItConfigData::findGroupByName(const std::string& name) -{ - // Find the group we are editing based on the goup name string - srdf::Model::Group* searched_group = nullptr; // used for holding our search results - - for (srdf::Model::Group& group : srdf_->groups_) - { - if (group.name_ == name) // string match - { - searched_group = &group; // convert to pointer from iterator - break; // we are done searching - } - } - - // Check if subgroup was found - if (searched_group == nullptr) // not found - { - RCLCPP_ERROR_STREAM(LOGGER, "An internal error has occurred while searching for groups. Group '" - << name << "' was not found in the SRDF."); - throw std::runtime_error(name + " was not found in the SRDF"); - } - - return searched_group; -} - // ****************************************************************************************** // Find a controller by name // ******************************************************************************************