diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 091ed68231..71d36d65a3 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -154,11 +154,16 @@ def __init__( self.__urdf_file_path = None self.__srdf_file_path = None + modified_urdf_path = Path("config") / (self.__robot_name + ".urdf.xacro") + if (self._package_path / modified_urdf_path).exists(): + self.__urdf_package = self._package_path + self.__urdf_file_path = modified_urdf_path + if setup_assistant_file.exists(): setup_assistant_yaml = load_yaml(setup_assistant_file) config = setup_assistant_yaml.get("moveit_setup_assistant_config", {}) urdf_config = config.get("urdf", config.get("URDF")) - if urdf_config: + if urdf_config and self.__urdf_package is None: self.__urdf_package = Path( get_package_share_directory(urdf_config["package"]) ) @@ -286,9 +291,10 @@ def trajectory_execution( controller_pattern = re.compile("^(.*)_controllers.yaml$") possible_names = get_pattern_matches(config_folder, controller_pattern) if not possible_names: - raise RuntimeError( - "trajectory_execution: `Parameter file_path is undefined " - f"and no matches for {config_folder}/*_controllers.yaml" + # Warn the user instead of raising exception + logging.warning( + "\x1b[33;20mtrajectory_execution: `Parameter file_path is undefined " + f"and no matches for {config_folder}/*_controllers.yaml\x1b[0m" ) else: chosen_name = None @@ -313,7 +319,8 @@ def trajectory_execution( else: file_path = self._package_path / file_path - self.__moveit_configs.trajectory_execution.update(load_yaml(file_path)) + if file_path: + self.__moveit_configs.trajectory_execution.update(load_yaml(file_path)) return self def planning_scene_monitor( diff --git a/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/CMakeLists.txt deleted file mode 100644 index 58d2749c95..0000000000 --- a/moveit_setup_assistant/CMakeLists.txt +++ /dev/null @@ -1,144 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(moveit_setup_assistant) - -# Common cmake code applied to all moveit packages -find_package(moveit_common REQUIRED) -moveit_package() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(moveit_core REQUIRED) -find_package(moveit_ros_planning REQUIRED) -find_package(moveit_ros_visualization REQUIRED) -find_package(rclcpp REQUIRED) -find_package(urdf REQUIRED) -find_package(srdfdom REQUIRED) -find_package(ompl REQUIRED) - -set(THIS_PACKAGE_INCLUDE_DEPENDS - moveit_core - moveit_ros_planning - moveit_ros_visualization - rclcpp - urdf - srdfdom -) - -# Header files that need Qt Moc pre-processing for use with Qt signals, etc: -#set(HEADERS -# src/widgets/navigation_widget.h -# src/widgets/header_widget.h -# src/widgets/setup_assistant_widget.h -# src/widgets/start_screen_widget.h -# src/widgets/planning_groups_widget.h -# src/widgets/double_list_widget.h -# src/widgets/kinematic_chain_widget.h -# src/widgets/group_edit_widget.h -# src/widgets/default_collisions_widget.h -# src/widgets/robot_poses_widget.h -# src/widgets/end_effectors_widget.h -# src/widgets/virtual_joints_widget.h -# src/widgets/passive_joints_widget.h -# src/widgets/perception_widget.h -# src/widgets/ros_controllers_widget.h -# src/widgets/controller_edit_widget.h -# src/widgets/configuration_files_widget.h -# src/widgets/setup_screen_widget.h -# src/widgets/simulation_widget.h -# src/widgets/author_information_widget.h -#) - -# Main Widgets Library - all screens (navigation options) -#add_library(${PROJECT_NAME}_widgets -# src/widgets/start_screen_widget.cpp -# src/widgets/planning_groups_widget.cpp -# src/widgets/double_list_widget.cpp -# src/widgets/kinematic_chain_widget.cpp -# src/widgets/group_edit_widget.cpp -# src/widgets/default_collisions_widget.cpp -# src/widgets/robot_poses_widget.cpp -# src/widgets/end_effectors_widget.cpp -# src/widgets/virtual_joints_widget.cpp -# src/widgets/passive_joints_widget.cpp -# src/widgets/perception_widget.cpp -# src/widgets/configuration_files_widget.cpp -# src/widgets/ros_controllers_widget.cpp -# src/widgets/controller_edit_widget.cpp -# src/widgets/navigation_widget.cpp -# src/widgets/header_widget.cpp -# src/widgets/setup_assistant_widget.cpp -# src/widgets/setup_screen_widget.cpp -# src/widgets/simulation_widget.cpp -# src/widgets/author_information_widget.cpp -# ${HEADERS} -#) - -# Tools Library -add_library(${PROJECT_NAME}_tools - src/tools/compute_default_collisions.cpp - src/tools/moveit_config_data.cpp - src/tools/xml_manipulation.cpp - src/tools/collision_linear_model.cpp - src/tools/collision_matrix_model.cpp - src/tools/rotated_header_view.cpp - src/tools/xml_syntax_highlighter.cpp -) -target_include_directories(${PROJECT_NAME}_tools PUBLIC - $ - $ - ${OMPL_INCLUDE_DIRS} -) -ament_target_dependencies( - ${PROJECT_NAME}_tools - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -add_executable(${PROJECT_NAME}_updater src/collisions_updater.cpp) -target_include_directories(${PROJECT_NAME}_updater PUBLIC - $ - $) -target_link_libraries( - ${PROJECT_NAME}_updater - ${PROJECT_NAME}_tools -) - -ament_target_dependencies( - ${PROJECT_NAME}_updater - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -set_target_properties( - ${PROJECT_NAME}_updater - PROPERTIES OUTPUT_NAME collisions_updater - PREFIX "" -) - -install( - TARGETS - ${PROJECT_NAME}_tools - ${PROJECT_NAME}_updater - EXPORT ${PROJECT_NAME}Targets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} - INCLUDES DESTINATION include -) -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY resources DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY templates DESTINATION share/${PROJECT_NAME}) -ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Unit tests -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/moveit_setup_assistant/README.md b/moveit_setup_assistant/README.md index 1cf2cff303..19104bf7e9 100644 --- a/moveit_setup_assistant/README.md +++ b/moveit_setup_assistant/README.md @@ -1,5 +1,138 @@ -# MoveIt Collisions Updater +# MoveIt Setup Assistant 2.0 + +## Migration Progress +### Fully Ported + * `moveit_setup::core::StartScreenWidget` + * `moveit_setup::srdf_setup::DefaultCollisionsWidget` + * `moveit_setup::srdf_setup::VirtualJointsWidget` + * `moveit_setup::srdf_setup::PlanningGroupsWidget` + * `moveit_setup::srdf_setup::RobotPosesWidget` + * `moveit_setup::srdf_setup::EndEffectorsWidget` + * `moveit_setup::srdf_setup::PassiveJointsWidget` + * `moveit_setup::controllers::UrdfModificationsWidget` + * `moveit_setup::controllers::ROS2ControllersWidget` + * `moveit_setup::controllers::MoveItControllersWidget` + * `moveit_setup::app::PerceptionWidget` + * `moveit_setup::app::LaunchesWidget` + * `moveit_setup::core::AuthorInformationWidget` + * `moveit_setup::core::ConfigurationFilesWidget` + +### Not Ported Yet + * `moveit_setup::simulation::SimulationWidget` + +### Notes + * `PerceptionWidget` is ported, except `moveit_configs_utils` needs to be modified to load `sensors_3d.yaml`. This may require adjustments to the format of `sensors_3d.yaml` + * Possible bug in `PlanningGroupsWidget`: Creating a new group does not properly create a new JointModelGroup + * There are additional templates that have not been ported in the `unported_templates` folder. + +## Quick Start +To run the Setup Assistant + +`ros2 run moveit_setup_assistant moveit_setup_assistant` To run the collision updater: `ros2 run moveit_setup_assistant moveit_setup_assistant --urdf --xacro-args --srdf --trials 100000` + +## Core Design + +There are four main goals with this refactored version of MoveIt Setup Assistant (MSA). + * Works in ROS 2 + * Divorce GUI Logic from Functional Logic + * Add Extensibility + * Customizable Control Flow + +To that end the functionality has been split into multiple packages, using a `pluginlib`-based class structure. + +There are four primary classes. + * `SetupStep`s + * `SetupStepWidget`s + * `SetupConfig`s + * `GeneratedFile`s + +## `SetupStep` +Contains all of the non-GUI code necessary for doing one "screen" worth of setup. The interface contains two methods to implement: + * `getName` - A string for the name of the setup step + * `isReady` - Return true if the data necessary to proceed with this step has been configured + * `onInit` - Code to run when the method is first initialized. Useful for loading pointers to necessary `SetupConfig`s. + +## `SetupStepWidget` +Keeping the GUI code separate from the business logic, the widget here is implemented with QT. There's one widget for each `SetupStep`. The widgets are also configured so that they can be loaded via `pluginlib`. + +Each widget has access to the `RVizPanel`, a common visual panel for displaying the robot itself. + +The interface is comprised of + * `onInit` - For initialization + * `focusGiven` and `focusLost` for when the widget gains and loses focus. + * `getSetupStep` which returns a reference to the `SetupStep` associated with this widget. + +The widgets can emit three `Q_SIGNALS`: + * `dataUpdated` - When the underlying data has been updated (which can cause other steps to become "Ready") + * `advanceRequest` - When this signal is received, the GUI should attempt to advance to the next step. + * `setModalMode` - A way to signal that the widget cannot be switched away from at this time. + +## `SetupConfig` +`SetupConfig`s are where all the data for each part of the configuration is stored in code. Collectively, all the configs are a replacement for the massive `MoveItConfigData` class in MSA 1.0. + +In the config package (i.e. on the filesystem), data can be stored in one of two places. + * As a file in the package itself + * In the `.setup_assistant` yaml file in the root of the configuration package. + +The interface for `SetupConfig`: + * `onInit` - For initialization + * `isConfigured` - Returns true if this part of the configuration is completely set up. + * `loadPrevious` - Loads the configuration from an existing MoveIt configuration. Arguments include the path to the configuration package AND the yaml node (if any) from the `.setup_assistant` file matching this config's name. + * `saveToYaml` - Optionally save "meta" information for saving in the `.setup_assistant` yaml file + * `collectFiles` - Collect the files generated by this configuration and add them to the vector. See further explanation of `GeneratedFile`s below. + * `collectDependencies` - Collect the names of the packages that should be dependencies in the generated package if this `SetupConfig` is used. + * `collectVariables` - Collect key/value pairs for use in templates. See further explanation of templates below. + +Each of the `SetupConfig`s exist as singletons, managed by the `DataWarehouse` object. The configs are loaded via `pluginlib` so that the arbitrary new configs can be added and common operations can be run on all configs. Generically, the configs can be retrieved with + + SetupConfigPtr get(string config_name, string config_class) + +This returns a shared pointer to the generic `SetupConfig` with the given name. The `config_class` specifies how to load it with `pluginlib`. For conciseness, `config_class` has an empty default value, and you can register a class to be used with a given name. + +For additional syntactic sugar, you can also specify the class via template and get back a pointer to the specific `SetupConfig` class, i.e. + + config_data_->get("urdf") + +## `GeneratedFile` + +This class is a container for the logic for a single file to appear in MoveIt configuration package. + +The `collectFiles` method of `SetupConfig` allows us to specify all of the files we'd like to generate relative to a specific `SetupConfig`. There can be any number of files generated (zero, one or many) for each config. + + * `getRelativePath` - Returns the path relative to the configuration package root + * `getDescription` - Returns an English description of this file's purpose. + * `hasChanges` - Returns true if this file will have changes when it is written to file + * `write` - Writes the file to disk + +There are also two methods which depend on the implementations of the above methods. + * `getPath` - Appends the configuration package path passed in to the constructor with the relative path returned by `getRelativePath`. + * `getStatus` - Returns the status of the file, which may depend on `hasChanges`. + +The status can be in one of five (5) states, as specified by the `FileStatus` enum. + + * `NEW` - The file does not exist in the configuration package + * `UNCHANGED` - The file exists and would be the same as the generated file + * `CHANGED` - The file exists, but a new version will be written + * `EXTERNALLY_MODIFIED` - The file exists and was externally modified + * `CONFLICTED` - The file exists, was externally modified and there are changes to be written + +Checking for external modification depends on the timestamps of the written files, which is why `collectFiles` and the constructor for `GeneratedFile` require passing in the timestamp of the last package generation. + +Note it may be useful to call `moveit_setup::createParentFolders` before writing. + +There is also `YamlGeneratedFile` as an easy abstraction of generating a YAML file. The `write` method is already implemented, and instead `writeYaml` must be implemented. + +### Templates +One additional special type of `GeneratedFile` is `TemplatedGeneratedFile` which generates a text file from a common template. The `write` method is already written, and instead you simply must specify the full path to the template via `getTemplatePath`. The file will be generated as a copy of the template but using the values of `TemplateVariable`s inserted in the proper locations. The `TemplateVariable`s are collected via `SetupConfig::collectVariables`. + +The format for the templates is custom to MSA. Let's assume you have a `TemplateVariable` with `key=GENERATED_PACKAGE_NAME` and `value=r2d2_moveit_config`. The key surrounded by square brackets will be replaced with the value. For example, + + [GENERATED_PACKAGE_NAME] + +becomes + + r2d2_moveit_config diff --git a/moveit_setup_assistant/architecture/setup_assistant_diagram.odg b/moveit_setup_assistant/architecture/setup_assistant_diagram.odg deleted file mode 100644 index adfece6724..0000000000 Binary files a/moveit_setup_assistant/architecture/setup_assistant_diagram.odg and /dev/null differ diff --git a/moveit_setup_assistant/architecture/setup_assistant_diagram.png b/moveit_setup_assistant/architecture/setup_assistant_diagram.png deleted file mode 100644 index 589da4b772..0000000000 Binary files a/moveit_setup_assistant/architecture/setup_assistant_diagram.png and /dev/null differ diff --git a/moveit_setup_assistant/graphics/Wizard.png b/moveit_setup_assistant/graphics/Wizard.png new file mode 100644 index 0000000000..2a7716babd Binary files /dev/null and b/moveit_setup_assistant/graphics/Wizard.png differ diff --git a/moveit_setup_assistant/resources/moveit_logo.png b/moveit_setup_assistant/graphics/moveit_logo.png similarity index 100% rename from moveit_setup_assistant/resources/moveit_logo.png rename to moveit_setup_assistant/graphics/moveit_logo.png diff --git a/moveit_setup_assistant/resources/source/MoveIt_Setup_Asst_Sm.xcf b/moveit_setup_assistant/graphics/source/MoveIt_Setup_Asst_Sm.xcf similarity index 100% rename from moveit_setup_assistant/resources/source/MoveIt_Setup_Asst_Sm.xcf rename to moveit_setup_assistant/graphics/source/MoveIt_Setup_Asst_Sm.xcf diff --git a/moveit_setup_assistant/resources/source/moveit_logo_large.png b/moveit_setup_assistant/graphics/source/moveit_logo_large.png similarity index 100% rename from moveit_setup_assistant/resources/source/moveit_logo_large.png rename to moveit_setup_assistant/graphics/source/moveit_logo_large.png diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp deleted file mode 100644 index dd371f1418..0000000000 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp +++ /dev/null @@ -1,535 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Dave Coleman */ - -#pragma once - -#include -#include // for getting kinematic model -#include // for LinkPairMap -#include // outputting yaml config files -#include // to share throughout app -#include // for writing srdf data -#include - -#include - -namespace moveit_setup_assistant -{ -// ****************************************************************************************** -// Constants -// ****************************************************************************************** - -// Used for loading kinematic model -static const std::string ROBOT_DESCRIPTION = "robot_description"; -static const std::string MOVEIT_ROBOT_STATE = "moveit_robot_state"; - -// Default kin solver values -static const double DEFAULT_KIN_SOLVER_SEARCH_RESOLUTION = 0.005; -static const double DEFAULT_KIN_SOLVER_TIMEOUT = 0.005; - -// Used for logging -static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); - -// ****************************************************************************************** -// 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 - */ -struct ControllerConfig -{ - std::string name_; // controller name - std::string type_; // controller type - std::vector joints_; // joints controller by this controller -}; - -/** - * Planning parameters which may be set in the config files - */ -struct OmplPlanningParameter -{ - std::string name; // name of parameter - std::string value; // value parameter will receive (but as a string) - std::string comment; // comment briefly describing what this parameter does -}; - -/** \brief This class describes the OMPL planners by name, type, and parameter list, used to create the - * ompl_planning.yaml file */ -class OMPLPlannerDescription -{ -public: - /** \brief Constructor - * @param name: name of planner - * @parameter type: type of planner - */ - OMPLPlannerDescription(const std::string& name, const std::string& type) - { - name_ = name; - type_ = type; - }; - /** \brief Destructor */ - ~OMPLPlannerDescription() - { - parameter_list_.clear(); - }; - /** \brief adds a parameter to the planner description - * @param name: name of parameter to add - * @parameter: value: value of parameter as a string - * @parameter: value: value of parameter as a string - */ - void addParameter(const std::string& name, const std::string& value = "", const std::string& comment = "") - { - OmplPlanningParameter temp; - temp.name = name; - temp.value = value; - temp.comment = comment; - parameter_list_.push_back(temp); - } - std::vector parameter_list_; - std::string name_; // name of planner - std::string type_; // type of planner (geometric) -}; - -/** - * Reusable parameter class which may be used in reading and writing configuration files - */ -class GenericParameter -{ -public: - GenericParameter() - { - comment_ = ""; - }; - - void setName(std::string name) - { - name_ = std::move(name); - }; - void setValue(std::string value) - { - value_ = std::move(value); - }; - void setComment(std::string comment) - { - comment_ = std::move(comment); - }; - const std::string& getName() const - { - return name_; - }; - const std::string& getValue() const - { - return value_; - }; - const std::string& getComment() const - { - return comment_; - }; - -private: - std::string name_; // name of parameter - std::string value_; // value parameter will receive (but as a string) - std::string comment_; // comment briefly describing what this parameter does -}; - -MOVEIT_CLASS_FORWARD(MoveItConfigData); // Defines MoveItConfigDataPtr, ConstPtr, WeakPtr... etc - -/** \brief This class is shared with all widgets and contains the common configuration data - needed for generating each robot's MoveIt configuration package. - - All SRDF data is contained in a subclass of this class - - srdf_writer.cpp. This class also contains the functions for writing - out the configuration files. */ -class MoveItConfigData -{ -public: - 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) - - // All of the data needed for creating a MoveIt Configuration Files - - // ****************************************************************************************** - // URDF Data - // ****************************************************************************************** - - /// Full file-system path to urdf - std::string urdf_path_; - - /// Name of package containing urdf (note: this may be empty b/c user may not have urdf in pkg) - std::string urdf_pkg_name_; - - /// Path relative to urdf package (note: this may be same as urdf_path_) - std::string urdf_pkg_relative_path_; - - /// Flag indicating whether the URDF was loaded from .xacro format - bool urdf_from_xacro_; - /// xacro arguments - std::string xacro_args_; - - /// URDF robot model - urdf::ModelSharedPtr urdf_model_; - - /// URDF robot model string - std::string urdf_string_; - - /// Gazebo URDF robot model string - // NOTE: Created when the robot urdf is not compatible with Gazebo. - std::string gazebo_urdf_string_; - - /// Whether a new Gazebo URDF is created - bool save_gazebo_urdf_; - - // ****************************************************************************************** - // SRDF Data - // ****************************************************************************************** - - /// Full file-system path to srdf - std::string srdf_path_; - - /// Path relative to loaded configuration package - std::string srdf_pkg_relative_path_; - - /// SRDF Data and Writer - srdf::SRDFWriterPtr srdf_; - - // ****************************************************************************************** - // Other Data - // ****************************************************************************************** - - /// 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_; - - /// Loaded configuration package path - if an existing package was loaded, holds that path - std::string config_pkg_path_; - - /// Location that moveit_setup_assistant stores its templates - std::string template_package_path_; - - /// Is this application in debug mode? - bool debug_; - - /// Allowed collision matrix for robot poses - collision_detection::AllowedCollisionMatrix allowed_collision_matrix_; - - /// Timestamp when configuration package was generated, if it was previously generated - std::time_t config_pkg_generated_timestamp_; - - /// Name of the author of this config - std::string author_name_; - - /// Email of the author of this config - std::string author_email_; - - // ****************************************************************************************** - // Public Functions - // ****************************************************************************************** - - /// Load a robot model - void setRobotModel(const moveit::core::RobotModelPtr& robot_model); - - /// Provide a shared kinematic model loader - moveit::core::RobotModelConstPtr getRobotModel(); - - /// Update the Kinematic Model with latest SRDF modifications - void updateRobotModel(); - - /// Provide a shared planning scene - planning_scene::PlanningScenePtr getPlanningScene(); - - /** - * 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); - - /// Load the allowed collision matrix from the SRDF's list of link pairs - void loadAllowedCollisionMatrix(); - - // ****************************************************************************************** - // Public Functions for outputting configuration and setting files - // ****************************************************************************************** - std::vector getOMPLPlanners() const; - std::map getInitialJoints() const; - bool outputSetupAssistantFile(const std::string& file_path); - bool outputGazeboURDFFile(const std::string& file_path); - bool outputOMPLPlanningYAML(const std::string& file_path); - bool outputSTOMPPlanningYAML(const std::string& file_path); - bool outputKinematicsYAML(const std::string& file_path); - bool outputJointLimitsYAML(const std::string& file_path); - bool outputFakeControllersYAML(const std::string& file_path); - bool outputSimpleControllersYAML(const std::string& file_path); - bool outputROSControllersYAML(const std::string& file_path); - bool output3DSensorPluginYAML(const std::string& file_path); - - /** - * \brief Helper function to get the controller that is controlling the joint - * \return controller type - */ - std::string getJointHardwareInterface(const std::string& joint_name); - - /** - * \brief Decide the best two joints to be used for the projection evaluator - * \param planning_group name of group to use - * \return string - value to insert into yaml file - */ - std::string decideProjectionJoints(const std::string& planning_group); - - /** - * Input ompl_planning.yaml file for editing its values - * @param file_path path to ompl_planning.yaml in the input package - * @return true if the file was read correctly - */ - bool inputOMPLYAML(const std::string& file_path); - - /** - * Input kinematics.yaml file for editing its values - * @param file_path path to kinematics.yaml in the input package - * @return true if the file was read correctly - */ - bool inputKinematicsYAML(const std::string& file_path); - - /** - * Input planning_context.launch for editing its values - * @param file_path path to planning_context.launch in the input package - * @return true if the file was read correctly - */ - bool inputPlanningContextLaunch(const std::string& file_path); - - /** - * Helper function for parsing ros_controllers.yaml file - * @param YAML::Node - individual controller to be parsed - * @return true if the file was read correctly - */ - bool parseROSController(const YAML::Node& controller); - - /** - * Helper function for parsing ros_controllers.yaml file - * @param std::ifstream of ros_controller.yaml - * @return true if the file was read correctly - */ - bool processROSControllers(std::ifstream& input_stream); - - /** - * Input ros_controllers.yaml file for editing its values - * @param file_path path to ros_controllers.yaml in the input package - * @return true if the file was read correctly - */ - bool inputROSControllersYAML(const std::string& file_path); - - /** - * \brief Add a Follow Joint Trajectory action Controller for each Planning Group - * \return true if controllers were added to the controller_configs_ data structure - */ - bool addDefaultControllers(const std::string& controller_type = "effort_controllers/JointTrajectoryController"); - - /** - * Set package path; try to resolve path from package name if directory does not exist - * @param pkg_path path to package or package name - * @return true if the path was set - */ - bool setPackagePath(const std::string& pkg_path); - - /** - * determine the package name containing a given file path - * @param path to a file - * @param package_name holds the ros package name if found - * @param relative_filepath holds the relative path of the file to the package root - * @return whether the file belongs to a known package - */ - bool extractPackageNameFromPath(const std::string& path, std::string& package_name, - std::string& relative_filepath) const; - - /** - * Resolve path to .setup_assistant file - * @param path resolved path - * @return true if the path could be resolved - */ - bool getSetupAssistantYAMLPath(std::string& path); - - /// Make the full URDF path using the loaded .setup_assistant data - bool createFullURDFPath(); - - /// Make the full SRDF path using the loaded .setup_assistant data - bool createFullSRDFPath(const std::string& package_path); - - /** - * Input .setup_assistant file - contains data used for the MoveIt Setup Assistant - * - * @param file_path path to .setup_assistant file - * @return true if the file was read correctly - */ - bool inputSetupAssistantYAML(const std::string& file_path); - - /// Load perception sensor config (sensors_3d.yaml) into internal data structure - void input3DSensorsYAML(const std::string& file_path); - /// Load perception sensor config - static std::vector> load3DSensorsYAML(const std::string& file_path); - - /** - * Helper Function for joining a file path and a file name, or two file paths, etc, - * in a cross-platform way - * - * @param path1 first half of path - * @param path2 second half of path, or filename - * @return string resulting combined paths - */ - std::string appendPaths(const std::string& path1, const std::string& path2); - - /** - * \brief Adds a controller to controller_configs_ vector - * \param new_controller a new Controller to add - * \return true if inserted correctly - */ - bool addController(const ControllerConfig& new_controller); - - /** - * \brief Gets controller_configs_ vector - * \return pointer to controller_configs_ - */ - std::vector& getControllers() - { - return controller_configs_; - } - - /** - * Find the associated controller by name - * - * @param controller_name - name of controller to find in datastructure - * @return pointer to data in datastructure - */ - ControllerConfig* findControllerByName(const std::string& controller_name); - - /** - * Delete controller by name - * - * @param controller_name - name of controller to delete - * @return true if deleted, false if not found - */ - bool deleteController(const std::string& controller_name); - - /** - * \brief Used for adding a sensor plugin configuration parameter to the sensor plugin configuration parameter list - */ - void addGenericParameterToSensorPluginConfig(const std::string& name, const std::string& value = "", - const std::string& comment = ""); - - /** - * \brief Clear the sensor plugin configuration parameter list - */ - void clearSensorPluginConfig(); - - /** - * \brief Used for adding a sensor plugin configuration parameter to the sensor plugin configuration parameter list - */ - const std::vector>& getSensorPluginConfig() const - { - return sensors_plugin_config_parameter_list_; - } - - /** - * \brief Helper function to get the default start pose for moveit_sim_hw_interface - */ - srdf::Model::GroupState getDefaultStartPose(); - - /** - * \brief Custom std::set comparator, used for sorting the joint_limits.yaml file into alphabetical order - * \param jm1 - a pointer to the first joint model to compare - * \param jm2 - a pointer to the second joint model to compare - * \return bool of alphabetical sorting comparison - */ - struct JointModelCompare - { - bool operator()(const moveit::core::JointModel* jm1, const moveit::core::JointModel* jm2) const - { - return jm1->getName() < jm2->getName(); - } - }; - -private: - // ****************************************************************************************** - // Private Vars - // ****************************************************************************************** - - /// Sensor plugin configuration parameter list, each sensor plugin type is a map - std::vector> sensors_plugin_config_parameter_list_; - - /// Shared kinematic model - moveit::core::RobotModelPtr robot_model_; - - /// Controllers config data - std::vector controller_configs_; - - /// Shared planning scene - planning_scene::PlanningScenePtr planning_scene_; -}; - -} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/launch/setup_assistant.launch b/moveit_setup_assistant/launch/setup_assistant.launch deleted file mode 100644 index 104dc655da..0000000000 --- a/moveit_setup_assistant/launch/setup_assistant.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt new file mode 100644 index 0000000000..bd7b6dbc2e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.10.2) +project(moveit_setup_app_plugins) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(moveit_ros_visualization REQUIRED) +find_package(moveit_setup_framework REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) + +# Instruct CMake to run moc automatically when needed. +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +qt5_wrap_cpp(MOC_FILES + include/moveit_setup_app_plugins/launches_widget.hpp + include/moveit_setup_app_plugins/perception_widget.hpp +) + +add_library(${PROJECT_NAME} + src/launches.cpp + src/launches_widget.cpp + src/launches_config.cpp + src/perception_config.cpp + src/perception.cpp + src/perception_widget.cpp + ${MOC_FILES} +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + moveit_ros_visualization + moveit_setup_framework + pluginlib + rclcpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY templates DESTINATION share/${PROJECT_NAME}) + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(FILES moveit_setup_framework_plugins.xml + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) +pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) + +ament_package() diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp new file mode 100644 index 0000000000..604bfa6518 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp @@ -0,0 +1,217 @@ +/********************************************************************* + * 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 +{ +namespace app +{ +/** + * @brief One launch file and any other bonus files that get bundled with it, i.e. the RViz launch file and its config. + * + * Each launch bundle is presented to the user as option to generate. They are bundled because it would not (generally) + * make sense to generate the RViz config without the launch file. + */ +class LaunchBundle +{ +public: + /** + * @param title The display name for this LaunchBundle + * @param description An English description of the files contained within + * @param launch_name The identifier that signifies the name of the generated launch file (launch_name.launch.py) + * @param dependencies Each string is the name of a package that should be added as a dependency if this bundle is generated + */ + LaunchBundle(const std::string& title, const std::string& description, const std::string& launch_name, + const std::set& dependencies = {}) + : title_(title), description_(description), launch_name_(launch_name), dependencies_(dependencies), id_(-1) + { + } + + const std::string& getTitle() const + { + return title_; + } + + const std::string& getDescription() const + { + return description_; + } + + /** + * @brief The ID is an index in a list, used for quick identification and argument passing + */ + unsigned int getID() const + { + return id_; + } + + void setID(unsigned int id) + { + id_ = id; + } + + void addFile(const std::filesystem::path& relative_path, const std::string& description) + { + bonus_files_.push_back(BonusFile(relative_path, description)); + } + + const std::set getDependencies() const + { + return dependencies_; + } + + /** + * @brief Defined so that LaunchBundles can be added to sets + */ + bool operator<(const LaunchBundle& other) const + { + return title_ < other.title_; + } + + class GenericLaunchTemplate : public TemplatedGeneratedFile + { + public: + GenericLaunchTemplate(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + const LaunchBundle& parent) + : TemplatedGeneratedFile(package_path, last_gen_time), parent_(parent) + { + function_name_ = "generate_" + parent_.launch_name_ + "_launch"; + relative_path_ = std::filesystem::path("launch") / (parent_.launch_name_ + ".launch.py"); + template_path_ = getSharePath("moveit_setup_app_plugins") / "templates/launch/generic.launch.py.template"; + } + + std::filesystem::path getRelativePath() const override + { + return relative_path_; + } + + std::string getDescription() const override + { + return parent_.description_; + } + + std::filesystem::path getTemplatePath() const override + { + return template_path_; + } + + bool hasChanges() const override + { + return false; + } + + bool write() override + { + // Add function name as a TemplateVariable, then remove it + variables_.push_back(TemplateVariable("FUNCTION_NAME", function_name_)); + bool ret = TemplatedGeneratedFile::write(); + variables_.pop_back(); + return ret; + } + + protected: + const LaunchBundle& parent_; + std::string function_name_; + std::filesystem::path relative_path_, template_path_; + }; + + struct BonusFile // basically a std::pair + { + BonusFile(const std::filesystem::path& path, const std::string& description) : path(path), description(description) + { + } + std::filesystem::path path; + std::string description; + }; + + class BonusTemplatedFile : public TemplatedGeneratedFile + { + public: + BonusTemplatedFile(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + const std::filesystem::path& relative_path, const std::string& description) + : TemplatedGeneratedFile(package_path, last_gen_time), relative_path_(relative_path), description_(description) + { + } + + std::filesystem::path getRelativePath() const override + { + return relative_path_; + } + + std::string getDescription() const override + { + return description_; + } + + std::filesystem::path getTemplatePath() const override + { + return getSharePath("moveit_setup_app_plugins") / "templates" / relative_path_; + } + + bool hasChanges() const override + { + return false; + } + + protected: + std::filesystem::path relative_path_; + std::string description_; + }; + + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) const + { + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + + for (const BonusFile& bonus_file : bonus_files_) + { + files.push_back( + std::make_shared(package_path, last_gen_time, bonus_file.path, bonus_file.description)); + } + } + +protected: + std::string title_, description_, launch_name_; + std::set dependencies_; + unsigned int id_; + + std::vector bonus_files_; +}; +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches.hpp new file mode 100644 index 0000000000..7dd89fa805 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches.hpp @@ -0,0 +1,93 @@ +/********************************************************************* + * 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 +{ +namespace app +{ +/** + * @brief Setup step for generating launch files that are not otherwise associated with a specific step. + */ +class Launches : public SetupStep +{ +public: + std::string getName() const override + { + return "Launch Files"; + } + + void onInit() override; + bool isReady() const override + { + return true; // no generic dependencies + } + + /** + * @brief Get all available launch bundles + */ + const std::vector& getAvailableLaunchBundles() const + { + return available_launch_bundles_; + } + + /** + * @returns True if the LaunchBundle with the given id is currently included + */ + bool getState(unsigned int id) const; + + /** + * @brief Sets whether the LaunchBundle with the given id is included (true) or not (false) + */ + void setState(unsigned int id, bool state); + + /** + * @returns The description for the LaunchBundle with the given id + */ + const std::string& getDescription(unsigned int id) const + { + return available_launch_bundles_[id].getDescription(); + } + +protected: + std::vector available_launch_bundles_; + std::shared_ptr launches_config_; +}; +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_config.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_config.hpp new file mode 100644 index 0000000000..4033d8eb9a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_config.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * 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 +#include + +namespace moveit_setup +{ +namespace app +{ +/** + * @brief Stores which LaunchBundles are configured to be generated. + * + * Saved as a set in which the bundles to be generated are included in the set. + */ +class LaunchesConfig : public SetupConfig +{ +public: + bool isConfigured() const override + { + return !bundles_.empty(); + } + + /** + * @returns True if bundle is currently included + */ + bool isIncluded(const LaunchBundle& bundle) const; + + /** + * @brief Add the given launch bundle to the set + */ + void include(const LaunchBundle& bundle); + + /** + * @brief Remove the given launch bundle from the set + */ + void remove(const LaunchBundle& bundle); + + /** + * @brief Add the dependencies from the launch bundles to the moveit config's dependencies + */ + void collectDependencies(std::set& packages) const override; + + /** + * @brief Provide the files to be generated + */ + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) override; + +protected: + std::set bundles_; +}; +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/setup_screen_widget.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_widget.hpp similarity index 59% rename from moveit_setup_assistant/src/widgets/setup_screen_widget.h rename to moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_widget.hpp index 442c8927a1..8cd3393a54 100644 --- a/moveit_setup_assistant/src/widgets/setup_screen_widget.h +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_widget.hpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2022, Metro Robots * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * 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 Willow Garage nor the names of its + * * 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. * @@ -32,45 +32,56 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Dave Coleman */ - +/* Author: David V. Lu!! */ #pragma once -#include +#include +#include + +#include +#include -// ****************************************************************************************** -// Provides the title and instructions -// ****************************************************************************************** -class SetupScreenWidget : public QWidget +namespace moveit_setup +{ +namespace app +{ +class LaunchesWidget : public SetupStepWidget { Q_OBJECT public: - SetupScreenWidget(QWidget* parent) : QWidget(parent) + // ****************************************************************************************** + // Public Functions + // ****************************************************************************************** + + void onInit() override; + + SetupStep& getSetupStep() override { + return setup_step_; } - /// function called when widget is activated, allows to update/initialize GUI - virtual void focusGiven(); - - /// function called when widget lost focus, allows to accept/reject changes and to reject switching (returning false) - virtual bool focusLost(); + /// Received when this widget is chosen from the navigation menu + void focusGiven() override; +private Q_SLOTS: // ****************************************************************************************** - // Emitted Signal Functions + // Slot Event Functions // ****************************************************************************************** + void onNewSelectedItem(QListWidgetItem* current, QListWidgetItem* previous); -Q_SIGNALS: + /// Disable or enable item in list + void changeCheckedState(QListWidgetItem* item); - /// Event for when the current screen is in modal view. Essential disabled the left navigation - void isModal(bool isModal); - - /// Event for telling rviz to highlight a link of the robot - void highlightLink(const std::string& name, const QColor& /*_t2*/); +private: + // ****************************************************************************************** + // Variables + // ****************************************************************************************** - /// Event for telling rviz to highlight a group of the robot - void highlightGroup(const std::string& name); + Launches setup_step_; - /// Event for telling rviz to unhighlight all links of the robot - void unhighlightAll(); + QListWidget* list_widget_; + QLabel* text_widget_; }; +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception.hpp new file mode 100644 index 0000000000..4793c88560 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception.hpp @@ -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!! */ +#pragma once + +#include +#include + +namespace moveit_setup +{ +namespace app +{ +class Perception : public SetupStep +{ +public: + std::string getName() const override + { + return "Perception"; + } + + void onInit() override; + + bool isReady() const override + { + return true; // no dependencies + } + + const std::vector& getSensorPluginConfig() + { + return perception_config_->getSensorPluginConfig(); + } + + /** + * \brief Clear the sensor plugin configuration parameter list + */ + void clearSensorPluginConfig() + { + perception_config_->clearSensorPluginConfig(); + } + + void setConfig(const SensorParameters& parameters) + { + perception_config_->setConfig(parameters); + } + +protected: + std::shared_ptr perception_config_; +}; +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.hpp b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.hpp new file mode 100644 index 0000000000..e3f64b6a94 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.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 +{ +namespace app +{ +using SensorParameters = std::map; + +class PerceptionConfig : public SetupConfig +{ +public: + void loadPrevious(const std::filesystem::path& package_path, const YAML::Node& node) override; + + /// Load perception sensor config + static std::vector load3DSensorsYAML(const std::filesystem::path& file_path); + + bool isConfigured() const override + { + return !sensors_plugin_config_parameter_list_.empty(); + } + + /** + * \brief Used for adding a sensor plugin configuration parameter to the sensor plugin configuration parameter list + */ + const std::vector& getSensorPluginConfig() + { + return sensors_plugin_config_parameter_list_; + } + + /** + * \brief Clear the sensor plugin configuration parameter list + */ + void clearSensorPluginConfig(); + + void setConfig(const SensorParameters& parameters); + + class GeneratedSensorConfig : public YamlGeneratedFile + { + public: + GeneratedSensorConfig(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + PerceptionConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + bool hasChanges() const override + { + return parent_.changed_; + } + + std::filesystem::path getRelativePath() const override + { + return "config/sensors_3d.yaml"; + } + + std::string getDescription() const override + { + return "Configuration for perception with 3d sensors."; + } + + bool writeYaml(YAML::Emitter& emitter) override; + + protected: + PerceptionConfig& parent_; + }; + + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) override + { + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + } + +protected: + /// Sensor plugin configuration parameter list, each sensor plugin type is a map of string pairs + std::vector sensors_plugin_config_parameter_list_; + bool changed_{ false }; +}; +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/perception_widget.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.hpp similarity index 87% rename from moveit_setup_assistant/src/widgets/perception_widget.h rename to moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.hpp index c67c4ef4d7..18b60c13bc 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.h +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.hpp @@ -41,18 +41,17 @@ class QGroupBox; class QLineEdit; // 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 +{ +namespace app { // ****************************************************************************************** // User Interface for setting up 3D sensor config // ****************************************************************************************** -class PerceptionWidget : public SetupScreenWidget +class PerceptionWidget : public SetupStepWidget { Q_OBJECT @@ -61,7 +60,7 @@ class PerceptionWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - PerceptionWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; /// Received when this widget is chosen from the navigation menu void focusGiven() override; @@ -71,7 +70,11 @@ class PerceptionWidget : public SetupScreenWidget /// Populate the combo dropdown box with sensor plugins void loadSensorPluginsComboBox(); - uint loadConfigIntoWidgets(std::map sensor_plugin_config); + + SetupStep& getSetupStep() override + { + return setup_step_; + } // ****************************************************************************************** // Qt Components @@ -116,9 +119,8 @@ private Q_SLOTS: // ****************************************************************************************** // Variables // ****************************************************************************************** - - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + Perception setup_step_; }; -} // namespace moveit_setup_assistant +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/moveit_setup_framework_plugins.xml b/moveit_setup_assistant/moveit_setup_app_plugins/moveit_setup_framework_plugins.xml new file mode 100644 index 0000000000..d5a0b087cc --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/moveit_setup_framework_plugins.xml @@ -0,0 +1,14 @@ + + + Configures launches to generate + + + Configures the sensor suite + + + Data for Launch Files + + + Data for perception step + + diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml new file mode 100644 index 0000000000..d5d979ecc1 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml @@ -0,0 +1,27 @@ + + + + moveit_setup_app_plugins + 2.5.1 + Various specialty plugins for MoveIt Setup Assistant + David V. Lu!! + BSD + David V. Lu!! + + ament_cmake + + ament_index_cpp + moveit_ros_visualization + moveit_setup_framework + pluginlib + rclcpp + + ament_lint_auto + ament_clang_format + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/src/launches.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/launches.cpp new file mode 100644 index 0000000000..917a86d6c2 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/launches.cpp @@ -0,0 +1,108 @@ +/********************************************************************* + * 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 +{ +namespace app +{ +void Launches::onInit() +{ + config_data_->registerType("launches", "moveit_setup::app::LaunchesConfig"); + launches_config_ = config_data_->get("launches"); + + available_launch_bundles_.push_back( + LaunchBundle("Robot State Publisher Launch", + "Launch file to publish the robot description and transforms generated by the joint states.", "rsp", + { "robot_state_publisher" })); + available_launch_bundles_.push_back( + LaunchBundle("RViz Launch and Config", + "Visualize in Rviz the robot's planning groups running with interactive " + "markers that allow goal states to be set.", + "moveit_rviz", { "rviz2", "rviz_common", "rviz_default_plugins", "moveit_ros_visualization" })); + available_launch_bundles_.back().addFile( + "config/moveit.rviz", "Configuration file for Rviz with the Motion Planning Plugin already setup."); + available_launch_bundles_.push_back(LaunchBundle( + "MoveGroup Launch", "Launch file to run the main MoveIt executable that provides the MoveGroup action", + "move_group", { "moveit_ros_move_group" })); + available_launch_bundles_.push_back(LaunchBundle("Static TF Launch", + "Launch file to broadcast static TF for the robot's virtual joints.", + "static_virtual_joint_tfs", { "tf2_ros" })); + available_launch_bundles_.push_back(LaunchBundle("Spawn Controllers Launch", + "Launch file to spawn the necessary controllers", + "spawn_controllers", { "controller_manager" })); + available_launch_bundles_.push_back(LaunchBundle("Demo Launch", + "Launch file to run a demo of MoveGroup. Warning that it requires " + "the above launch files to all be generated as well.", + "demo")); + + available_launch_bundles_.push_back(LaunchBundle("Setup Assistant Launch", + "Launch file for easily re-starting the MoveIt " + "Setup Assistant to edit this robot's generated " + "configuration package.", + "setup_assistant", { "moveit_setup_assistant" })); + available_launch_bundles_.push_back(LaunchBundle("Warehouse DB Launch", + "Launch file for starting the warehouse with a default MongoDB.", + "warehouse_db", { "warehouse_ros_mongo", "moveit_ros_warehouse" })); + + for (unsigned int i = 0; i < available_launch_bundles_.size(); i++) + { + available_launch_bundles_[i].setID(i); + + // By default, we start with all the launch bundles included + launches_config_->include(available_launch_bundles_[i]); + } +} + +bool Launches::getState(unsigned int id) const +{ + return launches_config_->isIncluded(available_launch_bundles_[id]); +} + +void Launches::setState(unsigned int id, bool state) +{ + const LaunchBundle& bundle = available_launch_bundles_[id]; + if (state) + { + launches_config_->include(bundle); + } + else + { + launches_config_->remove(bundle); + } +} +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/src/launches_config.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/launches_config.cpp new file mode 100644 index 0000000000..e8c4385d71 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/launches_config.cpp @@ -0,0 +1,82 @@ +/********************************************************************* + * 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 +{ +namespace app +{ +bool LaunchesConfig::isIncluded(const LaunchBundle& bundle) const +{ + return bundles_.count(bundle); +} + +void LaunchesConfig::include(const LaunchBundle& bundle) +{ + bundles_.insert(bundle); +} + +void LaunchesConfig::remove(const LaunchBundle& bundle) +{ + bundles_.erase(bundle); +} + +void LaunchesConfig::collectDependencies(std::set& packages) const +{ + packages.insert("moveit_configs_utils"); + for (const LaunchBundle& bundle : bundles_) + { + for (const std::string& dependency : bundle.getDependencies()) + { + packages.insert(dependency); + } + } +} + +void LaunchesConfig::collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) +{ + for (const LaunchBundle& bundle : bundles_) + { + bundle.collectFiles(package_path, last_gen_time, files); + } +} +} // namespace app +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::app::LaunchesConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/src/launches_widget.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/launches_widget.cpp new file mode 100644 index 0000000000..96680229e4 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/launches_widget.cpp @@ -0,0 +1,128 @@ +/********************************************************************* + * 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 +#include + +#include + +namespace moveit_setup +{ +namespace app +{ +void LaunchesWidget::onInit() +{ + // Basic widget container + QVBoxLayout* layout = new QVBoxLayout(); + + // Top Header Area ------------------------------------------------ + + auto header = new HeaderWidget("Configure Desired Launch Files", + "Figure out which launch files you want to be generated.", this); + layout->addWidget(header); + + QSplitter* splitter = new QSplitter(Qt::Horizontal, this); + splitter->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + list_widget_ = new QListWidget(this); + list_widget_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + list_widget_->setSelectionMode(QAbstractItemView::ExtendedSelection); + connect(list_widget_, SIGNAL(currentItemChanged(QListWidgetItem*, QListWidgetItem*)), this, + SLOT(onNewSelectedItem(QListWidgetItem*, QListWidgetItem*))); + + splitter->addWidget(list_widget_); + + for (const LaunchBundle& lb : setup_step_.getAvailableLaunchBundles()) + { + QListWidgetItem* item = new QListWidgetItem(QString(lb.getTitle().c_str()), list_widget_); + item->setData(Qt::UserRole, QVariant(lb.getID())); + + list_widget_->addItem(item); + } + + text_widget_ = new QLabel(this); + text_widget_->setFrameShape(QFrame::StyledPanel); + text_widget_->setFrameShadow(QFrame::Raised); + text_widget_->setLineWidth(1); + text_widget_->setMidLineWidth(0); + text_widget_->setWordWrap(true); + text_widget_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + text_widget_->setMinimumWidth(100); + text_widget_->setAlignment(Qt::AlignTop); + splitter->addWidget(text_widget_); + layout->addWidget(splitter); + + // Finish Layout -------------------------------------------------- + this->setLayout(layout); +} + +unsigned int getID(QListWidgetItem* item) +{ + QVariant data = item->data(Qt::UserRole); + return data.toUInt(); +} + +void LaunchesWidget::focusGiven() +{ + disconnect(list_widget_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(changeCheckedState(QListWidgetItem*))); + for (int i = 0; i < list_widget_->count(); ++i) + { + QListWidgetItem* item = list_widget_->item(i); + + unsigned int id = getID(item); + + item->setCheckState(setup_step_.getState(id) ? Qt::Checked : Qt::Unchecked); + } + connect(list_widget_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(changeCheckedState(QListWidgetItem*))); +} + +void LaunchesWidget::onNewSelectedItem(QListWidgetItem* current, QListWidgetItem* /*previous*/) +{ + unsigned int id = getID(current); + text_widget_->setText(setup_step_.getDescription(id).c_str()); +} + +void LaunchesWidget::changeCheckedState(QListWidgetItem* item) +{ + unsigned int id = getID(item); + bool state = (item->checkState() == Qt::Checked); + setup_step_.setState(id, state); +} + +} // namespace app +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::app::LaunchesWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/src/widgets/setup_screen_widget.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception.cpp similarity index 79% rename from moveit_setup_assistant/src/widgets/setup_screen_widget.cpp rename to moveit_setup_assistant/moveit_setup_app_plugins/src/perception.cpp index c305b032b4..1054abc5c9 100644 --- a/moveit_setup_assistant/src/widgets/setup_screen_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2021, PickNik Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * 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 Willow Garage nor the names of its + * * 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. * @@ -32,16 +32,19 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Dave Coleman */ +/* Author: David V. Lu!! */ -#include "setup_screen_widget.h" -#include +#include -void SetupScreenWidget::focusGiven() +namespace moveit_setup { -} - -bool SetupScreenWidget::focusLost() +namespace app { - return true; // accept switching by default +void Perception::onInit() +{ + config_data_->registerType("sensors", "moveit_setup::app::PerceptionConfig"); + perception_config_ = config_data_->get("sensors"); } + +} // namespace app +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_config.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_config.cpp new file mode 100644 index 0000000000..b099815d9e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_config.cpp @@ -0,0 +1,171 @@ +/********************************************************************* + * 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 +{ +namespace app +{ +// ****************************************************************************************** +// Loads sensors_3d yaml file +// ****************************************************************************************** +void PerceptionConfig::loadPrevious(const std::filesystem::path& package_path, const YAML::Node& /*node*/) +{ + // Loads parameters values from sensors_3d yaml file if available + std::filesystem::path sensors_3d_yaml_path = package_path / "config/sensors_3d.yaml"; + if (!std::filesystem::is_regular_file(sensors_3d_yaml_path)) + { + sensors_3d_yaml_path = getSharePath("moveit_setup_app_plugins") / "templates" / "config/sensors_3d.yaml"; + } + try + { + sensors_plugin_config_parameter_list_ = load3DSensorsYAML(sensors_3d_yaml_path); + } + catch (const std::runtime_error& e) + { + RCLCPP_ERROR_STREAM(*logger_, e.what()); + } +} + +// ****************************************************************************************** +// Input sensors_3d yaml file +// ****************************************************************************************** +std::vector PerceptionConfig::load3DSensorsYAML(const std::filesystem::path& file_path) +{ + std::vector config; + // Is there a sensors config in the package? + if (file_path.empty()) + return config; + + // Load file + std::ifstream input_stream(file_path); + if (!input_stream.good()) + { + throw std::runtime_error("Unable to open file for reading " + file_path.string()); + } + + // Begin parsing + try + { + const YAML::Node& doc = YAML::Load(input_stream); + // Get sensors node + const YAML::Node& sensors_node = doc["sensors"]; + + // Make sure that the sensors are written as a sequence + if (sensors_node && sensors_node.IsSequence()) + { + // Loop over the sensors available in the file + for (const YAML::Node& sensor : sensors_node) + { + SensorParameters sensor_map; + bool empty_node = true; + + for (YAML::const_iterator sensor_it = sensor.begin(); sensor_it != sensor.end(); ++sensor_it) + { + empty_node = false; + sensor_map[sensor_it->first.as()] = sensor_it->second.as(); + } + // Don't push empty nodes + if (!empty_node) + config.push_back(sensor_map); + } + } + return config; + } + catch (YAML::ParserException& e) // Catch errors, rethrow as runtime_error + { + throw std::runtime_error(std::string("Error parsing sensors yaml: ") + e.what()); + } +} + +// ****************************************************************************************** +// Used to clear sensor plugin configuration parameter list +// ****************************************************************************************** +void PerceptionConfig::clearSensorPluginConfig() +{ + if (sensors_plugin_config_parameter_list_.empty()) + { + return; + } + changed_ = true; + sensors_plugin_config_parameter_list_.clear(); +} + +// ****************************************************************************************** +// Used to add a sensor plugin configuration parameter to the sensor plugin configuration parameter list +// ****************************************************************************************** +void PerceptionConfig::setConfig(const SensorParameters& parameters) +{ + changed_ = true; + if (sensors_plugin_config_parameter_list_.empty()) + { + sensors_plugin_config_parameter_list_.push_back(parameters); + } + else + { + // Right now, the widget only supports editing one plugin + sensors_plugin_config_parameter_list_[0] = parameters; + } +} + +// ****************************************************************************************** +// Output 3D Sensor configuration file +// ****************************************************************************************** +bool PerceptionConfig::GeneratedSensorConfig::writeYaml(YAML::Emitter& emitter) +{ + emitter << YAML::BeginMap; + emitter << YAML::Key << "sensors"; + emitter << YAML::Value << YAML::BeginSeq; + for (auto& sensor_config : parent_.sensors_plugin_config_parameter_list_) + { + emitter << YAML::BeginMap; + for (auto& parameter : sensor_config) + { + emitter << YAML::Key << parameter.first; + emitter << YAML::Value << parameter.second; + } + emitter << YAML::EndMap; + } + emitter << YAML::EndSeq; + emitter << YAML::EndMap; + return true; +} +} // namespace app +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::app::PerceptionConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/src/widgets/perception_widget.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp similarity index 54% rename from moveit_setup_assistant/src/widgets/perception_widget.cpp rename to moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp index 929153fb66..e47aa00aad 100644 --- a/moveit_setup_assistant/src/widgets/perception_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp @@ -34,8 +34,8 @@ /* Author: Mohamad Ayman */ // SA -#include "perception_widget.h" -#include "header_widget.h" +#include +#include // Qt #include @@ -46,13 +46,11 @@ #include #include -namespace moveit_setup_assistant +namespace moveit_setup { -// ****************************************************************************************** -// Constructor -// ****************************************************************************************** -PerceptionWidget::PerceptionWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +namespace app +{ +void PerceptionWidget::onInit() { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -60,14 +58,12 @@ PerceptionWidget::PerceptionWidget(QWidget* parent, const MoveItConfigDataPtr& c // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Setup 3D Perception Sensors", - "Configure your 3D sensors to work with MoveIt " - "Please see Perception Documentation " - "for more details.", - this); + auto header = new HeaderWidget( + "Setup 3D Perception Sensors", + "Configure your 3D sensors to work with MoveIt. Please see Perception Documentation for more details.", + this); layout->addWidget(header); // Add spacing @@ -208,55 +204,39 @@ bool PerceptionWidget::focusLost() if (sensor_plugin_field_->currentIndex() == 1) { // Point Cloud plugin fields - config_data_->addGenericParameterToSensorPluginConfig("sensor_plugin", "occupancy_map_monitor/" - "PointCloudOctomapUpdater"); - config_data_->addGenericParameterToSensorPluginConfig("point_cloud_topic", - point_cloud_topic_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("max_range", max_range_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("point_subsample", - point_subsample_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("padding_offset", - padding_offset_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("padding_scale", - padding_scale_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("max_update_rate", - max_update_rate_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("filtered_cloud_topic", - filtered_cloud_topic_field_->text().trimmed().toStdString()); - - config_data_->changes |= MoveItConfigData::SENSORS_CONFIG; + SensorParameters params; + params["sensor_plugin"] = "occupancy_map_monitor/PointCloudOctomapUpdater"; + params["point_cloud_topic"] = point_cloud_topic_field_->text().trimmed().toStdString(); + params["max_range"] = max_range_field_->text().trimmed().toStdString(); + params["point_subsample"] = point_subsample_field_->text().trimmed().toStdString(); + params["padding_offset"] = padding_offset_field_->text().trimmed().toStdString(); + params["padding_scale"] = padding_scale_field_->text().trimmed().toStdString(); + params["max_update_rate"] = max_update_rate_field_->text().trimmed().toStdString(); + params["filtered_cloud_topic"] = filtered_cloud_topic_field_->text().trimmed().toStdString(); + + setup_step_.setConfig(params); } else if (sensor_plugin_field_->currentIndex() == 2) { // Depth Map plugin fields - config_data_->addGenericParameterToSensorPluginConfig("sensor_plugin", "occupancy_map_monitor/" - "DepthImageOctomapUpdater"); - config_data_->addGenericParameterToSensorPluginConfig("image_topic", - image_topic_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("queue_size", - queue_size_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("near_clipping_plane_distance", - near_clipping_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("far_clipping_plane_distance", - far_clipping_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("shadow_threshold", - shadow_threshold_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("padding_scale", - depth_padding_scale_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("padding_offset", - depth_padding_offset_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig( - "filtered_cloud_topic", depth_filtered_cloud_topic_field_->text().trimmed().toStdString()); - config_data_->addGenericParameterToSensorPluginConfig("max_update_rate", - depth_max_update_rate_field_->text().trimmed().toStdString()); - - config_data_->changes |= MoveItConfigData::SENSORS_CONFIG; + SensorParameters params; + params["sensor_plugin"] = "occupancy_map_monitor/DepthImageOctomapUpdater"; + params["image_topic"] = image_topic_field_->text().trimmed().toStdString(); + params["queue_size"] = queue_size_field_->text().trimmed().toStdString(); + params["near_clipping_plane_distance"] = near_clipping_field_->text().trimmed().toStdString(); + params["far_clipping_plane_distance"] = far_clipping_field_->text().trimmed().toStdString(); + params["shadow_threshold"] = shadow_threshold_field_->text().trimmed().toStdString(); + params["padding_scale"] = depth_padding_scale_field_->text().trimmed().toStdString(); + params["padding_offset"] = depth_padding_offset_field_->text().trimmed().toStdString(); + params["filtered_cloud_topic"] = depth_filtered_cloud_topic_field_->text().trimmed().toStdString(); + params["max_update_rate"] = depth_max_update_rate_field_->text().trimmed().toStdString(); + + setup_step_.setConfig(params); } else { // Clear the sensors_plugin_config data structure - config_data_->clearSensorPluginConfig(); - config_data_->changes ^= MoveItConfigData::SENSORS_CONFIG; + setup_step_.clearSensorPluginConfig(); } return true; } @@ -283,35 +263,6 @@ void PerceptionWidget::sensorPluginChanged(int index) } } -uint PerceptionWidget::loadConfigIntoWidgets(std::map sensor_plugin_config) -{ - if (sensor_plugin_config["sensor_plugin"].getValue() == "occupancy_map_monitor/PointCloudOctomapUpdater") - { - point_cloud_topic_field_->setText(QString(sensor_plugin_config["point_cloud_topic"].getValue().c_str())); - max_range_field_->setText(QString(sensor_plugin_config["max_range"].getValue().c_str())); - point_subsample_field_->setText(QString(sensor_plugin_config["point_subsample"].getValue().c_str())); - padding_offset_field_->setText(QString(sensor_plugin_config["padding_offset"].getValue().c_str())); - padding_scale_field_->setText(QString(sensor_plugin_config["padding_scale"].getValue().c_str())); - max_update_rate_field_->setText(QString(sensor_plugin_config["max_update_rate"].getValue().c_str())); - filtered_cloud_topic_field_->setText(QString(sensor_plugin_config["filtered_cloud_topic"].getValue().c_str())); - return 1; - } - else if (sensor_plugin_config["sensor_plugin"].getValue() == "occupancy_map_monitor/DepthImageOctomapUpdater") - { - image_topic_field_->setText(QString(sensor_plugin_config["image_topic"].getValue().c_str())); - queue_size_field_->setText(QString(sensor_plugin_config["queue_size"].getValue().c_str())); - near_clipping_field_->setText(QString(sensor_plugin_config["near_clipping_plane_distance"].getValue().c_str())); - far_clipping_field_->setText(QString(sensor_plugin_config["far_clipping_plane_distance"].getValue().c_str())); - shadow_threshold_field_->setText(QString(sensor_plugin_config["shadow_threshold"].getValue().c_str())); - depth_padding_scale_field_->setText(QString(sensor_plugin_config["padding_scale"].getValue().c_str())); - depth_padding_offset_field_->setText(QString(sensor_plugin_config["padding_offset"].getValue().c_str())); - depth_filtered_cloud_topic_field_->setText(QString(sensor_plugin_config["filtered_cloud_topic"].getValue().c_str())); - depth_max_update_rate_field_->setText(QString(sensor_plugin_config["max_update_rate"].getValue().c_str())); - return 2; - } - return 0; -} - void PerceptionWidget::loadSensorPluginsComboBox() { // Only load this combo box once @@ -328,18 +279,38 @@ void PerceptionWidget::loadSensorPluginsComboBox() sensor_plugin_field_->addItem("Point Cloud"); sensor_plugin_field_->addItem("Depth Map"); - // Load values from default config - auto default_config = MoveItConfigData::load3DSensorsYAML( - config_data_->setup_assistant_path_ + "/templates/moveit_config_pkg_template/config/sensors_3d.yaml"); - for (const auto& sensor_plugin_config : default_config) - loadConfigIntoWidgets(sensor_plugin_config); - - // Load values from existing config - for (const auto& sensor_plugin_config : config_data_->getSensorPluginConfig()) + const auto& sensors_vec_map = setup_step_.getSensorPluginConfig(); + for (auto& sensor_plugin_config : sensors_vec_map) { - uint idx = loadConfigIntoWidgets(sensor_plugin_config); - sensor_plugin_field_->setCurrentIndex(idx); + if (sensor_plugin_config.at("sensor_plugin") == std::string("occupancy_map_monitor/PointCloudOctomapUpdater")) + { + sensor_plugin_field_->setCurrentIndex(1); + point_cloud_topic_field_->setText(QString(sensor_plugin_config.at("point_cloud_topic").c_str())); + max_range_field_->setText(QString(sensor_plugin_config.at("max_range").c_str())); + point_subsample_field_->setText(QString(sensor_plugin_config.at("point_subsample").c_str())); + padding_offset_field_->setText(QString(sensor_plugin_config.at("padding_offset").c_str())); + padding_scale_field_->setText(QString(sensor_plugin_config.at("padding_scale").c_str())); + max_update_rate_field_->setText(QString(sensor_plugin_config.at("max_update_rate").c_str())); + filtered_cloud_topic_field_->setText(QString(sensor_plugin_config.at("filtered_cloud_topic").c_str())); + } + else if (sensor_plugin_config.at("sensor_plugin") == std::string("occupancy_map_monitor/DepthImageOctomapUpdater")) + { + sensor_plugin_field_->setCurrentIndex(2); + image_topic_field_->setText(QString(sensor_plugin_config.at("image_topic").c_str())); + queue_size_field_->setText(QString(sensor_plugin_config.at("queue_size").c_str())); + near_clipping_field_->setText(QString(sensor_plugin_config.at("near_clipping_plane_distance").c_str())); + far_clipping_field_->setText(QString(sensor_plugin_config.at("far_clipping_plane_distance").c_str())); + shadow_threshold_field_->setText(QString(sensor_plugin_config.at("shadow_threshold").c_str())); + depth_padding_scale_field_->setText(QString(sensor_plugin_config.at("padding_scale").c_str())); + depth_padding_offset_field_->setText(QString(sensor_plugin_config.at("padding_offset").c_str())); + depth_filtered_cloud_topic_field_->setText(QString(sensor_plugin_config.at("filtered_cloud_topic").c_str())); + depth_max_update_rate_field_->setText(QString(sensor_plugin_config.at("max_update_rate").c_str())); + } } } -} // namespace moveit_setup_assistant +} // namespace app +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::app::PerceptionWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/templates/config/moveit.rviz b/moveit_setup_assistant/moveit_setup_app_plugins/templates/config/moveit.rviz new file mode 100644 index 0000000000..13d64f45f3 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/templates/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: [PLANNING_FRAME] + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: [PLANNING_FRAME] + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/sensors_3d.yaml b/moveit_setup_assistant/moveit_setup_app_plugins/templates/config/sensors_3d.yaml similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/config/sensors_3d.yaml rename to moveit_setup_assistant/moveit_setup_app_plugins/templates/config/sensors_3d.yaml diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/templates/launch/generic.launch.py.template b/moveit_setup_assistant/moveit_setup_app_plugins/templates/launch/generic.launch.py.template new file mode 100644 index 0000000000..444aabdd83 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/templates/launch/generic.launch.py.template @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import [FUNCTION_NAME] + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("[ROBOT_NAME]", package_name="[GENERATED_PACKAGE_NAME]").to_moveit_configs() + return [FUNCTION_NAME](moveit_config) diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst similarity index 100% rename from moveit_setup_assistant/CHANGELOG.rst rename to moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst diff --git a/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt new file mode 100644 index 0000000000..8fe123799f --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.10.2) +project(moveit_setup_assistant) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(Boost REQUIRED program_options) +find_package(moveit_setup_framework REQUIRED) +find_package(moveit_setup_srdf_plugins REQUIRED) +find_package(pluginlib REQUIRED) +find_package(Qt5Core REQUIRED) +find_package(Qt5Widgets REQUIRED) +find_package(rclcpp REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_index_cpp + moveit_setup_framework + moveit_setup_srdf_plugins + pluginlib + Qt5Core + Qt5Widgets + rclcpp +) + +# Header files that need Qt Moc pre-processing for use with Qt signals, etc: +set(HEADERS + include/moveit_setup_assistant/navigation_widget.hpp + include/moveit_setup_assistant/setup_assistant_widget.hpp +) +# Instruct CMake to run moc automatically when needed. +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +qt5_wrap_cpp(MOC_FILES + ${HEADERS} +) + +add_executable(moveit_setup_assistant + src/main.cpp + src/setup_assistant_widget.cpp + src/navigation_widget.cpp + ${MOC_FILES} +) +target_include_directories(moveit_setup_assistant PUBLIC + $ + $ +) +ament_target_dependencies( + moveit_setup_assistant + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +target_link_libraries(moveit_setup_assistant + ${Boost_LIBRARIES} +) + +add_executable(${PROJECT_NAME}_updater src/collisions_updater.cpp) +target_include_directories(${PROJECT_NAME}_updater PUBLIC + $ + $) + +ament_target_dependencies( + ${PROJECT_NAME}_updater + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +set_target_properties( + ${PROJECT_NAME}_updater + PROPERTIES OUTPUT_NAME collisions_updater + PREFIX "" +) + +install( + TARGETS + moveit_setup_assistant + ${PROJECT_NAME}_updater + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + INCLUDES DESTINATION include +) +install(DIRECTORY include/ + DESTINATION include +) +ament_export_include_directories(include) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY resources DESTINATION share/${PROJECT_NAME}) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Unit tests +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.h b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp similarity index 95% rename from moveit_setup_assistant/src/widgets/navigation_widget.h rename to moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp index 59bfba9e09..7ff6162f40 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.h +++ b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp @@ -40,7 +40,9 @@ #include class QStandardItemModel; -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace assistant { /** * Widget for showing a left hand side list of navigation items @@ -59,6 +61,8 @@ class NavigationWidget : public QListView void setEnabled(const int& index, bool enabled); void setSelected(const int& index); + bool isEnabled(const int& index) const; + private: QStandardItemModel* model_; }; @@ -79,4 +83,5 @@ class NavDelegate : public QStyledItemDelegate QSize sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const override; void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; }; -} // namespace moveit_setup_assistant +} // namespace assistant +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/setup_assistant_widget.hpp similarity index 64% rename from moveit_setup_assistant/src/widgets/setup_assistant_widget.h rename to moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/setup_assistant_widget.hpp index d855d79781..5b02bafdd0 100644 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.h +++ b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/setup_assistant_widget.hpp @@ -36,46 +36,30 @@ #pragma once +// ROS +#include +#include + // Qt +#include +#include +#include class QSplitter; // Setup Assistant -#include "navigation_widget.h" -#include "start_screen_widget.h" -#include "default_collisions_widget.h" -#include "planning_groups_widget.h" -#include "robot_poses_widget.h" -#include "end_effectors_widget.h" -#include "virtual_joints_widget.h" -#include "passive_joints_widget.h" -#include "author_information_widget.h" -#include "simulation_widget.h" -#include "configuration_files_widget.h" -#include "perception_widget.h" -#include "controllers_widget.h" +#include "moveit_setup_framework/qt/setup_step_widget.hpp" +#include "moveit_setup_framework/qt/rviz_panel.hpp" +#include "moveit_setup_framework/data_warehouse.hpp" +#include "moveit_setup_assistant/navigation_widget.hpp" #ifndef Q_MOC_RUN -#include - // Other #include // for parsing input arguments -#include #endif -// Forward declarations -namespace rviz -{ -class GridDisplay; -class RenderPanel; -class VisualizationManager; -} // namespace rviz - -namespace moveit_rviz_plugin +namespace moveit_setup { -class RobotStateDisplay; -} - -namespace moveit_setup_assistant +namespace assistant { class SetupAssistantWidget : public QWidget { @@ -91,13 +75,8 @@ class SetupAssistantWidget : public QWidget * @param parent - used by Qt for destructing all elements * @return */ - SetupAssistantWidget(QWidget* parent, const boost::program_options::variables_map& args); - - /** - * Deconstructor - * - */ - ~SetupAssistantWidget() override; + SetupAssistantWidget(rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr node, QWidget* parent, + const boost::program_options::variables_map& args); /** * Changes viewable screen @@ -148,80 +127,47 @@ private Q_SLOTS: void updateTimer(); /** - * Call a function that enables navigation and goes to screen 2 + * Function for handling the dataUpdated event */ - void progressPastStartScreen(); + void onDataUpdate(); /** - * Load Rviz once we have a robot description ready - * + * Advance to the next step */ - void loadRviz(); + void onAdvanceRequest(); /** * Change the widget modal state based on subwidgets state * * @param isModal if true disable left navigation */ - void setModalMode(bool isModal); - - /** - * Highlight a link of the robot - * - * @param link_name name of link to highlight - */ - void highlightLink(const std::string& link_name, const QColor& color); - - /** - * Highlight a robot group - */ - void highlightGroup(const std::string& group_name); - - /** - * Unhighlight all links of a robot - */ - void unhighlightAll(); - - // received when virtual joints that change the reference frame are added - void virtualJointReferenceFrameChanged(); + void onModalModeUpdate(bool isModal); private: // ****************************************************************************************** // Variables // ****************************************************************************************** + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr node_abstraction_; + rclcpp::Node::SharedPtr node_; QList nav_name_list_; NavigationWidget* navs_view_; - QWidget* rviz_container_; + RVizPanel* rviz_panel_; QSplitter* splitter_; QStackedWidget* main_content_; int current_index_; std::mutex change_screen_lock_; - // Rviz Panel - rviz::RenderPanel* rviz_render_panel_; - rviz::VisualizationManager* rviz_manager_; - moveit_rviz_plugin::RobotStateDisplay* robot_state_display_; - - // Screen Widgets - StartScreenWidget* start_screen_widget_; - DefaultCollisionsWidget* default_collisions_widget_; - PlanningGroupsWidget* planning_groups_widget; - RobotPosesWidget* robot_poses_widget_; - EndEffectorsWidget* end_effectors_widget_; - VirtualJointsWidget* virtual_joints_widget_; - PassiveJointsWidget* passive_joints_widget_; - AuthorInformationWidget* author_information_widget_; - ConfigurationFilesWidget* configuration_files_widget_; - SimulationWidget* simulation_widget_; - PerceptionWidget* perception_widget_; - ControllersWidget* controllers_widget_; + // Setup Steps + pluginlib::ClassLoader widget_loader_; + std::vector> steps_; /// Contains all the configuration data for the setup assistant - MoveItConfigDataPtr config_data_; + DataWarehousePtr config_data_; // ****************************************************************************************** // Private Functions // ****************************************************************************************** }; -} // namespace moveit_setup_assistant +} // namespace assistant +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_assistant/launch/setup_assistant.launch.py b/moveit_setup_assistant/moveit_setup_assistant/launch/setup_assistant.launch.py new file mode 100644 index 0000000000..e16f9dc6d6 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_assistant/launch/setup_assistant.launch.py @@ -0,0 +1,18 @@ +from launch import LaunchDescription +from moveit_configs_utils.launch_utils import ( + add_debuggable_node, + DeclareBooleanLaunchArg, +) + + +def generate_launch_description(): + ld = LaunchDescription() + + ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) + add_debuggable_node( + ld, + package="moveit_setup_assistant", + executable="moveit_setup_assistant", + ) + + return ld diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/moveit_setup_assistant/package.xml similarity index 73% rename from moveit_setup_assistant/package.xml rename to moveit_setup_assistant/moveit_setup_assistant/package.xml index 65dbf9ea6d..8273d3aab2 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/moveit_setup_assistant/package.xml @@ -8,6 +8,7 @@ Tyler Weaver Robert Haschke MoveIt Release Team + David V. Lu!! BSD @@ -15,26 +16,22 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 + David V. Lu!! Dave Coleman ament_cmake - moveit_common - ompl - ament_index_cpp - moveit_core - moveit_ros_planning - moveit_ros_visualization + moveit_setup_framework + moveit_setup_srdf_plugins + pluginlib + qtbase5-dev rclcpp - urdf - yaml-cpp - srdfdom - - xacro ament_lint_auto - ament_lint_common + ament_clang_format + ament_cmake_lint_cmake + ament_cmake_xmllint moveit_resources_panda_moveit_config ament_cmake_gtest diff --git a/moveit_setup_assistant/resources/MoveIt_Setup_Assistant2.png b/moveit_setup_assistant/moveit_setup_assistant/resources/MoveIt_Setup_Assistant2.png similarity index 100% rename from moveit_setup_assistant/resources/MoveIt_Setup_Assistant2.png rename to moveit_setup_assistant/moveit_setup_assistant/resources/MoveIt_Setup_Assistant2.png diff --git a/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp new file mode 100644 index 0000000000..48babb454c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp @@ -0,0 +1,171 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Fraunhofer IPA + * 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 Fraunhofer IPA 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: Mathias Lüdtke */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace po = boost::program_options; + +int main(int argc, char* argv[]) +{ + std::filesystem::path config_pkg_path; + std::filesystem::path urdf_path; + std::filesystem::path srdf_path; + std::filesystem::path output_path; + + bool include_default = false, include_always = false, keep_old = false, verbose = false; + + double min_collision_fraction = 1.0; + + uint32_t never_trials = 0; + + // clang-format off + po::options_description desc("Allowed options"); + desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to MoveIt config package")( + "urdf", po::value(&urdf_path), "path to URDF ( or xacro)")("srdf", po::value(&srdf_path), + "path to SRDF ( or xacro)")( + "output", po::value(&output_path), + "output path for SRDF")("xacro-args", po::value >()->composing(), + "additional arguments for xacro")("default", po::bool_switch(&include_default), + "disable default colliding pairs")( + "always", po::bool_switch(&include_always), "disable always colliding pairs")("keep", po::bool_switch(&keep_old), + "keep disabled link from SRDF")( + "verbose", po::bool_switch(&verbose), "verbose output")("trials", po::value(&never_trials), + "number of trials for searching never colliding pairs")( + "min-collision-fraction", po::value(&min_collision_fraction), + "fraction of small sample size to determine links that are always colliding"); + // clang-format on + + po::positional_options_description pos_desc; + pos_desc.add("xacro-args", -1); + + po::variables_map vm; + po::store(po::command_line_parser(argc, argv).options(desc).positional(pos_desc).run(), vm); + po::notify(vm); + + if (vm.count("help")) + { + std::cout << desc << '\n'; + return 1; + } + + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("collision_updater"); + static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); + moveit_setup::DataWarehousePtr config_data = std::make_shared(node); + + moveit_setup::srdf_setup::DefaultCollisions setup_step; + setup_step.initialize(node, config_data); + + if (!config_pkg_path.empty()) + { + auto package_settings = config_data->get("package_settings"); + try + { + package_settings->loadExisting(config_pkg_path); + } + catch (const std::runtime_error& e) + { + RCLCPP_ERROR_STREAM(LOGGER, "Could not load config at '" << config_pkg_path << "'. " << e.what()); + return 1; + } + } + else if (urdf_path.empty() || srdf_path.empty()) + { + RCLCPP_ERROR_STREAM(LOGGER, "Please provide config package or URDF and SRDF path"); + return 1; + } + else if (rdf_loader::RDFLoader::isXacroFile(srdf_path) && output_path.empty()) + { + RCLCPP_ERROR_STREAM(LOGGER, "Please provide a different output file for SRDF xacro input file"); + return 1; + } + + auto srdf_config = config_data->get("srdf"); + + // overwrite config paths if applicable + if (!urdf_path.empty()) + { + auto config = config_data->get("urdf"); + + std::vector xacro_args; + if (vm.count("xacro-args")) + xacro_args = vm["xacro-args"].as >(); + + config->loadFromPath(urdf_path, xacro_args); + } + if (!srdf_path.empty()) + { + srdf_config->loadSRDFFile(srdf_path); + } + + if (!keep_old) + { + srdf_config->clearCollisionData(); + } + + setup_step.startGenerationThread(never_trials, min_collision_fraction, verbose); + int thread_progress; + int last_progress = 0; + while ((thread_progress = setup_step.getThreadProgress()) < 100) + { + if (thread_progress - last_progress > 10) + { + RCLCPP_INFO(LOGGER, "%d%% complete...", thread_progress); + last_progress = thread_progress; + } + } + setup_step.joinGenerationThread(); + RCLCPP_INFO(LOGGER, "100%% complete..."); + + size_t skip_mask = 0; + if (!include_default) + skip_mask |= (1 << moveit_setup::srdf_setup::DEFAULT); + if (!include_always) + skip_mask |= (1 << moveit_setup::srdf_setup::ALWAYS); + + setup_step.linkPairsToSRDFSorted(skip_mask); + + srdf_config->write(output_path.empty() ? srdf_config->getPath() : output_path); + + return 0; +} diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/moveit_setup_assistant/src/main.cpp similarity index 79% rename from moveit_setup_assistant/src/setup_assistant_main.cpp rename to moveit_setup_assistant/moveit_setup_assistant/src/main.cpp index 2a22178710..f415cfaef4 100644 --- a/moveit_setup_assistant/src/setup_assistant_main.cpp +++ b/moveit_setup_assistant/moveit_setup_assistant/src/main.cpp @@ -34,8 +34,8 @@ /* Author: Dave Coleman */ -#include "widgets/setup_assistant_widget.h" -#include +#include "moveit_setup_assistant/setup_assistant_widget.hpp" +#include #include #include #include @@ -55,6 +55,14 @@ void usage(boost::program_options::options_description& desc, int exit_code) int main(int argc, char** argv) { + std::vector remaining_args = rclcpp::remove_ros_arguments(argc, argv); + std::vector clean_argv; + clean_argv.reserve(remaining_args.size()); + for (const std::string& arg : remaining_args) + { + clean_argv.push_back(const_cast(arg.c_str())); + } + // Parse parameters namespace po = boost::program_options; @@ -62,7 +70,7 @@ int main(int argc, char** argv) // clang-format off po::options_description desc("Allowed options"); desc.add_options()("help,h", "Show help message")("debug,g", "Run in debug/test mode")( - "urdf_path,u", po::value(), "Optional, path to URDF file in ROS package")( + "urdf_path,u", po::value(), "Optional, path to URDF file in ROS package")( "config_pkg,c", po::value(), "Optional, pass in existing config package to load"); // clang-format on @@ -70,7 +78,7 @@ int main(int argc, char** argv) po::variables_map vm; try { - po::store(po::parse_command_line(argc, argv, desc), vm); + po::store(po::parse_command_line(clean_argv.size(), &clean_argv[0], desc), vm); po::notify(vm); if (vm.count("help")) @@ -82,13 +90,8 @@ int main(int argc, char** argv) usage(desc, 1); } // Start ROS Node - ros::init(argc, argv, "moveit_setup_assistant", ros::init_options::NoSigintHandler); - - // ROS Spin - ros::AsyncSpinner spinner(1); - spinner.start(); - - ros::NodeHandle nh; + auto client = std::make_unique(); + auto node = client->init(argc, argv, "moveit_setup_assistant", false); // Create Qt Application QApplication qt_app(argc, argv); @@ -96,7 +99,7 @@ int main(int argc, char** argv) setlocale(LC_NUMERIC, "C"); // Load Qt Widget - moveit_setup_assistant::SetupAssistantWidget saw(nullptr, vm); + moveit_setup::assistant::SetupAssistantWidget saw(node, nullptr, vm); saw.setMinimumWidth(1090); saw.setMinimumHeight(600); // saw.setWindowState( Qt::WindowMaximized ); @@ -109,7 +112,7 @@ int main(int argc, char** argv) const int result = qt_app.exec(); // Shutdown ROS - ros::shutdown(); + rclcpp::shutdown(); return result; } diff --git a/moveit_setup_assistant/src/widgets/navigation_widget.cpp b/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp similarity index 95% rename from moveit_setup_assistant/src/widgets/navigation_widget.cpp rename to moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp index 56777454ef..bfd9b3cff9 100644 --- a/moveit_setup_assistant/src/widgets/navigation_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp @@ -34,14 +34,16 @@ /* Author: Dave Coleman */ -#include "navigation_widget.h" +#include "moveit_setup_assistant/navigation_widget.hpp" #include #include #include #include #include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace assistant { // ****************************************************************************************** // CLASS @@ -111,6 +113,11 @@ void NavigationWidget::setSelected(const int& index) selectionModel()->select(selection, QItemSelectionModel::Select); } +bool NavigationWidget::isEnabled(const int& index) const +{ + return model_->item(index)->flags() > Qt::NoItemFlags; +} + // ****************************************************************************************** // CLASS // ****************************************************************************************** @@ -174,4 +181,5 @@ void NavDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, c painter->restore(); } -} // namespace moveit_setup_assistant +} // namespace assistant +} // namespace moveit_setup 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 new file mode 100644 index 0000000000..4537f07c08 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_assistant/src/setup_assistant_widget.cpp @@ -0,0 +1,302 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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: Dave Coleman */ + +// SA +#include "moveit_setup_assistant/setup_assistant_widget.hpp" +#include +#include + +// Qt +#include +#include +#include +#include +#include +#include +#include +#include +#include // for loading all avail kinematic planners + +namespace moveit_setup +{ +namespace assistant +{ +// ****************************************************************************************** +// Outer User Interface for MoveIt Configuration Assistant +// ****************************************************************************************** +SetupAssistantWidget::SetupAssistantWidget(rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr node, + QWidget* parent, const boost::program_options::variables_map& args) + : QWidget(parent) + , node_abstraction_(node) + , node_(node_abstraction_.lock()->get_raw_node()) + , widget_loader_("moveit_setup_framework", "moveit_setup::SetupStepWidget") +{ + // Create object to hold all MoveIt configuration data + config_data_ = std::make_shared(node_); + + // Set debug mode flag if necessary + if (args.count("debug")) + config_data_->debug = true; + + // Setting the window icon + auto icon_path = getSharePath("moveit_ros_visualization") / "icons/classes/MotionPlanning.png"; + this->setWindowIcon(QIcon(icon_path.c_str())); + + // Basic widget container ----------------------------------------- + QHBoxLayout* layout = new QHBoxLayout(); + layout->setAlignment(Qt::AlignTop); + + // Create main content stack for various screens + main_content_ = new QStackedWidget(); + current_index_ = -1; + + // Setup Steps -------------------------------------------------------- + std::vector setup_steps; + // TODO: The list of setup steps should be read from a parameter with some as the default + // TODO: (or be configured dynamically in some other step) + setup_steps.push_back("moveit_setup::core::StartScreenWidget"); + setup_steps.push_back("moveit_setup::srdf_setup::DefaultCollisionsWidget"); + setup_steps.push_back("moveit_setup::srdf_setup::VirtualJointsWidget"); + setup_steps.push_back("moveit_setup::srdf_setup::PlanningGroupsWidget"); + setup_steps.push_back("moveit_setup::srdf_setup::RobotPosesWidget"); + setup_steps.push_back("moveit_setup::srdf_setup::EndEffectorsWidget"); + setup_steps.push_back("moveit_setup::srdf_setup::PassiveJointsWidget"); + setup_steps.push_back("moveit_setup::controllers::UrdfModificationsWidget"); + setup_steps.push_back("moveit_setup::controllers::ROS2ControllersWidget"); + setup_steps.push_back("moveit_setup::controllers::MoveItControllersWidget"); + // setup_steps.push_back("moveit_setup::simulation::SimulationWidget"); + setup_steps.push_back("moveit_setup::app::PerceptionWidget"); + setup_steps.push_back("moveit_setup::app::LaunchesWidget"); + setup_steps.push_back("moveit_setup::core::AuthorInformationWidget"); + setup_steps.push_back("moveit_setup::core::ConfigurationFilesWidget"); + node_->declare_parameter("setup_steps", setup_steps); + setup_steps = node_->get_parameter("setup_steps").as_string_array(); + + // Rviz View Right Pane --------------------------------------------------- + rviz_panel_ = new RVizPanel(this, node_abstraction_, config_data_); + rviz_panel_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + rviz_panel_->hide(); // do not show until after the start screen + + for (const std::string& step_class : setup_steps) + { + auto widget = widget_loader_.createSharedInstance(step_class); + widget->initialize(node_, this, rviz_panel_, config_data_); + + connect(widget.get(), SIGNAL(dataUpdated()), this, SLOT(onDataUpdate())); + connect(widget.get(), SIGNAL(advanceRequest()), this, SLOT(onAdvanceRequest())); + connect(widget.get(), SIGNAL(setModalMode(bool)), this, SLOT(onModalModeUpdate(bool))); + const std::string name = widget->getSetupStep().getName(); + steps_.push_back(widget); + + main_content_->addWidget(widget.get()); + + nav_name_list_ << name.c_str(); + } + + // Pass command arg values to start screen and show appropriate part of screen + if (args.count("urdf_path")) + { + std::filesystem::path urdf_path = args["urdf_path"].as(); + auto config = config_data_->get("urdf"); + config->loadFromPath(urdf_path); + } + if (args.count("config_pkg")) + { + std::string config_pkg = args["config_pkg"].as(); + auto package_settings = config_data_->get("package_settings"); + package_settings->loadExisting(config_pkg); + } + + // Navigation Left Pane -------------------------------------------------- + navs_view_ = new NavigationWidget(this); + navs_view_->setNavs(nav_name_list_); + if (!steps_.empty()) + { + navs_view_->setEnabled(0, true); + moveToScreen(0); + } + + // Split screen ----------------------------------------------------- + splitter_ = new QSplitter(Qt::Horizontal, this); + splitter_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + splitter_->addWidget(navs_view_); + splitter_->addWidget(main_content_); + splitter_->addWidget(rviz_panel_); + splitter_->setHandleWidth(6); + // splitter_->setCollapsible( 0, false ); // don't let navigation collapse + layout->addWidget(splitter_); + + // Add event for switching between screens ------------------------- + connect(navs_view_, SIGNAL(clicked(const QModelIndex&)), this, SLOT(navigationClicked(const QModelIndex&))); + + // Final Layout Setup --------------------------------------------- + this->setLayout(layout); + + // Title + this->setWindowTitle("MoveIt Setup Assistant"); // title of window + + // Show screen before message + QApplication::processEvents(); +} + +// ****************************************************************************************** +// Change screens of Setup Assistant +// ****************************************************************************************** +void SetupAssistantWidget::navigationClicked(const QModelIndex& index) +{ + // Convert QModelIndex to int + moveToScreen(index.row()); +} + +void SetupAssistantWidget::onDataUpdate() +{ + for (size_t index = 0; index < steps_.size(); index++) + { + bool ready = steps_[index]->isReady(); + navs_view_->setEnabled(index, ready); + } + + if (rviz_panel_->isReadyForInitialization()) + { + rviz_panel_->initialize(); + // Replace logo with Rviz screen + rviz_panel_->show(); + } +} + +void SetupAssistantWidget::onAdvanceRequest() +{ + if (static_cast(current_index_ + 1) < steps_.size()) + { + moveToScreen(current_index_ + 1); + } +} + +// ****************************************************************************************** +// Change screens +// ****************************************************************************************** +void SetupAssistantWidget::moveToScreen(const int index) +{ + std::scoped_lock slock(change_screen_lock_); + if (!navs_view_->isEnabled(index)) + { + return; + } + + if (current_index_ != index) + { + // Send the focus lost command to the screen widget + if (current_index_ >= 0) + { + auto ssw = steps_[current_index_]; + if (!ssw->focusLost()) + { + navs_view_->setSelected(current_index_); + return; // switching not accepted + } + } + + current_index_ = index; + + // Unhighlight anything on robot + rviz_panel_->unhighlightAll(); + + // Change screens + main_content_->setCurrentIndex(index); + + // Send the focus given command to the screen widget + steps_[current_index_]->focusGiven(); + + // Change navigation selected option + navs_view_->setSelected(index); + } +} + +// ****************************************************************************************** +// Ping ROS on interval +// ****************************************************************************************** +void SetupAssistantWidget::updateTimer() +{ + // TODO: Figure out if there's a ROS 2 equivalent of this that needs to be run + // ros::spinOnce(); // keep ROS node alive +} + +// ****************************************************************************************** +// Qt close event function for reminding user to save +// ****************************************************************************************** +void SetupAssistantWidget::closeEvent(QCloseEvent* event) +{ + // Only prompt to close if not in debug mode + if (!config_data_->debug) + { + if (QMessageBox::question(this, "Exit Setup Assistant", + QString("Are you sure you want to exit the MoveIt Setup Assistant?"), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) + { + event->ignore(); + return; + } + } + + // Shutdown app + event->accept(); +} + +// ****************************************************************************************** +// Qt Error Handling - TODO +// ****************************************************************************************** +bool SetupAssistantWidget::notify(QObject* /*receiver*/, QEvent* /*event*/) +{ + QMessageBox::critical(this, "Error", "An error occurred and was caught by Qt notify event handler.", QMessageBox::Ok); + + return false; +} + +// ****************************************************************************************** +// Change the widget modal state based on subwidgets state +// ****************************************************************************************** +void SetupAssistantWidget::onModalModeUpdate(bool isModal) +{ + navs_view_->setDisabled(isModal); + + for (int i = 0; i < nav_name_list_.count(); ++i) + { + navs_view_->setEnabled(i, !isModal && steps_[i]->isReady()); + } +} + +} // namespace assistant +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt new file mode 100644 index 0000000000..2ccb04e884 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.10.2) +project(moveit_setup_controllers) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(moveit_setup_framework REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) + +# Instruct CMake to run moc automatically when needed. +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +qt5_wrap_cpp(MOC_FILES + include/moveit_setup_controllers/controller_edit_widget.hpp + include/moveit_setup_controllers/controllers_widget.hpp + include/moveit_setup_controllers/urdf_modifications_widget.hpp +) + +add_library(${PROJECT_NAME} + src/control_xacro_config.cpp + src/controller_edit_widget.cpp + src/controllers.cpp + src/controllers_widget.cpp + src/controllers_config.cpp + src/modified_urdf_config.cpp + src/moveit_controllers_config.cpp + src/moveit_controllers_widget.cpp + src/ros2_controllers_config.cpp + src/ros2_controllers_widget.cpp + src/urdf_modifications.cpp + src/urdf_modifications_widget.cpp + ${MOC_FILES} +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + moveit_setup_framework + pluginlib + rclcpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY templates DESTINATION share/${PROJECT_NAME}) + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(FILES moveit_setup_framework_plugins.xml + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) +pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) + +ament_package() diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/control_xacro_config.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/control_xacro_config.hpp new file mode 100644 index 0000000000..d7a597d221 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/control_xacro_config.hpp @@ -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!! */ + +#pragma once + +#include +#include + +namespace moveit_setup +{ +namespace controllers +{ +struct ControlInterfaces +{ + std::vector command_interfaces; + std::vector state_interfaces; +}; + +inline std::vector getAvailableInterfaceNames() +{ + return { "position", "velocity", "effort" }; +} + +class ControlXacroConfig : public IncludedXacroConfig +{ +public: + /**@name SetupConfig overrides */ + /**@{*/ + void onInit() override; + void loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node) override; + YAML::Node saveToYaml() const override; + bool isConfigured() const override; + /**@}*/ + + /**@name IncludedXacroConfig overrides */ + /**@{*/ + std::string getFilepath() const override; + bool hasChanges() const override; + std::vector> getArguments() const override; + std::vector getCommands() const override; + /**@}*/ + + /** + * @brief Load the original command interfaces from the original (unmodified) URDF + * + * Needs to be run whenever the URDF/SRDF may have changed + */ + void loadFromDescription(); + + bool hasAllControlTagsInOriginal() const; + + /**@name Data access and modification */ + /**@{*/ + + const ControlInterfaces& getAvailableControlInterfaces() const + { + return available_ci_; + } + + const ControlInterfaces& getDefaultControlInterfaces() const + { + return default_ci_; + } + + /** + * @brief Use the specified controller interfaces for all the lacking joints + */ + void setControlInterfaces(const ControlInterfaces& ci); + + /** + * @brief Get all the control interfaces for all of the specified joint names + */ + const ControlInterfaces getControlInterfaces(const std::vector& joint_names) const; + + /**@}*/ + + /** + * @brief Return the additional joint xml needed for ros2_control tags + */ + std::string getJointsXML() const; + + class GeneratedControlHeader : public TemplatedGeneratedFile + { + public: + GeneratedControlHeader(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + ControlXacroConfig& parent) + : TemplatedGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + std::filesystem::path getRelativePath() const override + { + return std::filesystem::path("config") / (parent_.urdf_config_->getRobotName() + ".ros2_control.xacro"); + } + + std::filesystem::path getTemplatePath() const override + { + return getSharePath("moveit_setup_controllers") / "templates" / "config" / "ros2_control.xacro"; + } + + std::string getDescription() const override + { + return "Macro definition for required ros2_control xacro additions."; + } + + bool hasChanges() const override + { + return parent_.hasChanges(); + } + + protected: + ControlXacroConfig& parent_; + }; + + class GeneratedInitialPositions : public YamlGeneratedFile + { + public: + GeneratedInitialPositions(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + ControlXacroConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + bool hasChanges() const override + { + return parent_.hasChanges(); + } + + std::filesystem::path getRelativePath() const override + { + return "config/initial_positions.yaml"; + } + + std::string getDescription() const override + { + return "Initial positions for ros2_control"; + } + + bool writeYaml(YAML::Emitter& emitter) override; + + protected: + ControlXacroConfig& parent_; + }; + + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) override + { + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + } + + void collectVariables(std::vector& variables) override; + +protected: + void getControlInterfaces(const std::string& joint_name, ControlInterfaces& ci) const; + + std::vector joint_names_; /// A list of all joints used by the current SRDF groups + std::unordered_map original_joint_interfaces_, new_joint_interfaces_; + + ControlInterfaces default_ci_, available_ci_; + srdf::Model::GroupState initial_group_state_; + + bool changed_; +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controller_edit_widget.hpp similarity index 89% rename from moveit_setup_assistant/src/widgets/controller_edit_widget.h rename to moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controller_edit_widget.hpp index 0b5b8c4de0..23cad7b03b 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.h +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controller_edit_widget.hpp @@ -35,17 +35,16 @@ #pragma once +#include #include -class QComboBox; -class QLabel; -class QLineEdit; -class QPushButton; +#include +#include +#include +#include -#ifndef Q_MOC_RUN -#include -#endif - -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace controllers { class ControllerEditWidget : public QWidget { @@ -57,13 +56,13 @@ class ControllerEditWidget : public QWidget // ****************************************************************************************** /// Constructor - ControllerEditWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + ControllerEditWidget(QWidget* parent); /// Set the previous data - void setSelected(const std::string& controller_name); + void setSelected(const std::string& controller_name, const ControllerInfo* info); /// Populate the combo dropdown box with controllers types - void loadControllersTypesComboBox(); + void loadControllersTypesComboBox(const std::vector& controller_types); /// Hide delete controller button void hideDelete(); @@ -137,7 +136,6 @@ private Q_SLOTS: // For loading default types combo box just once bool has_loaded_ = false; - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; }; -} // namespace moveit_setup_assistant +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.hpp new file mode 100644 index 0000000000..aadaad49a3 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.hpp @@ -0,0 +1,106 @@ +/********************************************************************* + * 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 + +namespace moveit_setup +{ +namespace controllers +{ +class Controllers : public SetupStep +{ +public: + virtual std::string getInstructions() const = 0; + + virtual std::string getButtonText() const = 0; + + virtual std::vector getAvailableTypes() const = 0; + + virtual std::string getDefaultType() const = 0; + + bool isReady() const override + { + return !srdf_config_->getGroups().empty(); + } + + const std::vector& getJointNames() const + { + return srdf_config_->getRobotModel()->getJointModelNames(); + } + + std::vector getGroupNames() const + { + return srdf_config_->getGroupNames(); + } + + std::string getChildOfJoint(const std::string& joint_name) const + { + return srdf_config_->getChildOfJoint(joint_name); + } + + std::vector& getControllers() const + { + return controllers_config_->getControllers(); + } + + bool addController(const ControllerInfo& new_controller) + { + return controllers_config_->addController(new_controller); + } + + ControllerInfo* findControllerByName(const std::string& controller_name) + { + return controllers_config_->findControllerByName(controller_name); + } + + bool deleteController(const std::string& controller_name) + { + return controllers_config_->deleteController(controller_name); + } + + bool addDefaultControllers(); + + std::vector getJointsFromGroups(const std::vector& group_names) const; + +protected: + std::shared_ptr srdf_config_; + std::shared_ptr controllers_config_; +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_config.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_config.hpp new file mode 100644 index 0000000000..0de715a930 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_config.hpp @@ -0,0 +1,121 @@ +/********************************************************************* + * 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 +#include +#include + +namespace moveit_setup +{ +namespace controllers +{ +/** + * Single controller "instance" configuration + */ +struct ControllerInfo +{ + std::string name_; // controller name + std::string type_; // controller type + std::vector joints_; // joints controlled by this controller +}; + +/** + * @brief All the controller configurations + */ +class ControllersConfig : public SetupConfig +{ +public: + bool isConfigured() const override + { + return !controllers_.empty(); + } + + /** + * \brief Gets controllers_ vector + */ + std::vector& getControllers() + { + return controllers_; + } + + /** + * \brief Adds a controller to controllers_ vector + * \param name Name of the controller + * \param type type of the controller + * \param joint_names vector of the joint names + * \return true if inserted correctly + */ + bool addController(const std::string& name, const std::string& type, const std::vector& joint_names); + + /** + * \brief Adds a controller to controllers_ vector + * \param new_controller a new Controller to add + * \return true if inserted correctly + */ + bool addController(const ControllerInfo& new_controller); + + /** + * Find the associated controller by name + * + * @param controller_name - name of controller to find in datastructure + * @return pointer to data in datastructure + */ + ControllerInfo* findControllerByName(const std::string& controller_name); + + /** + * Delete controller by name + * + * @param controller_name - name of controller to delete + * @return true if deleted, false if not found + */ + bool deleteController(const std::string& controller_name); + + bool hasChangedGroups() const + { + auto srdf_config = config_data_->get("srdf"); + return srdf_config->getChangeMask() & GROUPS; + } + +protected: + /// Controllers config data + std::vector controllers_; + bool changed_; +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/controllers_widget.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_widget.hpp similarity index 79% rename from moveit_setup_assistant/src/widgets/controllers_widget.h rename to moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_widget.hpp index 5ab5c4d2aa..303f977c55 100644 --- a/moveit_setup_assistant/src/widgets/controllers_widget.h +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_widget.hpp @@ -36,25 +36,23 @@ #pragma once // Qt -class QHBoxLayout; -class QPushButton; -class QStackedWidget; -class QTreeWidget; -class QTreeWidgetItem; +#include +#include +#include +#include +#include // SA -#ifndef Q_MOC_RUN -#include -#endif +#include +#include +#include +#include -#include "setup_screen_widget.h" // a base class for screens in the setup assistant - -namespace moveit_setup_assistant +namespace moveit_setup { -class DoubleListWidget; -class ControllerEditWidget; - -class ControllersWidget : public SetupScreenWidget +namespace controllers +{ +class ControllersWidget : public SetupStepWidget { Q_OBJECT @@ -63,13 +61,18 @@ class ControllersWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - ControllersWidget(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; + SetupStep& getSetupStep() override + { + return *setup_step_; + } + private Q_SLOTS: // ****************************************************************************************** @@ -115,7 +118,7 @@ private Q_SLOTS: /// Called sleceted item changed void itemSelectionChanged(); -private: +protected: // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -132,8 +135,8 @@ private Q_SLOTS: QPushButton* btn_add_; QPushButton* btn_edit_; QHBoxLayout* controls_layout_; - moveit_setup_assistant::DoubleListWidget* joints_widget_; - moveit_setup_assistant::DoubleListWidget* joint_groups_widget_; + DoubleListWidget* joints_widget_; + DoubleListWidget* joint_groups_widget_; /// Remember what controller we are editing when an edit screen is being shown std::string current_edit_controller_; @@ -141,18 +144,18 @@ private Q_SLOTS: /// Remember whethere we're editing a controller or adding a new one bool adding_new_controller_; - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + std::shared_ptr setup_step_; /// Builds the main screen list widget QWidget* createContentsWidget(); void loadControllersTree(); - void loadToControllersTree(const moveit_setup_assistant::ControllerConfig& controller_it); + void loadToControllersTree(const ControllerInfo& controller_it); void showMainScreen(); - void loadJointsScreen(moveit_setup_assistant::ControllerConfig* this_controller); - void loadGroupsScreen(moveit_setup_assistant::ControllerConfig* this_controller); - void loadControllerScreen(moveit_setup_assistant::ControllerConfig* this_controller); + void loadJointsScreen(ControllerInfo* this_controller); + void loadGroupsScreen(ControllerInfo* this_controller); + void loadControllerScreen(ControllerInfo* this_controller); }; -} // namespace moveit_setup_assistant +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/included_xacro_config.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/included_xacro_config.hpp new file mode 100644 index 0000000000..19d2bd8ff9 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/included_xacro_config.hpp @@ -0,0 +1,99 @@ +/********************************************************************* + * 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 +{ +/** + * @brief A virtual class that represents a xacro header that should be included in the modified urdf configuration + */ +class IncludedXacroConfig : public SetupConfig +{ +public: + void onInit() override + { + // Register the modified config and access it to ensure it is generated + config_data_->registerType("modified_urdf", "moveit_setup::ModifiedUrdfConfig"); + config_data_->get("modified_urdf", "moveit_setup::ModifiedUrdfConfig"); + + urdf_config_ = config_data_->get("urdf"); + } + + /** + * @brief The file path to use in the tag. + * + * If the file is in MoveIt config's config folder, just the file name is fine. + * Alternatively you can return a package string a la + * `$(find SOME_PACKAGE_NAME)/relative/path/to/header.xacro` + */ + virtual std::string getFilepath() const = 0; + + /** + * @brief Returns if the xacro and its properties have changed, resulting in the whole urdf needing regeneration + */ + virtual bool hasChanges() const = 0; + + /** + * @brief Returns a list of name/value pairs for arguments that the modified urdf should have + * + * Result will be + */ + virtual std::vector> getArguments() const + { + return {}; + }; + + /** + * @brief Return a list of additional commands that need to be inserted after the xacro is included. + * + * e.g. If the included xacro includes a macro definition, the command to run the macro could be here. + */ + virtual std::vector getCommands() const + { + return {}; + } + + using Ptr = std::shared_ptr; + +protected: + std::shared_ptr urdf_config_; +}; + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/modified_urdf_config.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/modified_urdf_config.hpp new file mode 100644 index 0000000000..b58db6abc6 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/modified_urdf_config.hpp @@ -0,0 +1,133 @@ +/********************************************************************* + * 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 +{ +/** + * @brief A configuration that stores info about modifications to the URDF + * + * The modifications are primarily made in an included xacro file (which can be configured dynamically) + */ +class ModifiedUrdfConfig : public SetupConfig +{ +public: + void onInit() override; + bool isConfigured() const override; + + void loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node) override; + YAML::Node saveToYaml() const override; + + /** + * @brief Returns true if this or any of the included xacros have changes, requiring the URDF to be regenerated + */ + bool hasChanges() const; + + class GeneratedModifiedURDF : public TemplatedGeneratedFile + { + public: + GeneratedModifiedURDF(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + ModifiedUrdfConfig& parent) + : TemplatedGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + std::filesystem::path getRelativePath() const override + { + return std::filesystem::path("config") / (parent_.urdf_config_->getRobotName() + ".urdf.xacro"); + } + + std::filesystem::path getTemplatePath() const override + { + return getSharePath("moveit_setup_controllers") / "templates" / "config" / "modified.urdf.xacro"; + } + + std::string getDescription() const override + { + return "A modified version of the original URDF file with additional interfaces (e.g. ros2_control, simulation)"; + } + + bool hasChanges() const override + { + return parent_.hasChanges(); + } + + protected: + ModifiedUrdfConfig& parent_; + }; + + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) override; + void collectDependencies(std::set& packages) const override; + void collectVariables(std::vector& variables) override; + +protected: + std::unordered_map getIncludedXacroMap() const + { + return config_data_->getAll(); + } + + std::vector getIncludedXacros() const + { + std::vector xacros; + for (auto& pair : getIncludedXacroMap()) + { + if (pair.second->isConfigured()) + xacros.push_back(pair.second); + } + return xacros; + } + + std::vector getIncludedXacroNames() const + { + std::vector names; + for (auto& pair : getIncludedXacroMap()) + { + if (pair.second->isConfigured()) + names.push_back(pair.first); + } + + return names; + } + + std::shared_ptr urdf_config_; + std::set cached_xacro_names_; +}; +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp new file mode 100644 index 0000000000..6e9d24a945 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp @@ -0,0 +1,83 @@ +/********************************************************************* + * 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 +{ +namespace controllers +{ +class MoveItControllers : public Controllers +{ +public: + std::string getName() const override + { + return "MoveIt Controllers"; + } + + void onInit() override + { + config_data_->registerType("moveit_controllers", "moveit_setup::controllers::MoveItControllersConfig"); + srdf_config_ = config_data_->get("srdf"); + controllers_config_ = config_data_->get("moveit_controllers"); + } + + std::string getInstructions() const override + { + return "Configure controllers to be used in executing trajectories with MoveIt (hardware or simulation)." + "hardware"; + } + + std::string getButtonText() const override + { + return "Auto Add &FollowJointsTrajectory \n Controllers For Each Planning Group"; + } + + std::vector getAvailableTypes() const override + { + return { "FollowJointTrajectory", "GripperCommand" }; + } + + std::string getDefaultType() const override + { + return "FollowJointTrajectory"; + } +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.hpp new file mode 100644 index 0000000000..2d7055df2b --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.hpp @@ -0,0 +1,97 @@ +/********************************************************************* + * 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 + +namespace moveit_setup +{ +namespace controllers +{ +static const std::string MOVEIT_CONTROLLERS_YAML = "config/moveit_controllers.yaml"; + +class MoveItControllersConfig : public ControllersConfig +{ +public: + void loadPrevious(const std::filesystem::path& package_path, const YAML::Node& node) override; + + class GeneratedControllersConfig : public YamlGeneratedFile + { + public: + GeneratedControllersConfig(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + MoveItControllersConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + bool hasChanges() const override + { + return parent_.changed_ || parent_.hasChangedGroups(); + } + + std::filesystem::path getRelativePath() const override + { + return MOVEIT_CONTROLLERS_YAML; + } + + std::string getDescription() const override + { + return "Creates configurations for moveit_controllers."; + } + + bool writeYaml(YAML::Emitter& emitter) override; + + protected: + MoveItControllersConfig& parent_; + }; + + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) override + { + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + } + +protected: + /** + * Helper function for parsing ros_controllers.yaml file + * @param YAML::Node - individual controller to be parsed + * @return true if the file was read correctly + */ + bool parseController(const std::string& name, const YAML::Node& controller); +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.hpp new file mode 100644 index 0000000000..581ccb5a38 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.hpp @@ -0,0 +1,84 @@ +/********************************************************************* + * 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 +{ +namespace controllers +{ +class ROS2Controllers : public Controllers +{ +public: + std::string getName() const override + { + return "ROS 2 Controllers"; + } + + void onInit() override + { + config_data_->registerType("ros2_controllers", "moveit_setup::controllers::ROS2ControllersConfig"); + config_data_->registerType("control_xacro", "moveit_setup::controllers::ControlXacroConfig"); + srdf_config_ = config_data_->get("srdf"); + controllers_config_ = config_data_->get("ros2_controllers"); + } + + std::string getInstructions() const override + { + return "Configure ros2_controllers. By default, ros2_control fake_components are used to create a simple " + "simulation."; + } + + std::string getButtonText() const override + { + return "Auto Add &JointTrajectoryController \n Controllers For Each Planning Group"; + } + + std::vector getAvailableTypes() const override + { + return { "joint_trajectory_controller/JointTrajectoryController", "position_controllers/GripperActionController" }; + } + + std::string getDefaultType() const override + { + return "joint_trajectory_controller/JointTrajectoryController"; + } +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers_config.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers_config.hpp new file mode 100644 index 0000000000..dd961a2c8c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers_config.hpp @@ -0,0 +1,95 @@ +/********************************************************************* + * 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 +{ +namespace controllers +{ +static const std::string CONTROLLERS_YAML = "config/ros2_controllers.yaml"; +class ROS2ControllersConfig : public ControllersConfig +{ +public: + void onInit() override; + void loadPrevious(const std::filesystem::path& package_path, const YAML::Node& node) override; + + const ControlInterfaces getControlInterfaces(const std::vector& joint_names) const; + + class GeneratedControllersConfig : public YamlGeneratedFile + { + public: + GeneratedControllersConfig(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + ROS2ControllersConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + bool hasChanges() const override + { + return parent_.changed_ || parent_.hasChangedGroups(); + } + + std::filesystem::path getRelativePath() const override + { + return CONTROLLERS_YAML; + } + + std::string getDescription() const override + { + return "Creates configurations for ros2_controllers."; + } + + bool writeYaml(YAML::Emitter& emitter) override; + + protected: + ROS2ControllersConfig& parent_; + }; + + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) override + { + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + } + +protected: + std::shared_ptr control_xacro_config_; +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications.hpp new file mode 100644 index 0000000000..d3548967e3 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications.hpp @@ -0,0 +1,96 @@ +/********************************************************************* + * 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 +{ +namespace controllers +{ +class UrdfModifications : public SetupStep +{ +public: + std::string getName() const override + { + return "ros2_control URDF Modifications"; + } + + void onInit() override; + + bool isReady() const override + { + return !srdf_config_->getGroups().empty(); + } + + void refresh() + { + control_xacro_config_->loadFromDescription(); + } + + /** + * @brief Return true if there aren't ros2_control tags for all the joints + */ + bool needsModification() const; + + /** + * @brief Add ros2_control tags for all unconfigured joints with the specified interfaces + */ + void setInterfaces(const std::vector& command_interfaces, + const std::vector& state_interfaces); + + std::string getJointsXML() const + { + return control_xacro_config_->getJointsXML(); + } + + const ControlInterfaces& getDefaultControlInterfaces() const + { + return control_xacro_config_->getDefaultControlInterfaces(); + } + + const ControlInterfaces& getAvailableControlInterfaces() const + { + return control_xacro_config_->getAvailableControlInterfaces(); + } + +protected: + std::shared_ptr srdf_config_; + std::shared_ptr control_xacro_config_; +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications_widget.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications_widget.hpp new file mode 100644 index 0000000000..eee5f6c213 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications_widget.hpp @@ -0,0 +1,77 @@ +/********************************************************************* + * 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 +#include +#include + +namespace moveit_setup +{ +namespace controllers +{ +class UrdfModificationsWidget : public SetupStepWidget +{ + Q_OBJECT + +public: + void onInit() override; + void focusGiven() override; + + SetupStep& getSetupStep() override + { + return setup_step_; + } +private Q_SLOTS: + void addInterfaces(); + +private: + QWidget* makeInterfacesBox(const std::string& interface_type, const std::vector& available_interfaces, + const std::vector& selected_interfaces, QWidget* parent = nullptr); + std::vector getInterfaces(const char first_letter, const std::vector& available_interfaces); + QWidget* content_widget_; + QPushButton* btn_add_interfaces_; + QTextEdit* generated_text_widget_; + std::unordered_map check_boxes_; + + UrdfModifications setup_step_; +}; +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/launch/control.launch.py b/moveit_setup_assistant/moveit_setup_controllers/launch/control.launch.py new file mode 100644 index 0000000000..0c1cc64397 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/launch/control.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from moveit_configs_utils.launch_utils import ( + add_debuggable_node, + DeclareBooleanLaunchArg, +) + + +def generate_launch_description(): + ld = LaunchDescription() + + ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) + add_debuggable_node( + ld, + package="moveit_setup_assistant", + executable="moveit_setup_assistant", + parameters=[ + { + "setup_steps": [ + "moveit_setup::core::StartScreenWidget", + "moveit_setup::controllers::UrdfModificationsWidget", + "moveit_setup::controllers::ROS2ControllersWidget", + "moveit_setup::controllers::MoveItControllersWidget", + "moveit_setup::core::ConfigurationFilesWidget", + ] + }, + ], + ) + + return ld diff --git a/moveit_setup_assistant/moveit_setup_controllers/moveit_setup_framework_plugins.xml b/moveit_setup_assistant/moveit_setup_controllers/moveit_setup_framework_plugins.xml new file mode 100644 index 0000000000..45fd53c0d1 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/moveit_setup_framework_plugins.xml @@ -0,0 +1,23 @@ + + + A modified version of the original URDF file with additional interfaces (e.g. ros2_control, simulation) + + + Information about ros2_control tags that need to be added via xacro + + + Data for MoveIt Controllers + + + Data for ROS2 Controllers + + + Widget to modify the URDF for ros2_control interfaces + + + Configures the moveit_controllers + + + Configures the ros2_controllers + + diff --git a/moveit_setup_assistant/moveit_setup_controllers/package.xml b/moveit_setup_assistant/moveit_setup_controllers/package.xml new file mode 100644 index 0000000000..8495ec550e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/package.xml @@ -0,0 +1,26 @@ + + + + moveit_setup_controllers + 2.5.1 + MoveIt Setup Steps for ROS 2 Control + David V. Lu!! + BSD + David V. Lu!! + + ament_cmake + + ament_index_cpp + moveit_setup_framework + pluginlib + rclcpp + + ament_lint_auto + ament_clang_format + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/control_xacro_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/control_xacro_config.cpp new file mode 100644 index 0000000000..1caf7bf39c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/control_xacro_config.cpp @@ -0,0 +1,319 @@ +/********************************************************************* + * 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 +#include +#include +#include + +namespace moveit_setup +{ +namespace controllers +{ +void ControlXacroConfig::onInit() +{ + IncludedXacroConfig::onInit(); + available_ci_.command_interfaces = getAvailableInterfaceNames(); + available_ci_.state_interfaces = getAvailableInterfaceNames(); + default_ci_.command_interfaces = { "position" }; + default_ci_.state_interfaces = { "position", "velocity" }; +} + +void ControlXacroConfig::loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node) +{ + if (!node.IsDefined()) + { + return; + } + + getYamlProperty(node, "command", default_ci_.command_interfaces); + getYamlProperty(node, "state", default_ci_.state_interfaces); + setControlInterfaces(default_ci_); + changed_ = false; +} + +YAML::Node ControlXacroConfig::saveToYaml() const +{ + YAML::Node node; + node["command"] = default_ci_.command_interfaces; + node["state"] = default_ci_.state_interfaces; + return node; +} + +bool ControlXacroConfig::isConfigured() const +{ + return !new_joint_interfaces_.empty(); +} + +std::string ControlXacroConfig::getFilepath() const +{ + return urdf_config_->getRobotName() + ".ros2_control.xacro"; +} + +bool ControlXacroConfig::hasChanges() const +{ + return changed_; +} + +std::vector> ControlXacroConfig::getArguments() const +{ + std::vector> arguments; + arguments.push_back(std::make_pair("initial_positions_file", "initial_positions.yaml")); + return arguments; +} + +std::vector ControlXacroConfig::getCommands() const +{ + std::string command = "getRobotName(); + command += "_ros2_control name=\"FakeSystem\" initial_positions_file=\"$(arg initial_positions_file)\"/>"; + return { command }; +} + +void getInterfaceNames(const tinyxml2::XMLElement* joint_el, const std::string& element_name, + std::vector& interface_names) +{ + for (const tinyxml2::XMLElement* el = joint_el->FirstChildElement(element_name.c_str()); el != nullptr; + el = el->NextSiblingElement()) + { + interface_names.push_back(el->Attribute("name")); + } +} + +void ControlXacroConfig::loadFromDescription() +{ + // Reset Data + original_joint_interfaces_.clear(); + joint_names_.clear(); + + // Load the joint names for all joints used by the groups + auto srdf_config = config_data_->get("srdf"); + for (const std::string& group_name : srdf_config->getGroupNames()) + { + for (const std::string& joint_name : srdf_config->getJointNames(group_name, true, false)) // exclude passive + { + joint_names_.push_back(joint_name); + } + } + + // Read the URDF + tinyxml2::XMLDocument urdf_xml; + using tinyxml2::XMLElement; + + auto urdf_config = config_data_->get("urdf"); + urdf_xml.Parse(urdf_config->getURDFContents().c_str()); + for (XMLElement* control_el = urdf_xml.FirstChildElement("ros2_control"); control_el != nullptr; + control_el = control_el->NextSiblingElement()) + { + for (XMLElement* joint_el = control_el->FirstChildElement("joint"); joint_el != nullptr; + joint_el = joint_el->NextSiblingElement()) + { + std::string joint_name = joint_el->Attribute("name"); + + // Parse the interfaces + ControlInterfaces ci; + getInterfaceNames(joint_el, "command_interface", ci.command_interfaces); + getInterfaceNames(joint_el, "state_interface", ci.state_interfaces); + original_joint_interfaces_[joint_name] = ci; + } + } + setControlInterfaces(default_ci_); +} + +bool ControlXacroConfig::hasAllControlTagsInOriginal() const +{ + return joint_names_.size() == original_joint_interfaces_.size(); +} + +void ControlXacroConfig::setControlInterfaces(const ControlInterfaces& ci) +{ + default_ci_ = ci; + for (const std::string& joint_name : joint_names_) + { + if (original_joint_interfaces_.count(joint_name)) + { + continue; + } + + new_joint_interfaces_[joint_name] = default_ci_; + + // Setup the initial state + std::vector joint_value; + // TODO: There could be an option to load a group state from the RobotPoses + // For now, just assume one DOF, and it is zero + joint_value.push_back(0.0); + initial_group_state_.joint_values_[joint_name] = joint_value; + } + + changed_ = true; +} + +void uniqueMerge(std::vector& main, const std::vector& addition) +{ + for (const std::string& s : addition) + { + if (std::find(main.begin(), main.end(), s) == main.end()) + { + main.push_back(s); + } + } +} + +bool getControlInterfaceHelper(const std::unordered_map& interfaces, + const std::string& joint_name, ControlInterfaces& ci) +{ + const auto& it = interfaces.find(joint_name); + if (it == interfaces.end()) + { + return false; + } + const ControlInterfaces& found_interfaces = it->second; + uniqueMerge(ci.command_interfaces, found_interfaces.command_interfaces); + uniqueMerge(ci.state_interfaces, found_interfaces.state_interfaces); + return true; +} + +void ControlXacroConfig::getControlInterfaces(const std::string& joint_name, ControlInterfaces& ci) const +{ + if (getControlInterfaceHelper(original_joint_interfaces_, joint_name, ci)) + { + return; + } + getControlInterfaceHelper(new_joint_interfaces_, joint_name, ci); +} + +const ControlInterfaces ControlXacroConfig::getControlInterfaces(const std::vector& joint_names) const +{ + ControlInterfaces ci; + for (const std::string& joint_name : joint_names) + { + getControlInterfaces(joint_name, ci); + } + return ci; +} + +std::string ControlXacroConfig::getJointsXML() const +{ + std::string joints = ""; + const std::string tab = " "; + // Loop through all joints to preserve joint ordering + for (const std::string& joint_name : joint_names_) + { + auto pair = new_joint_interfaces_.find(joint_name); + if (pair == new_joint_interfaces_.end()) + { + continue; + } + + const ControlInterfaces& ci = pair->second; + + joints += tab; + joints += "\n"; + for (const std::string& command_interface : ci.command_interfaces) + { + joints += tab; + joints += " \n"; + } + for (const std::string& state_interface : ci.state_interfaces) + { + joints += tab; + joints += " \n"; + joints += tab; + joints += " ${initial_positions['"; + joints += joint_name; + joints += "']}\n"; + joints += tab; + joints += " \n"; + } + else + { + joints += "\"/>\n"; + } + } + joints += tab; + joints += "\n"; + } + return joints; +} +bool ControlXacroConfig::GeneratedInitialPositions::writeYaml(YAML::Emitter& emitter) +{ + emitter << YAML::Comment("Default initial positions for " + parent_.urdf_config_->getRobotName() + + "'s ros2_control fake system"); + emitter << YAML::Newline; + emitter << YAML::BeginMap; + { + emitter << YAML::Key << "initial_positions"; + emitter << YAML::Value; + emitter << YAML::BeginMap; + for (const auto& pair : parent_.initial_group_state_.joint_values_) + { + emitter << YAML::Key << pair.first; + + const std::vector& jv = pair.second; + + emitter << YAML::Value; + if (jv.size() == 1) + { + emitter << jv[0]; + } + else + { + emitter << jv; + } + } + emitter << YAML::EndMap; + } + emitter << YAML::EndMap; + + return true; +} + +void ControlXacroConfig::collectVariables(std::vector& variables) +{ + variables.push_back(TemplateVariable("ROS2_CONTROL_JOINTS", getJointsXML())); +} + +} // namespace controllers +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::controllers::ControlXacroConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/controller_edit_widget.cpp similarity index 89% rename from moveit_setup_assistant/src/widgets/controller_edit_widget.cpp rename to moveit_setup_assistant/moveit_setup_controllers/src/controller_edit_widget.cpp index fe7894064f..e2c66fa01b 100644 --- a/moveit_setup_assistant/src/widgets/controller_edit_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/controller_edit_widget.cpp @@ -43,15 +43,16 @@ #include #include #include -#include "controller_edit_widget.h" +#include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace controllers { // ****************************************************************************************** // ControllerEditWidget constructor, create controller edit screen GUI // ****************************************************************************************** -ControllerEditWidget::ControllerEditWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : QWidget(parent), config_data_(config_data) +ControllerEditWidget::ControllerEditWidget(QWidget* parent) : QWidget(parent) { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -158,10 +159,9 @@ ControllerEditWidget::ControllerEditWidget(QWidget* parent, const MoveItConfigDa // ****************************************************************************************** // Set the fields with previous values // ****************************************************************************************** -void ControllerEditWidget::setSelected(const std::string& controller_name) +void ControllerEditWidget::setSelected(const std::string& controller_name, const ControllerInfo* searched_controller) { controller_name_field_->setText(QString(controller_name.c_str())); - moveit_setup_assistant::ControllerConfig* searched_controller = config_data_->findControllerByName(controller_name); if (searched_controller != nullptr) { const std::string controller_type = searched_controller->type_; @@ -187,23 +187,18 @@ void ControllerEditWidget::setSelected(const std::string& controller_name) // ****************************************************************************************** // Populate the combo dropdown box with controllers types // ****************************************************************************************** -void ControllerEditWidget::loadControllersTypesComboBox() +void ControllerEditWidget::loadControllersTypesComboBox(const std::vector& controller_types) { // Only load this combo box once if (has_loaded_) return; has_loaded_ = true; - const std::vector default_types = { "effort_controllers/JointTrajectoryController", - "velocity_controllers/JointTrajectoryController", - "position_controllers/JointTrajectoryController", - "FollowJointTrajectory", "GripperCommand" }; - // Remove all old items controller_type_field_->clear(); - // Loop through all controller default_types and add to combo box - for (const std::string& type : default_types) + // Loop through all controller types and add to combo box + for (const std::string& type : controller_types) controller_type_field_->addItem(type.c_str()); } @@ -252,4 +247,5 @@ std::string ControllerEditWidget::getControllerType() return controller_type_field_->currentText().toStdString(); } -} // namespace moveit_setup_assistant +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/controllers.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/controllers.cpp new file mode 100644 index 0000000000..a7737b6147 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/controllers.cpp @@ -0,0 +1,85 @@ +/********************************************************************* + * 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 +{ +namespace controllers +{ +// ****************************************************************************************** +// Add a Follow Joint Trajectory action Controller for each Planning Group +// ****************************************************************************************** +bool Controllers::addDefaultControllers() +{ + std::vector group_names = getGroupNames(); + if (group_names.empty()) + { + return false; + } + + // Loop through groups + bool success = true; + for (const std::string& group_name : group_names) + { + // Get list of associated joints + std::vector joint_names = srdf_config_->getJointNames(group_name, true, false); // exclude passive + if (joint_names.empty()) + { + continue; + } + bool ret = controllers_config_->addController(group_name + "_controller", getDefaultType(), joint_names); + success &= ret; + } + + return success; +} + +std::vector Controllers::getJointsFromGroups(const std::vector& group_names) const +{ + std::vector joint_names; + for (const std::string& group_name : group_names) + { + for (const std::string& joint_name : srdf_config_->getJointNames(group_name, true, false)) // exclude passive + { + joint_names.push_back(joint_name); + } + } + return joint_names; +} + +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_config.cpp new file mode 100644 index 0000000000..25962cf130 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_config.cpp @@ -0,0 +1,102 @@ +/********************************************************************* + * 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 +#include +#include + +namespace moveit_setup +{ +namespace controllers +{ +bool ControllersConfig::addController(const std::string& name, const std::string& type, + const std::vector& joint_names) +{ + ControllerInfo controller; + controller.name_ = name; + controller.type_ = type; + controller.joints_ = joint_names; + return addController(controller); +} + +bool ControllersConfig::addController(const ControllerInfo& new_controller) +{ + // Used for holding our search results + ControllerInfo* searched_ros_controller = nullptr; + + // Find if there is an existing controller with the same name + searched_ros_controller = findControllerByName(new_controller.name_); + + if (searched_ros_controller && searched_ros_controller->type_ == new_controller.type_) + return false; + + controllers_.push_back(new_controller); + return true; +} + +ControllerInfo* ControllersConfig::findControllerByName(const std::string& controller_name) +{ + // Find the Controller we are editing based on the Controller name string + ControllerInfo* searched_ros_controller = nullptr; // used for holding our search results + + for (ControllerInfo& ros_control_config : controllers_) + { + if (ros_control_config.name_ == controller_name) // string match + { + searched_ros_controller = &ros_control_config; // convert to pointer from iterator + break; // we are done searching + } + } + + return searched_ros_controller; +} + +bool ControllersConfig::deleteController(const std::string& controller_name) +{ + for (std::vector::iterator controller_it = controllers_.begin(); controller_it != controllers_.end(); + ++controller_it) + { + if (controller_it->name_ == controller_name) // string match + { + controllers_.erase(controller_it); + // we are done searching + return true; + } + } + return false; +} +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/controllers_widget.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp similarity index 83% rename from moveit_setup_assistant/src/widgets/controllers_widget.cpp rename to moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp index e61efb9d33..4e995a47ce 100644 --- a/moveit_setup_assistant/src/widgets/controllers_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp @@ -34,10 +34,8 @@ /* Author: Mohamad Ayman */ // SA -#include "controllers_widget.h" -#include "double_list_widget.h" -#include "controller_edit_widget.h" -#include "header_widget.h" +#include +#include #include // Qt #include @@ -60,13 +58,14 @@ #include #include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace controllers { // ****************************************************************************************** // Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** -ControllersWidget::ControllersWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +void ControllersWidget::onInit() { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -77,32 +76,28 @@ ControllersWidget::ControllersWidget(QWidget* parent, const MoveItConfigDataPtr& this->setWindowTitle("Controller Configuration"); // title of window // Top Header Area ------------------------------------------------ - moveit_setup_assistant::HeaderWidget* header = new moveit_setup_assistant::HeaderWidget( - "Setup Controllers", - "Configure controllers to be used by MoveIt's controller manager(s) to operate the robot's physical hardware", - this); + auto header = new HeaderWidget("Setup " + setup_step_->getName(), setup_step_->getInstructions(), this); layout->addWidget(header); // Tree Box ---------------------------------------------------------------------- controllers_tree_widget_ = createContentsWidget(); // Joints edit widget - joints_widget_ = new moveit_setup_assistant::DoubleListWidget(this, config_data_, "Joint Collection", "Joint"); + joints_widget_ = new 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))); // Joints Groups Widget - joint_groups_widget_ = - new moveit_setup_assistant::DoubleListWidget(this, config_data_, "Group Joints Collection", "Group"); + joint_groups_widget_ = new DoubleListWidget(this, "Group Joints Collection", "Group"); connect(joint_groups_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing())); connect(joint_groups_widget_, SIGNAL(doneEditing()), this, SLOT(saveJointsGroupsScreen())); connect(joint_groups_widget_, SIGNAL(previewSelected(std::vector)), this, SLOT(previewSelectedGroup(std::vector))); // Controller Edit Widget - controller_edit_widget_ = new ControllerEditWidget(this, config_data_); + controller_edit_widget_ = new ControllerEditWidget(this); connect(controller_edit_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing())); connect(controller_edit_widget_, SIGNAL(deleteController()), this, SLOT(deleteController())); connect(controller_edit_widget_, SIGNAL(save()), this, SLOT(saveControllerScreenEdit())); @@ -134,8 +129,7 @@ QWidget* ControllersWidget::createContentsWidget() QHBoxLayout* upper_controls_layout = new QHBoxLayout(); // Add default controller - QPushButton* btn_add_default = - new QPushButton("Auto Add &FollowJointsTrajectory \n Controllers For Each Planning Group", this); + QPushButton* btn_add_default = new QPushButton(setup_step_->getButtonText().c_str(), this); btn_add_default->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); btn_add_default->setMaximumWidth(600); connect(btn_add_default, SIGNAL(clicked()), this, SLOT(addDefaultControllers())); @@ -214,7 +208,7 @@ void ControllersWidget::loadControllersTree() controllers_tree_->clear(); // reset the tree // Display all controllers by looping through them - for (ControllerConfig& controller : config_data_->getControllers()) + for (ControllerInfo& controller : setup_step_->getControllers()) { loadToControllersTree(controller); } @@ -229,7 +223,7 @@ void ControllersWidget::loadControllersTree() // ****************************************************************************************** // Displays data in the controller_config_ data structure into a QtTableWidget // ****************************************************************************************** -void ControllersWidget::loadToControllersTree(const moveit_setup_assistant::ControllerConfig& controller_it) +void ControllersWidget::loadToControllersTree(const ControllerInfo& controller_it) { // Fonts for tree const QFont top_level_font(QFont().defaultFamily(), 11, QFont::Bold); @@ -286,13 +280,10 @@ void ControllersWidget::focusGiven() // ****************************************************************************************** // Load the popup screen with correct data for joints // ****************************************************************************************** -void ControllersWidget::loadJointsScreen(moveit_setup_assistant::ControllerConfig* this_controller) +void ControllersWidget::loadJointsScreen(ControllerInfo* this_controller) { - // 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()) { @@ -317,20 +308,10 @@ void ControllersWidget::loadJointsScreen(moveit_setup_assistant::ControllerConfi // ****************************************************************************************** // Load the popup screen with correct data for gcroups // ****************************************************************************************** -void ControllersWidget::loadGroupsScreen(moveit_setup_assistant::ControllerConfig* this_controller) +void ControllersWidget::loadGroupsScreen(ControllerInfo* this_controller) { - // Load all groups into the subgroup screen - std::vector groups; - - // Display all groups by looping through them - for (const srdf::Model::Group& group : config_data_->srdf_->srdf_model_->getGroups()) - { - // Add to available groups list - groups.push_back(group.name_); - } - // Set the available groups (left box) - joint_groups_widget_->setAvailable(groups); + joint_groups_widget_->setAvailable(setup_step_->getGroupNames()); // Set the selected groups (right box) joint_groups_widget_->setSelected(this_controller->joints_); @@ -372,13 +353,13 @@ void ControllersWidget::deleteController() return; } // Delete actual controller - if (config_data_->deleteController(controller_name)) + if (setup_step_->deleteController(controller_name)) { - ROS_INFO_STREAM_NAMED("Setup Assistant", "Controller " << controller_name << " deleted succefully"); + RCLCPP_INFO_STREAM(setup_step_->getLogger(), "Controller " << controller_name << " deleted succefully"); } else { - ROS_WARN_STREAM_NAMED("Setup Assistant", "Couldn't delete Controller " << controller_name); + RCLCPP_WARN_STREAM(setup_step_->getLogger(), "Couldn't delete Controller " << controller_name); } current_edit_controller_.clear(); @@ -408,7 +389,7 @@ void ControllersWidget::addController() // ****************************************************************************************** void ControllersWidget::addDefaultControllers() { - if (!config_data_->addDefaultControllers()) + if (!setup_step_->addDefaultControllers()) QMessageBox::warning(this, "Error adding controllers", "No Planning Groups configured!"); loadControllersTree(); } @@ -416,10 +397,10 @@ void ControllersWidget::addDefaultControllers() // ****************************************************************************************** // Load the popup screen with correct data for controllers // ****************************************************************************************** -void ControllersWidget::loadControllerScreen(moveit_setup_assistant::ControllerConfig* this_controller) +void ControllersWidget::loadControllerScreen(ControllerInfo* this_controller) { // Load the avail controllers. This function only runs once - controller_edit_widget_->loadControllersTypesComboBox(); + controller_edit_widget_->loadControllersTypesComboBox(setup_step_->getAvailableTypes()); if (this_controller == nullptr) // this is a new screen { @@ -439,7 +420,8 @@ void ControllersWidget::loadControllerScreen(moveit_setup_assistant::ControllerC } // Set the data in the edit box - controller_edit_widget_->setSelected(current_edit_controller_); + ControllerInfo* searched_controller = setup_step_->findControllerByName(current_edit_controller_); + controller_edit_widget_->setSelected(current_edit_controller_, searched_controller); } // ****************************************************************************************** @@ -449,10 +431,10 @@ void ControllersWidget::cancelEditing() { if (!current_edit_controller_.empty() && adding_new_controller_) { - moveit_setup_assistant::ControllerConfig* editing = config_data_->findControllerByName(current_edit_controller_); + ControllerInfo* editing = setup_step_->findControllerByName(current_edit_controller_); if (editing && editing->joints_.empty()) { - config_data_->deleteController(current_edit_controller_); + setup_step_->deleteController(current_edit_controller_); current_edit_controller_.clear(); // Load the data to the tree @@ -474,20 +456,12 @@ void ControllersWidget::cancelEditing() void ControllersWidget::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()) { @@ -495,7 +469,7 @@ void ControllersWidget::previewSelectedJoints(const std::vector& jo } // Highlight link - Q_EMIT highlightLink(link, QColor(255, 0, 0)); + rviz_panel_->highlightLink(link, QColor(255, 0, 0)); } } @@ -505,12 +479,12 @@ void ControllersWidget::previewSelectedJoints(const std::vector& jo void ControllersWidget::previewSelectedGroup(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); } } @@ -544,8 +518,7 @@ void ControllersWidget::saveControllerScreenJoints() return; // Find the controller we are editing based on the controller name string - moveit_setup_assistant::ControllerConfig* editing_controller = - config_data_->findControllerByName(current_edit_controller_); + ControllerInfo* editing_controller = setup_step_->findControllerByName(current_edit_controller_); loadJointsScreen(editing_controller); @@ -563,8 +536,7 @@ void ControllersWidget::saveControllerScreenGroups() return; // Find the controller we are editing based on the controller name string - moveit_setup_assistant::ControllerConfig* editing_controller = - config_data_->findControllerByName(current_edit_controller_); + ControllerInfo* editing_controller = setup_step_->findControllerByName(current_edit_controller_); loadGroupsScreen(editing_controller); @@ -578,8 +550,7 @@ void ControllersWidget::saveControllerScreenGroups() void ControllersWidget::saveJointsScreen() { // Find the controller we are editing based on the controller name string - moveit_setup_assistant::ControllerConfig* searched_controller = - config_data_->findControllerByName(current_edit_controller_); + ControllerInfo* searched_controller = setup_step_->findControllerByName(current_edit_controller_); // Clear the old data searched_controller->joints_.clear(); @@ -603,28 +574,10 @@ void ControllersWidget::saveJointsScreen() void ControllersWidget::saveJointsGroupsScreen() { // Find the controller we are editing based on the controller name string - moveit_setup_assistant::ControllerConfig* searched_controller = - config_data_->findControllerByName(current_edit_controller_); + ControllerInfo* searched_controller = setup_step_->findControllerByName(current_edit_controller_); // Clear the old data - searched_controller->joints_.clear(); - - // Copy the data - for (int i = 0; i < joint_groups_widget_->selected_data_table_->rowCount(); ++i) - { - // Get list of associated joints - const moveit::core::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup( - joint_groups_widget_->selected_data_table_->item(i, 0)->text().toStdString()); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); - - // Iterate through the joints - for (const moveit::core::JointModel* joint : joint_models) - { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) - continue; - searched_controller->joints_.push_back(joint->getName()); - } - } + searched_controller->joints_ = setup_step_->getJointsFromGroups(joint_groups_widget_->getSelectedValues()); // Switch to main screen showMainScreen(); @@ -656,7 +609,7 @@ bool ControllersWidget::saveControllerScreen() const std::string& controller_type = controller_edit_widget_->getControllerType(); // Used for editing existing controllers - moveit_setup_assistant::ControllerConfig* searched_controller = nullptr; + ControllerInfo* searched_controller = nullptr; std::smatch invalid_name_match; std::regex invalid_reg_ex("[^a-z|^1-9|^_]"); @@ -671,12 +624,12 @@ bool ControllersWidget::saveControllerScreen() // Check if this is an existing controller if (!current_edit_controller_.empty()) { - // Find the controller we are editing based on the goup name string - searched_controller = config_data_->findControllerByName(current_edit_controller_); + // Find the controller we are editing based on the group name string + searched_controller = setup_step_->findControllerByName(current_edit_controller_); } // Check that the controller name is unique - for (const auto& controller : config_data_->getControllers()) + for (const auto& controller : setup_step_->getControllers()) { if (controller.name_.compare(controller_name) == 0) // the names are the same { @@ -694,10 +647,10 @@ bool ControllersWidget::saveControllerScreen() // Save the new controller name or create the new controller if (searched_controller == nullptr) // create new { - moveit_setup_assistant::ControllerConfig new_controller; + ControllerInfo new_controller; new_controller.name_ = controller_name; new_controller.type_ = controller_type; - config_data_->addController(new_controller); + setup_step_->addController(new_controller); adding_new_controller_ = true; // remember this controller is new } @@ -742,8 +695,7 @@ void ControllersWidget::editSelected() // The controller this joint belong to controller_item = item->parent()->parent(); current_edit_controller_ = controller_item->text(0).toUtf8().constData(); - moveit_setup_assistant::ControllerConfig* this_controller = - config_data_->findControllerByName(current_edit_controller_); + ControllerInfo* this_controller = setup_step_->findControllerByName(current_edit_controller_); // Load the data loadJointsScreen(this_controller); @@ -755,8 +707,7 @@ void ControllersWidget::editSelected() { controller_item = item->parent(); current_edit_controller_ = controller_item->text(0).toUtf8().constData(); - moveit_setup_assistant::ControllerConfig* this_controller = - config_data_->findControllerByName(current_edit_controller_); + ControllerInfo* this_controller = setup_step_->findControllerByName(current_edit_controller_); // Load the data loadJointsScreen(this_controller); @@ -768,8 +719,7 @@ void ControllersWidget::editSelected() { // Load the data current_edit_controller_ = item->text(0).toUtf8().constData(); - moveit_setup_assistant::ControllerConfig* this_controller = - config_data_->findControllerByName(current_edit_controller_); + ControllerInfo* this_controller = setup_step_->findControllerByName(current_edit_controller_); loadControllerScreen(this_controller); // Switch to screen @@ -794,7 +744,7 @@ void ControllersWidget::editController() adding_new_controller_ = false; - loadControllerScreen(config_data_->findControllerByName(current_edit_controller_)); + loadControllerScreen(setup_step_->findControllerByName(current_edit_controller_)); // Switch to screen changeScreen(2); // 1 is index of controller edit @@ -808,7 +758,7 @@ void ControllersWidget::showMainScreen() stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode - Q_EMIT isModal(false); + Q_EMIT setModalMode(false); } // ****************************************************************************************** @@ -819,7 +769,7 @@ void ControllersWidget::changeScreen(int index) stacked_widget_->setCurrentIndex(index); // Announce this widget's mode - Q_EMIT isModal(index != 0); + Q_EMIT setModalMode(index != 0); } // ****************************************************************************************** @@ -843,4 +793,5 @@ void ControllersWidget::itemSelectionChanged() } } -} // namespace moveit_setup_assistant +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/modified_urdf_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/modified_urdf_config.cpp new file mode 100644 index 0000000000..bd2dd06cd1 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/modified_urdf_config.cpp @@ -0,0 +1,144 @@ +/********************************************************************* + * 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 +{ +void ModifiedUrdfConfig::onInit() +{ + urdf_config_ = config_data_->get("urdf"); +} + +bool ModifiedUrdfConfig::isConfigured() const +{ + return !getIncludedXacros().empty(); +} + +void ModifiedUrdfConfig::loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node) +{ + std::vector xacro_names_vector; + if (node.IsDefined()) + getYamlProperty(node, "xacros", xacro_names_vector); + cached_xacro_names_ = std::set(xacro_names_vector.begin(), xacro_names_vector.end()); +} + +YAML::Node ModifiedUrdfConfig::saveToYaml() const +{ + YAML::Node node; + node["xacros"] = getIncludedXacroNames(); + return node; +} + +bool ModifiedUrdfConfig::hasChanges() const +{ + // Returns true if any of the included xacros are configured and have individually changed + // or the list of included xacros have changed. + unsigned int count = 0; + for (auto& pair : getIncludedXacroMap()) + { + if (!pair.second->isConfigured()) + { + continue; + } + if (pair.second->hasChanges() || cached_xacro_names_.count(pair.first) == 0) + { + return true; + } + count++; + } + + return count != cached_xacro_names_.size(); +} + +void ModifiedUrdfConfig::collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) +{ + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + + auto xacro_names = getIncludedXacroNames(); + cached_xacro_names_ = std::set(xacro_names.begin(), xacro_names.end()); +} + +void ModifiedUrdfConfig::collectDependencies(std::set& packages) const +{ + packages.insert("xacro"); +} + +void ModifiedUrdfConfig::collectVariables(std::vector& variables) +{ + std::string args = "", imports = "", commands = ""; + + for (const auto& pair : getIncludedXacroMap()) + { + const IncludedXacroConfig::Ptr& xacro = pair.second; + if (!xacro->isConfigured()) + { + continue; + } + for (const std::pair& argument : xacro->getArguments()) + { + args += " \n"; + } + + imports += " \n"; + + imports += " getFilepath(); + imports += "\" />\n\n"; + + for (const std::string& command : xacro->getCommands()) + { + commands += " "; + commands += command; + commands += "\n"; + } + } + + variables.push_back(TemplateVariable("MODIFIED_XACRO_ARGS", args)); + variables.push_back(TemplateVariable("MODIFIED_XACRO_IMPORTS", imports)); + variables.push_back(TemplateVariable("MODIFIED_XACRO_COMMANDS", commands)); +} +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::ModifiedUrdfConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_config.cpp new file mode 100644 index 0000000000..cafb83f15b --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_config.cpp @@ -0,0 +1,229 @@ +/********************************************************************* + * 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 +#include + +namespace moveit_setup +{ +namespace controllers +{ +void MoveItControllersConfig::loadPrevious(const std::filesystem::path& package_path, const YAML::Node& /*node*/) +{ + changed_ = false; + controllers_.clear(); + + // Load moveit controllers yaml file if available----------------------------------------------- + std::filesystem::path ros_controllers_yaml_path = package_path / MOVEIT_CONTROLLERS_YAML; + std::ifstream input_stream(ros_controllers_yaml_path); + if (!input_stream.good()) + { + RCLCPP_WARN_STREAM(*logger_, "Does not exist " << ros_controllers_yaml_path); + return; + } + + // Begin parsing + try + { + // Used in parsing controllers + ControllerInfo control_setting; + YAML::Node controllers = YAML::Load(input_stream)["moveit_simple_controller_manager"]; + + std::vector controller_names; + getYamlProperty(controllers, "controller_names", controller_names); + + // Loop through all controllers + for (const std::string& controller_name : controller_names) + { + const YAML::Node& cnode = controllers[controller_name]; + if (!cnode.IsDefined()) + { + RCLCPP_WARN_STREAM(*logger_, "Configuration for controller " << controller_name << " does not exist! Ignoring."); + continue; + } + + if (!parseController(controller_name, cnode)) + { + return; + } + } + } + catch (YAML::ParserException& e) // Catch errors + { + RCLCPP_ERROR_STREAM(*logger_, e.what()); + } +} + +// ****************************************************************************************** +// Helper function for parsing an individual Controller from moveit_controllers yaml file +// ****************************************************************************************** +bool MoveItControllersConfig::parseController(const std::string& name, const YAML::Node& controller_node) +{ + // Used in parsing controllers + ControllerInfo control_setting; + control_setting.name_ = name; + getYamlProperty(controller_node, "type", control_setting.type_); + if (control_setting.type_.empty()) + { + RCLCPP_ERROR_STREAM(*logger_, "Couldn't parse type for controller " << name << " in moveit_controllers.yaml"); + return false; + } + + const YAML::Node& joints_node = controller_node["joints"]; + + if (joints_node.IsSequence()) + { + control_setting.joints_ = joints_node.as>(); + } + else if (joints_node.IsDefined()) + { + control_setting.joints_.push_back(joints_node.as()); + } + if (control_setting.joints_.empty()) + { + RCLCPP_ERROR_STREAM(*logger_, "Couldn't parse joints for controller " << name << " in moveit_controllers.yaml"); + return false; + } + // All required fields were parsed correctly + controllers_.push_back(control_setting); + return true; +} + +// ****************************************************************************************** +// Output controllers config files +// ****************************************************************************************** +bool MoveItControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitter& emitter) +{ + emitter << YAML::Comment("MoveIt uses this configuration for controller management"); + emitter << YAML::Newline; + emitter << YAML::BeginMap; + { + // TODO: Output possible trajectory_execution parameters including... + // * allowed_execution_duration_scaling: 1.2 + // * allowed_goal_duration_margin: 0.5 + // * allowed_start_tolerance: 0.01 + + emitter << YAML::Key << "moveit_controller_manager" << YAML::Value + << "moveit_simple_controller_manager/MoveItSimpleControllerManager"; + emitter << YAML::Newline; + emitter << YAML::Newline; + + emitter << YAML::Key << "moveit_simple_controller_manager" << YAML::Value; + emitter << YAML::BeginMap; + { + emitter << YAML::Key << "controller_names"; + emitter << YAML::Value; + emitter << YAML::BeginSeq; + for (const ControllerInfo& ci : parent_.controllers_) + { + emitter << ci.name_; + } + emitter << YAML::EndSeq; + emitter << YAML::Newline; + emitter << YAML::Newline; + + for (const auto& controller : parent_.controllers_) + { + emitter << YAML::Key << controller.name_; + emitter << YAML::Value; + emitter << YAML::BeginMap; + { + emitter << YAML::Key << "type" << YAML::Value << controller.type_; + // Write joints + emitter << YAML::Key << "joints"; + emitter << YAML::Value; + if (controller.joints_.size() != 1) + { + emitter << YAML::BeginSeq; + + // Iterate through the joints + for (const std::string& joint : controller.joints_) + { + emitter << joint; + } + emitter << YAML::EndSeq; + } + else + { + emitter << YAML::BeginMap; + emitter << controller.joints_[0]; + emitter << YAML::EndMap; + } + // Depending on the controller type, fill the required data + if (controller.type_ == "FollowJointTrajectory") + { + emitter << YAML::Key << "action_ns" << YAML::Value << "follow_joint_trajectory"; + emitter << YAML::Key << "default" << YAML::Value << "true"; + } + else + { + // Write gains as they are required for vel and effort controllers + emitter << YAML::Key << "gains"; + emitter << YAML::Value; + emitter << YAML::BeginMap; + { + // Iterate through the joints + for (const std::string& joint : controller.joints_) + { + emitter << YAML::Key << joint << YAML::Value << YAML::BeginMap; + emitter << YAML::Key << "p"; + emitter << YAML::Value << "100"; + emitter << YAML::Key << "d"; + emitter << YAML::Value << "1"; + emitter << YAML::Key << "i"; + emitter << YAML::Value << "1"; + emitter << YAML::Key << "i_clamp"; + emitter << YAML::Value << "1" << YAML::EndMap; + } + } + emitter << YAML::EndMap; + } + emitter << YAML::EndMap; + } + } + } + emitter << YAML::EndMap; + } + emitter << YAML::EndMap; + + return true; +} + +} // namespace controllers +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::controllers::MoveItControllersConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_widget.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_widget.cpp new file mode 100644 index 0000000000..f1da70daa8 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_widget.cpp @@ -0,0 +1,57 @@ +/********************************************************************* + * 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 +#include + +namespace moveit_setup +{ +namespace controllers +{ +class MoveItControllersWidget : public ControllersWidget +{ +public: + MoveItControllersWidget() : ControllersWidget() + { + setup_step_ = std::make_shared(); + } +}; + +} // namespace controllers +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::controllers::MoveItControllersWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp new file mode 100644 index 0000000000..3ce21bffe6 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp @@ -0,0 +1,224 @@ +/********************************************************************* + * 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 +{ +namespace controllers +{ +void ROS2ControllersConfig::onInit() +{ + control_xacro_config_ = config_data_->get("control_xacro"); +} + +void ROS2ControllersConfig::loadPrevious(const std::filesystem::path& package_path, const YAML::Node& /*node*/) +{ + changed_ = false; + controllers_.clear(); + + // Load ros2 controllers yaml file if available----------------------------------------------- + std::filesystem::path ros_controllers_yaml_path = package_path / CONTROLLERS_YAML; + std::ifstream input_stream(ros_controllers_yaml_path); + if (!input_stream.good()) + { + RCLCPP_WARN_STREAM(*logger_, "Does not exist " << ros_controllers_yaml_path); + return; + } + + // Begin parsing + try + { + YAML::Node config = YAML::Load(input_stream); + + // Parse Controller Manager Config + const YAML::Node& cm_node = config["controller_manager"]["ros__parameters"]; + if (!cm_node.IsDefined()) + { + return; + } + for (const auto& it : cm_node) + { + if (!it.second.IsMap()) + continue; + + const auto& type_node = it.second["type"]; + if (!type_node.IsDefined()) + { + continue; + } + + ControllerInfo controller; + controller.name_ = it.first.as(); + controller.type_ = type_node.as(); + + if (controller.type_ != "joint_state_broadcaster/JointStateBroadcaster") + { + controllers_.push_back(controller); + } + } + + // ParseIndividual Controller Configs + for (auto& controller : controllers_) + { + const YAML::Node& controller_node = config[controller.name_]["ros__parameters"]; + if (!controller_node.IsDefined() || !controller_node.IsMap()) + { + continue; + } + auto jnode = controller_node["joints"]; + if (jnode.IsDefined() && jnode.IsSequence()) + { + // Looping through sequences with yaml is sometimes buggy when using iterators + // so here we use a "classic" loop and disable the clang-tidy rule + for (std::size_t i = 0; i < jnode.size(); i++) // NOLINT(modernize-loop-convert) + { + controller.joints_.push_back(jnode[i].as()); + } + } + + if (controller.joints_.empty()) + { + std::string one_joint; + getYamlProperty(controller_node, "joint", one_joint); + if (!one_joint.empty()) + { + controller.joints_.push_back(one_joint); + } + } + } + } + catch (YAML::ParserException& e) // Catch errors + { + RCLCPP_ERROR_STREAM(*logger_, e.what()); + } +} + +const ControlInterfaces ROS2ControllersConfig::getControlInterfaces(const std::vector& joint_names) const +{ + return control_xacro_config_->getControlInterfaces(joint_names); +} + +// ****************************************************************************************** +// Output controllers config files +// ****************************************************************************************** +bool ROS2ControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitter& emitter) +{ + emitter << YAML::Comment("This config file is used by ros2_control"); + emitter << YAML::BeginMap; + { + // Output controller Manager Config + emitter << YAML::Key << "controller_manager"; + emitter << YAML::Value; + emitter << YAML::BeginMap; + { + emitter << YAML::Key << "ros__parameters"; + emitter << YAML::Value; + emitter << YAML::BeginMap; + { + emitter << YAML::Key << "update_rate" << YAML::Value << 100; // Hz + emitter << YAML::Comment("Hz"); + emitter << YAML::Newline; + emitter << YAML::Newline; + + // Output Types + for (const ControllerInfo& ci : parent_.controllers_) + { + emitter << YAML::Key << ci.name_; + emitter << YAML::Value; + emitter << YAML::BeginMap; + { + emitter << YAML::Key << "type" << YAML::Value << ci.type_; + } + emitter << YAML::Newline << YAML::Newline; + emitter << YAML::EndMap; + } + + emitter << YAML::Key << "joint_state_broadcaster"; + emitter << YAML::Value; + emitter << YAML::BeginMap; + { + emitter << YAML::Key << "type" << YAML::Value << "joint_state_broadcaster/JointStateBroadcaster"; + } + emitter << YAML::EndMap; + } + emitter << YAML::EndMap; + } + emitter << YAML::EndMap; + emitter << YAML::Newline; + emitter << YAML::Newline; + } + // Output Controller Configs + for (const ControllerInfo& ci : parent_.controllers_) + { + emitter << YAML::Key << ci.name_; + emitter << YAML::Value; + emitter << YAML::BeginMap; + { + emitter << YAML::Key << "ros__parameters"; + emitter << YAML::Value; + emitter << YAML::BeginMap; + { + if (ci.joints_.size() != 1) + { + emitter << YAML::Key << "joints" << YAML::Value << ci.joints_; + } + else + { + emitter << YAML::Key << "joint" << YAML::Value << ci.joints_[0]; + } + + if (ci.type_ == "joint_trajectory_controller/JointTrajectoryController") + { + const ControlInterfaces interfaces = parent_.getControlInterfaces(ci.joints_); + emitter << YAML::Key << "command_interfaces" << YAML::Value << interfaces.command_interfaces; + emitter << YAML::Key << "state_interfaces" << YAML::Value << interfaces.state_interfaces; + } + } + emitter << YAML::EndMap; + } + emitter << YAML::EndMap; + } + emitter << YAML::EndMap; + + return true; +} + +} // namespace controllers +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::controllers::ROS2ControllersConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_widget.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_widget.cpp new file mode 100644 index 0000000000..509273696d --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_widget.cpp @@ -0,0 +1,57 @@ +/********************************************************************* + * 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 +#include + +namespace moveit_setup +{ +namespace controllers +{ +class ROS2ControllersWidget : public ControllersWidget +{ +public: + ROS2ControllersWidget() : ControllersWidget() + { + setup_step_ = std::make_shared(); + } +}; + +} // namespace controllers +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::controllers::ROS2ControllersWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/urdf_modifications.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/urdf_modifications.cpp new file mode 100644 index 0000000000..38f13b4df1 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/urdf_modifications.cpp @@ -0,0 +1,65 @@ +/********************************************************************* + * 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 +{ +namespace controllers +{ +void UrdfModifications::onInit() +{ + config_data_->registerType("control_xacro", "moveit_setup::controllers::ControlXacroConfig"); + control_xacro_config_ = config_data_->get("control_xacro"); + srdf_config_ = config_data_->get("srdf"); +} + +void UrdfModifications::setInterfaces(const std::vector& command_interfaces, + const std::vector& state_interfaces) +{ + ControlInterfaces ci; + ci.command_interfaces = command_interfaces; + ci.state_interfaces = state_interfaces; + control_xacro_config_->setControlInterfaces(ci); +} + +bool UrdfModifications::needsModification() const +{ + return !control_xacro_config_->hasAllControlTagsInOriginal(); +} + +} // namespace controllers +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/urdf_modifications_widget.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/urdf_modifications_widget.cpp new file mode 100644 index 0000000000..3be22f15f2 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/src/urdf_modifications_widget.cpp @@ -0,0 +1,157 @@ +/********************************************************************* + * 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 +#include +#include +#include +#include +#include + +namespace moveit_setup +{ +namespace controllers +{ +void UrdfModificationsWidget::onInit() +{ + QVBoxLayout* layout = new QVBoxLayout(); + + layout->setAlignment(Qt::AlignTop); + + // Top Header Area ------------------------------------------------ + auto header = new HeaderWidget("ros2_control URDF Modifications", + "This step can add the tags to the URDF required for interfacing with ros2_control. " + "See https://control.ros.org/ for more info.", + this); + layout->addWidget(header); + content_widget_ = new QWidget(this); + layout->addWidget(content_widget_); + + this->setLayout(layout); +} + +QWidget* UrdfModificationsWidget::makeInterfacesBox(const std::string& interface_type, + const std::vector& available_interfaces, + const std::vector& selected_interfaces, + QWidget* parent) +{ + QGroupBox* box = new QGroupBox((interface_type + " Interfaces").c_str(), parent); + QVBoxLayout* box_layout = new QVBoxLayout(parent); + for (const std::string& interface_name : available_interfaces) + { + QCheckBox* check = new QCheckBox(interface_name.c_str(), parent); + box_layout->addWidget(check); + + std::string key = interface_type[0] + interface_name; + check_boxes_[key] = check; + } + for (const std::string& interface_name : selected_interfaces) + { + std::string key = interface_type[0] + interface_name; + check_boxes_[key]->setChecked(true); + } + box->setLayout(box_layout); + return box; +} + +void UrdfModificationsWidget::focusGiven() +{ + setup_step_.refresh(); + qDeleteAll(content_widget_->children()); + check_boxes_.clear(); + + QVBoxLayout* layout = new QVBoxLayout(); + if (!setup_step_.needsModification()) + { + layout->addWidget(new QLabel("All of the joints used by this MoveIt config have already been configured using\n" + "ros2_control, so there is no need to modify the URDF with ros2_control tags.")); + content_widget_->setLayout(layout); + return; + } + + QWidget* interface_widget = new QWidget(this); + QHBoxLayout* interface_layout = new QHBoxLayout(); + + auto available_interfaces = setup_step_.getAvailableControlInterfaces(), + selected_interfaces = setup_step_.getDefaultControlInterfaces(); + + interface_layout->addWidget(makeInterfacesBox("Command", available_interfaces.command_interfaces, + selected_interfaces.command_interfaces, interface_widget)); + interface_layout->addWidget(makeInterfacesBox("State", available_interfaces.state_interfaces, + selected_interfaces.state_interfaces, interface_widget)); + + interface_widget->setLayout(interface_layout); + layout->addWidget(interface_widget); + + btn_add_interfaces_ = new QPushButton("Add interfaces"); + connect(btn_add_interfaces_, SIGNAL(clicked()), this, SLOT(addInterfaces())); + layout->addWidget(btn_add_interfaces_); + + generated_text_widget_ = new QTextEdit(); + generated_text_widget_->setReadOnly(true); + generated_text_widget_->setText(setup_step_.getJointsXML().c_str()); + layout->addWidget(generated_text_widget_); + content_widget_->setLayout(layout); +} + +std::vector UrdfModificationsWidget::getInterfaces(const char first_letter, + const std::vector& available_interfaces) +{ + std::vector interface_names; + for (const std::string& interface_name : available_interfaces) + { + std::string key = first_letter + interface_name; + if (check_boxes_[key]->isChecked()) + { + interface_names.push_back(interface_name); + } + } + return interface_names; +} + +void UrdfModificationsWidget::addInterfaces() +{ + auto available_interfaces = setup_step_.getAvailableControlInterfaces(); + setup_step_.setInterfaces(getInterfaces('C', available_interfaces.command_interfaces), + getInterfaces('S', available_interfaces.state_interfaces)); + generated_text_widget_->setText(setup_step_.getJointsXML().c_str()); +} + +} // namespace controllers +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::controllers::UrdfModificationsWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/gazebo_controllers.yaml b/moveit_setup_assistant/moveit_setup_controllers/templates/config/gazebo_controllers.yaml similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/config/gazebo_controllers.yaml rename to moveit_setup_assistant/moveit_setup_controllers/templates/config/gazebo_controllers.yaml diff --git a/moveit_setup_assistant/moveit_setup_controllers/templates/config/modified.urdf.xacro b/moveit_setup_assistant/moveit_setup_controllers/templates/config/modified.urdf.xacro new file mode 100644 index 0000000000..40791f5570 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/templates/config/modified.urdf.xacro @@ -0,0 +1,9 @@ + + +[MODIFIED_XACRO_ARGS] + + + +[MODIFIED_XACRO_IMPORTS] +[MODIFIED_XACRO_COMMANDS] + diff --git a/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro b/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro new file mode 100644 index 0000000000..3a821003ef --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro @@ -0,0 +1,14 @@ + + + + + + + + + fake_components/GenericSystem + +[ROS2_CONTROL_JOINTS] + + + diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_core_plugins/CMakeLists.txt new file mode 100644 index 0000000000..6cfc256a8a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 3.10.2) +project(moveit_setup_core_plugins) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(moveit_ros_visualization REQUIRED) +find_package(moveit_setup_framework REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(srdfdom REQUIRED) +find_package(urdf REQUIRED) + +# Instruct CMake to run moc automatically when needed. +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +qt5_wrap_cpp(MOC_FILES + include/moveit_setup_core_plugins/start_screen_widget.hpp + include/moveit_setup_core_plugins/configuration_files_widget.hpp + include/moveit_setup_core_plugins/author_information_widget.hpp +) + +add_library(${PROJECT_NAME} + src/start_screen.cpp + src/start_screen_widget.cpp + src/configuration_files.cpp + src/configuration_files_widget.cpp + src/author_information.cpp + src/author_information_widget.cpp + ${MOC_FILES} +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + moveit_ros_visualization + moveit_setup_framework + pluginlib + rclcpp + srdfdom + urdf +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(FILES moveit_setup_framework_plugins.xml + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) +pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) + +ament_package() diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information.hpp b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information.hpp new file mode 100644 index 0000000000..382952cc85 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information.hpp @@ -0,0 +1,70 @@ +/********************************************************************* + * 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 +{ +namespace core +{ +class AuthorInformation : public SetupStep +{ +public: + std::string getName() const override + { + return "Author Information"; + } + + void onInit() override; + + bool isReady() const override + { + return true; // always ready, no dependencies + } + + // Note, we use getAuthorName instead of getName because getName is defined above + std::string getAuthorName() const; + std::string getAuthorEmail() const; + void setAuthorName(const std::string& name); + void setAuthorEmail(const std::string& email); + +protected: + std::shared_ptr package_settings_; +}; +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information_widget.hpp similarity index 84% rename from moveit_setup_assistant/src/widgets/author_information_widget.h rename to moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information_widget.hpp index 4f9709c8f2..1cafb6027d 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.h +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information_widget.hpp @@ -38,15 +38,14 @@ class QLineEdit; -#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 +{ +namespace core { -class AuthorInformationWidget : public SetupScreenWidget +class AuthorInformationWidget : public SetupStepWidget { Q_OBJECT @@ -55,11 +54,16 @@ class AuthorInformationWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - AuthorInformationWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; /// Received when this widget is chosen from the navigation menu void focusGiven() override; + SetupStep& getSetupStep() override + { + return setup_step_; + } + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -77,8 +81,8 @@ private Q_SLOTS: void editedEmail(); private: - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + AuthorInformation setup_step_; }; -} // namespace moveit_setup_assistant +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files.hpp b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files.hpp new file mode 100644 index 0000000000..bfca1e8d62 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files.hpp @@ -0,0 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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. + *********************************************************************/ + +#pragma once +#include +#include + +namespace moveit_setup +{ +namespace core +{ +class ConfigurationFiles : public SetupStep +{ +public: + std::string getName() const override + { + return "Configuration Files"; + } + + void onInit() override; + + const std::filesystem::path& getPackagePath() + { + return package_settings_->getPackagePath(); + } + + void setPackagePath(const std::filesystem::path& package_path) + { + package_settings_->setPackagePath(package_path); + } + + void setPackageName(const std::string& package_name) + { + package_settings_->setPackageName(package_name); + } + + /// Populate the 'Files to be generated' list + void loadFiles(); + + const std::vector getGeneratedFiles() const + { + return gen_files_; + } + + unsigned int getNumFiles() const + { + return gen_files_.size(); + } + + bool shouldGenerate(const GeneratedFilePtr& file) const + { + std::string rel_path = file->getRelativePath(); + auto it = should_generate_.find(rel_path); + if (it == should_generate_.end()) + { + return true; + } + return it->second; + } + + bool hasModifiedFiles() const + { + return hasMatchingFileStatus(FileStatus::EXTERNALLY_MODIFIED); + } + + bool hasConflictingFiles() const + { + return hasMatchingFileStatus(FileStatus::CONFLICTED); + } + + void setShouldGenerate(const std::string& rel_path, bool should_generate); + + bool isExistingConfig(); + bool hasSetupAssistantFile(); + + void loadTemplateVariables(); + + std::vector getIncompleteWarnings() const; + + std::string getInvalidGroupName() const; + + void setGenerationTime() + { + package_settings_->setGenerationTime(); + } + +protected: + bool hasMatchingFileStatus(FileStatus status) const; + + // ****************************************************************************************** + // Variables + // ****************************************************************************************** + /// Vector of all files to be generated + std::vector gen_files_; + + std::unordered_map should_generate_; + + std::shared_ptr package_settings_; +}; +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files_widget.hpp similarity index 59% rename from moveit_setup_assistant/src/widgets/configuration_files_widget.h rename to moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files_widget.hpp index 2e6673705b..b4d45e3467 100644 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.h +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files_widget.hpp @@ -36,43 +36,23 @@ #pragma once -#include -class QLabel; -class QListWidget; -class QListWidgetItem; -class QProgressBar; -class QPushButton; - -#ifndef Q_MOC_RUN -#include -#endif +#include +#include "moveit_setup_framework/qt/setup_step_widget.hpp" +#include "moveit_setup_framework/qt/helper_widgets.hpp" -#include "setup_screen_widget.h" // a base class for screens in the setup assistant +#include +#include +#include +#include +#include +#include -namespace moveit_setup_assistant +namespace moveit_setup { -class LoadPathWidget; - -// Struct for storing all the file operations -struct GenerateFile +namespace core { - GenerateFile() : write_on_changes(0), generate_(true), modified_(false) - { - } - std::string file_name_; - std::string rel_path_; - std::string description_; - unsigned long write_on_changes; // bitfield indicating required rewrite - bool generate_; // "generate" checkbox ticked? - bool modified_; // file externally modified? - std::function gen_func_; -}; - -// Typedef for storing template string replacement pairs -typedef std::vector > StringPairVector; - // Class -class ConfigurationFilesWidget : public SetupScreenWidget +class ConfigurationFilesWidget : public SetupStepWidget { Q_OBJECT @@ -81,7 +61,12 @@ class ConfigurationFilesWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - ConfigurationFilesWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; + + SetupStep& getSetupStep() override + { + return setup_step_; + } /// Received when this widget is chosen from the navigation menu void focusGiven() override; @@ -121,16 +106,15 @@ private Q_SLOTS: /// Set checked state of all selected items void setCheckSelected(bool checked); + // When the configuration path changes + void onPackagePathChanged(const QString& path); + private: // ****************************************************************************************** // Variables // ****************************************************************************************** - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; - - /// Name of the new package that is being (or going) to be generated, based on user specified save path - std::string new_package_name_; + ConfigurationFiles setup_step_; /// Track progress unsigned int action_num_; @@ -138,25 +122,12 @@ private Q_SLOTS: /// Has the package been generated yet this program execution? Used for popping up exit warning bool has_generated_pkg_; - /// Vector of all files to be generated - std::vector gen_files_; - - /// Vector of all strings to search for in templates, and their replacements - StringPairVector template_strings_; - // ****************************************************************************************** // Private Functions // ****************************************************************************************** - /// Populate the 'Files to be generated' list - bool loadGenFiles(); - - /// Check the list of files to be generated for modification - /// Returns true if files were detected as modified - bool checkGenFiles(); - /// Show the list of files to be generated - bool showGenFiles(); + void showGenFiles(); /// Verify with user if certain screens have not been completed bool checkDependencies(); @@ -164,43 +135,9 @@ private Q_SLOTS: /// A function for showing progress and user feedback about what happened void updateProgress(); - /// Get the last folder name in a directory path - const std::string getPackageName(std::string package_path); - /// Check that no group is empty (without links/joints/etc) bool noGroupsEmpty(); - - /** - * \brief Load the strings that will be replaced in all templates - * \return void - */ - void loadTemplateStrings(); - - /** - * \brief Insert a string pair into the template_strings_ datastructure - * \param key string to search in template - * \param value string to replace with - * \return void - */ - bool addTemplateString(const std::string& key, const std::string& value); - - /** - * Copy a template from location to location and replace package name - * - * @param template_path path to template file - * @param output_path desired path to copy to - * @param new_package_name name of the new package being created, to replace key word in template - * - * @return bool if the template was copied correctly - */ - bool copyTemplate(const std::string& template_path, const std::string& output_path); - - /** - * \brief Create a folder - * \param output_path name of folder relative to package - * \return bool if success - */ - bool createFolder(const std::string& output_path); }; -} // namespace moveit_setup_assistant +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp new file mode 100644 index 0000000000..f54c91a089 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.hpp @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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. + *********************************************************************/ + +#pragma once +#include +#include +#include +#include + +namespace moveit_setup +{ +namespace core +{ +class StartScreen : public SetupStep +{ +public: + std::string getName() const override + { + return "Start Screen"; + } + + void onInit() override; + + std::filesystem::path getURDFPath(); + std::string getXacroArgs(); + std::filesystem::path getPackagePath(); + + bool isXacroFile(); + + void loadURDFFile(const std::filesystem::path& urdf_file_path, const std::string& xacro_args); + + void loadExisting(const std::filesystem::path& package_path); + +protected: + std::shared_ptr package_settings_; + std::shared_ptr srdf_config_; + std::shared_ptr urdf_config_; +}; +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.hpp similarity index 69% rename from moveit_setup_assistant/src/widgets/start_screen_widget.h rename to moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.hpp index 995ad325ac..bdf2a1b690 100644 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.h +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.hpp @@ -36,28 +36,30 @@ #pragma once +#include "moveit_setup_framework/qt/setup_step_widget.hpp" +#include "moveit_setup_framework/qt/helper_widgets.hpp" +#include +#include +#include class QLabel; class QProgressBar; class QPushButton; #ifndef Q_MOC_RUN -#include // for testing a valid urdf is loaded -#include // for testing a valid srdf is loaded -#include // common datastructure class +#include #endif -#include "setup_screen_widget.h" // a base class for screens in the setup assistant - -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace core { // Class Prototypes class SelectModeWidget; -class LoadPathArgsWidget; /** * \brief Start screen user interface for MoveIt Configuration Assistant */ -class StartScreenWidget : public SetupScreenWidget +class StartScreenWidget : public SetupStepWidget { Q_OBJECT @@ -66,13 +68,12 @@ class StartScreenWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - /** - * \brief Start screen user interface for MoveIt Configuration Assistant - */ - StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; ~StartScreenWidget() override; + void focusGiven() override; + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -81,12 +82,14 @@ class StartScreenWidget : public SetupScreenWidget LoadPathArgsWidget* urdf_file_; QPushButton* btn_load_; QLabel* next_label_; - QProgressBar* progress_bar_; + QProgressBar* progress_bar_; // TODO: Note that since the refactoring, the progress bar is less useful than before QImage* right_image_; QLabel* right_image_label_; - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + SetupStep& getSetupStep() override + { + return setup_step_; + } private Q_SLOTS: @@ -109,22 +112,12 @@ private Q_SLOTS: /// enable xacro arguments void onUrdfPathChanged(const QString& path); -Q_SIGNALS: - - // ****************************************************************************************** - // Emitted Signal Functions - // ****************************************************************************************** - - /// Event that is fired when the start screen has all its requirements completed and user can move on - void readyToProgress(); - - /// Inform the parent widget to load rviz. This is done so that progress bar can be more accurate - void loadRviz(); - private: // ****************************************************************************************** // Variables // ****************************************************************************************** + rclcpp::Node::SharedPtr node_; + StartScreen setup_step_; /// Create new config files, or load existing one? bool create_new_package_; @@ -141,27 +134,6 @@ private Q_SLOTS: /// Load existing package files bool loadExistingFiles(); - - /// Load URDF File to Parameter Server - bool loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args); - - /// Load SRDF File - bool loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args); - - /// Put SRDF File on Parameter Server - bool setSRDFFile(const std::string& srdf_string); - - //// Extract the package/stack name and relative path to urdf from an absolute path name - bool extractPackageNameFromPath(); - - /// Make the full URDF path using the loaded .setup_assistant data - bool createFullURDFPath(); - - /// Make the full SRDF path using the loaded .setup_assistant data - bool createFullSRDFPath(const std::string& package_path); - - /// Loads sensors_3d yaml file - bool load3DSensorsFile(); }; // ****************************************************************************************** @@ -185,4 +157,5 @@ private Q_SLOTS: QPushButton* btn_exist_; QLabel* widget_instructions_; }; -} // namespace moveit_setup_assistant +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/moveit_setup_framework_plugins.xml b/moveit_setup_assistant/moveit_setup_core_plugins/moveit_setup_framework_plugins.xml new file mode 100644 index 0000000000..70790e1ac7 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/moveit_setup_framework_plugins.xml @@ -0,0 +1,11 @@ + + + Start Screen for loading the URDF or initial MoveIt Config + + + The screen that actually generates the files for the Config package + + + The screen that configures the author's name and email + + diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/package.xml b/moveit_setup_assistant/moveit_setup_core_plugins/package.xml new file mode 100644 index 0000000000..7d7935973b --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/package.xml @@ -0,0 +1,29 @@ + + + + moveit_setup_core_plugins + 2.5.1 + Core (meta) plugins for MoveIt Setup Assistant + David V. Lu!! + BSD + David V. Lu!! + + ament_cmake + + ament_index_cpp + moveit_ros_visualization + moveit_setup_framework + pluginlib + rclcpp + srdfdom + urdf + + ament_lint_auto + ament_clang_format + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/author_information.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/author_information.cpp new file mode 100644 index 0000000000..4c3b54d50d --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/author_information.cpp @@ -0,0 +1,69 @@ +/********************************************************************* + * 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 +{ +namespace core +{ +void AuthorInformation::onInit() +{ + package_settings_ = config_data_->get("package_settings"); +} + +std::string AuthorInformation::getAuthorName() const +{ + return package_settings_->getAuthorName(); +} + +std::string AuthorInformation::getAuthorEmail() const +{ + return package_settings_->getAuthorEmail(); +} + +void AuthorInformation::setAuthorName(const std::string& name) +{ + package_settings_->setAuthorName(name); +} + +void AuthorInformation::setAuthorEmail(const std::string& email) +{ + package_settings_->setAuthorEmail(email); +} + +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/author_information_widget.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/author_information_widget.cpp similarity index 76% rename from moveit_setup_assistant/src/widgets/author_information_widget.cpp rename to moveit_setup_assistant/moveit_setup_core_plugins/src/author_information_widget.cpp index 3df16c7d02..e50fef3495 100644 --- a/moveit_setup_assistant/src/widgets/author_information_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/author_information_widget.cpp @@ -41,16 +41,17 @@ #include #include #include -#include "author_information_widget.h" -#include "header_widget.h" +#include +#include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace core { // ****************************************************************************************** // Outer User Interface for MoveIt Configuration Assistant // ****************************************************************************************** -AuthorInformationWidget::AuthorInformationWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +void AuthorInformationWidget::onInit() { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -58,11 +59,10 @@ AuthorInformationWidget::AuthorInformationWidget(QWidget* parent, const MoveItCo // Top Header Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget("Specify Author Information", - "Input contact information of the author and initial maintainer of the " - "generated package. catkin requires valid details in the package's " - "package.xml", - this); + auto header = new HeaderWidget("Specify Author Information", + "Input contact information of the author and initial maintainer of the generated " + "package. catkin requires valid details in the package's package.xml", + this); layout->addWidget(header); QLabel* name_title = new QLabel(this); @@ -91,20 +91,22 @@ AuthorInformationWidget::AuthorInformationWidget(QWidget* parent, const MoveItCo void AuthorInformationWidget::focusGiven() { // Allow list box to populate - this->name_edit_->setText(QString::fromStdString(config_data_->author_name_)); - this->email_edit_->setText(QString::fromStdString(config_data_->author_email_)); + this->name_edit_->setText(QString::fromStdString(setup_step_.getAuthorName())); + this->email_edit_->setText(QString::fromStdString(setup_step_.getAuthorEmail())); } void AuthorInformationWidget::editedName() { - config_data_->author_name_ = this->name_edit_->text().toStdString(); - config_data_->changes |= MoveItConfigData::AUTHOR_INFO; + setup_step_.setAuthorName(this->name_edit_->text().toStdString()); } void AuthorInformationWidget::editedEmail() { - config_data_->author_email_ = this->email_edit_->text().toStdString(); - config_data_->changes |= MoveItConfigData::AUTHOR_INFO; + setup_step_.setAuthorEmail(this->email_edit_->text().toStdString()); } -} // namespace moveit_setup_assistant +} // namespace core +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::core::AuthorInformationWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp new file mode 100644 index 0000000000..1388809114 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp @@ -0,0 +1,177 @@ +/********************************************************************* + * 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. + *********************************************************************/ + +#include +#include +#include + +namespace moveit_setup +{ +namespace core +{ +void ConfigurationFiles::onInit() +{ + package_settings_ = config_data_->get("package_settings"); +} + +void ConfigurationFiles::loadTemplateVariables() +{ + auto& variables = TemplatedGeneratedFile::variables_; + variables.clear(); + for (const auto& config : config_data_->getConfigured()) + { + config->collectVariables(variables); + } +} + +void ConfigurationFiles::loadFiles() +{ + package_settings_->loadDependencies(); + loadTemplateVariables(); + std::filesystem::path package_path = package_settings_->getPackagePath(); + auto gen_time = package_settings_->getGenerationTime(); + + // Check if we are 'editing' a prev generated config pkg + if (package_path.empty()) + { + return; // this is not configured yet + } + + gen_files_.clear(); + + for (const auto& config : config_data_->getConfigured()) + { + config->collectFiles(package_path, gen_time, gen_files_); + } +} + +bool ConfigurationFiles::hasMatchingFileStatus(FileStatus status) const +{ + for (const auto& gen_file : gen_files_) + { + if (gen_file->getStatus() == status) + { + return true; + } + } + return false; +} + +void ConfigurationFiles::setShouldGenerate(const std::string& rel_path, bool should_generate) +{ + should_generate_[rel_path] = should_generate; +} + +bool ConfigurationFiles::isExistingConfig() +{ + // if the folder doesn't exist or is empty + std::filesystem::path package_path(getPackagePath()); + return std::filesystem::is_directory(package_path) && !std::filesystem::is_empty(package_path); +} + +bool ConfigurationFiles::hasSetupAssistantFile() +{ + if (!isExistingConfig()) + { + return true; + } + return std::filesystem::is_regular_file(getPackagePath() / SETUP_ASSISTANT_FILE); +} + +std::vector ConfigurationFiles::getIncompleteWarnings() const +{ + // There may be a better way to generalize this, but for now, we add some manual checks of the srdf/author info + std::vector warnings; + + auto srdf_config = config_data_->get("srdf"); + // Check that at least 1 planning group exists + if (srdf_config->getGroups().empty()) + { + warnings.push_back("No robot model planning groups have been created"); + } + + // Check that at least 1 link pair is disabled from collision checking + if (srdf_config->getDisabledCollisions().empty()) + { + warnings.push_back("No self-collisions have been disabled"); + } + + // Check that there is at least 1 end effector added + if (srdf_config->getEndEffectors().empty()) + { + warnings.push_back("No end effectors have been added"); + } + + // Check that there is at least 1 virtual joint added + if (srdf_config->getVirtualJoints().empty()) + { + warnings.push_back("No virtual joints have been added"); + } + + // Check that there is a author name + if (!package_settings_->hasValidName()) + { + warnings.push_back("No author name added"); + } + + // Check that email information is filled + if (!package_settings_->hasValidEmail()) + { + warnings.push_back("No valid email address added"); + } + + return warnings; +} + +std::string ConfigurationFiles::getInvalidGroupName() const +{ + auto srdf_config = config_data_->get("srdf"); + for (const auto& group : srdf_config->getGroups()) + { + // Whenever 1 of the 4 component types are found, stop checking this group + if (!group.joints_.empty()) + continue; + if (!group.links_.empty()) + continue; + if (!group.chains_.empty()) + continue; + if (!group.subgroups_.empty()) + continue; + return group.name_; + } + return ""; +} + +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files_widget.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files_widget.cpp new file mode 100644 index 0000000000..1272788a48 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files_widget.cpp @@ -0,0 +1,530 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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: Dave Coleman */ + +// Qt +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "moveit_setup_core_plugins/configuration_files_widget.hpp" + +// Boost +#include // string trim +// Read write files +#include // For writing yaml and launch files +#include + +namespace moveit_setup +{ +namespace core +{ +// ****************************************************************************************** +// Outer User Interface for MoveIt Configuration Assistant +// ****************************************************************************************** +void ConfigurationFilesWidget::onInit() +{ + has_generated_pkg_ = false; + + // Basic widget container + QVBoxLayout* layout = new QVBoxLayout(); + + // Top Header Area ------------------------------------------------ + + auto header = + new HeaderWidget("Generate Configuration Files", + "Create or update the configuration files package needed to run your robot with MoveIt. Uncheck " + "files to disable them from being generated - this is useful if you have made custom changes to " + "them. Files in orange have been automatically detected as changed.", + this); + layout->addWidget(header); + + // Path Widget ---------------------------------------------------- + + // Stack Path Dialog + stack_path_ = new LoadPathWidget("Configuration Package Save Path", + "Specify the desired directory for the MoveIt configuration package to be " + "generated. Overwriting an existing configuration package directory is acceptable. " + "Example: /u/robot/ros/panda_moveit_config", + this, true); // is directory + layout->addWidget(stack_path_); + connect(stack_path_, SIGNAL(pathChanged(QString)), this, SLOT(onPackagePathChanged(QString))); + + // Generated Files List ------------------------------------------- + QLabel* generated_list = new QLabel("Check files you want to be generated:", this); + layout->addWidget(generated_list); + + QSplitter* splitter = new QSplitter(Qt::Horizontal, this); + splitter->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + // List Box + action_list_ = new QListWidget(this); + action_list_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + action_list_->setSelectionMode(QAbstractItemView::ExtendedSelection); + connect(action_list_, SIGNAL(currentRowChanged(int)), this, SLOT(changeActionDesc(int))); + // Allow checking / unchecking of multiple items + action_list_->setContextMenuPolicy(Qt::ActionsContextMenu); + QAction* action = new QAction("Check all selected files", this); + connect(action, &QAction::triggered, [this]() { setCheckSelected(true); }); + action_list_->addAction(action); + action = new QAction("Uncheck all selected files", this); + connect(action, &QAction::triggered, [this]() { setCheckSelected(false); }); + action_list_->addAction(action); + + // Description + action_label_ = new QLabel(this); + action_label_->setFrameShape(QFrame::StyledPanel); + action_label_->setFrameShadow(QFrame::Raised); + action_label_->setLineWidth(1); + action_label_->setMidLineWidth(0); + action_label_->setWordWrap(true); + action_label_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + action_label_->setMinimumWidth(100); + action_label_->setAlignment(Qt::AlignTop); + action_label_->setOpenExternalLinks(true); // open with web browser + + // Add to splitter + splitter->addWidget(action_list_); + splitter->addWidget(action_label_); + + // Add Layout + layout->addWidget(splitter); + + // Progress bar and generate buttons --------------------------------------------------- + QHBoxLayout* hlayout1 = new QHBoxLayout(); + + // Progress Bar + progress_bar_ = new QProgressBar(this); + progress_bar_->setMaximum(100); + progress_bar_->setMinimum(0); + hlayout1->addWidget(progress_bar_); + // hlayout1->setContentsMargins( 20, 30, 20, 30 ); + + // Generate Package Button + btn_save_ = new QPushButton("&Generate Package", this); + // btn_save_->setMinimumWidth(180); + btn_save_->setMinimumHeight(40); + connect(btn_save_, SIGNAL(clicked()), this, SLOT(savePackage())); + hlayout1->addWidget(btn_save_); + + // Add Layout + layout->addLayout(hlayout1); + + // Bottom row -------------------------------------------------- + + QHBoxLayout* hlayout3 = new QHBoxLayout(); + + // Success label + success_label_ = new QLabel(this); + QFont success_label_font(QFont().defaultFamily(), 12, QFont::Bold); + success_label_->setFont(success_label_font); + success_label_->hide(); // only show once the files have been generated + success_label_->setText("Configuration package generated successfully!"); + hlayout3->addWidget(success_label_); + hlayout3->setAlignment(success_label_, Qt::AlignRight); + + // Exit button + QPushButton* btn_exit = new QPushButton("E&xit Setup Assistant", this); + btn_exit->setMinimumWidth(180); + connect(btn_exit, SIGNAL(clicked()), this, SLOT(exitSetupAssistant())); + hlayout3->addWidget(btn_exit); + hlayout3->setAlignment(btn_exit, Qt::AlignRight); + + layout->addLayout(hlayout3); + + // Finish Layout -------------------------------------------------- + this->setLayout(layout); +} + +void ConfigurationFilesWidget::setCheckSelected(bool checked) +{ + for (const QModelIndex& row : action_list_->selectionModel()->selectedRows()) + action_list_->model()->setData(row, checked ? Qt::Checked : Qt::Unchecked, Qt::CheckStateRole); +} + +void ConfigurationFilesWidget::onPackagePathChanged(const QString& path) +{ + std::filesystem::path package_path = path.toStdString(); + + if (package_path == setup_step_.getPackagePath()) + { + return; + } + setup_step_.setPackagePath(package_path); + setup_step_.setPackageName(package_path.filename().string()); + + focusGiven(); +} + +// ****************************************************************************************** +// Verify with user if certain screens have not been completed +// ****************************************************************************************** +bool ConfigurationFilesWidget::checkDependencies() +{ + std::vector dependencies = setup_step_.getIncompleteWarnings(); + bool required_actions = false; + + // Note that MSA 1.0 required that you have valid author information before proceedings, and if not, would + // set required_actions to true here. We ignore this for now. + + // Display all accumumlated errors: + if (!dependencies.empty()) + { + // Create a dependency message + QString dep_message; + if (!required_actions) + { + dep_message = "Some setup steps have not been completed. None of the steps are required, but here is a reminder " + "of what was not filled in, just in case something was forgotten:
    "; + } + else + { + dep_message = "Some setup steps have not been completed. Please fix the required steps (printed in bold), " + "otherwise the setup cannot be completed:
      "; + } + + for (const auto& dependency : dependencies) + { + dep_message.append("
    • ").append(QString::fromStdString(dependency)).append("
      • "); + } + + if (!required_actions) + { + dep_message.append("

    Press Ok to continue generating files."); + if (QMessageBox::question(this, "Incomplete MoveIt Setup Assistant Steps", dep_message, + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) + { + return false; // abort + } + } + else + { + QMessageBox::warning(this, "Incomplete MoveIt Setup Assistant Steps", dep_message); + return false; + } + } + + return true; +} + +// ****************************************************************************************** +// A function for showing progress and user feedback about what happened +// ****************************************************************************************** +void ConfigurationFilesWidget::updateProgress() +{ + action_num_++; + + // Calc percentage + progress_bar_->setValue(double(action_num_) / setup_step_.getNumFiles() * 100); + + // allow the progress bar to be shown + QApplication::processEvents(); +} + +// ****************************************************************************************** +// Display the selected action in the desc box +// ****************************************************************************************** +void ConfigurationFilesWidget::changeActionDesc(int id) +{ + // Only allow event if list is not empty + if (id >= 0) + { + // Show the selected text + action_label_->setText(action_desc_.at(id)); + } +} + +// ****************************************************************************************** +// Disable or enable item in gen_files_ array +// ****************************************************************************************** +void ConfigurationFilesWidget::changeCheckedState(QListWidgetItem* item) +{ + std::size_t index = item->data(Qt::UserRole).toUInt(); + + auto gen_file = setup_step_.getGeneratedFiles()[index]; + + bool generate = (item->checkState() == Qt::Checked); + + if (!generate && gen_file->hasChanges()) + { + QMessageBox::warning(this, "Package Generation", + "You should generate this file to ensure your changes will take " + "effect."); + } + + // Enable/disable file + setup_step_.setShouldGenerate(gen_file->getRelativePath(), generate); +} + +// ****************************************************************************************** +// Called when setup assistant navigation switches to this screen +// ****************************************************************************************** +void ConfigurationFilesWidget::focusGiven() +{ + // Pass the package path from start screen to configuration files screen + stack_path_->setPath(setup_step_.getPackagePath()); + + setup_step_.loadFiles(); + + // disable reaction to checkbox changes + disconnect(action_list_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(changeCheckedState(QListWidgetItem*))); + + // Show files in GUI + showGenFiles(); + + // react to manual changes only (not programmatic ones) + connect(action_list_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(changeCheckedState(QListWidgetItem*))); + + // Allow list box to populate + QApplication::processEvents(); + + // Which files have been modified outside the Setup Assistant? + if (setup_step_.hasModifiedFiles()) + { + // Some were found to be modified + QString msg("Some files have been modified outside of the Setup Assistant (according to timestamp). " + "The Setup Assistant will not overwrite these changes by default because often changing configuration " + "files manually is necessary, " + "but we recommend you check the list and enable the checkbox next to files you would like to " + "overwrite. "); + if (setup_step_.hasConflictingFiles()) + msg += "
    Attention: Some files (marked red) are changed " + "both, externally and in Setup Assistant."; + QMessageBox::information(this, "Files Modified", msg); + } +} + +// ****************************************************************************************** +// Show the list of files to be generated +// ****************************************************************************************** +void ConfigurationFilesWidget::showGenFiles() +{ + action_list_->clear(); + + auto gen_files = setup_step_.getGeneratedFiles(); + + // Display this list in the GUI + for (std::size_t i = 0; i < gen_files.size(); ++i) + { + auto gen_file = gen_files[i]; + + // Create a formatted row + QListWidgetItem* item = new QListWidgetItem(QString(gen_file->getRelativePath().c_str()), action_list_, 0); + + // Checkbox + item->setCheckState(setup_step_.shouldGenerate(gen_file) ? Qt::Checked : Qt::Unchecked); + + auto status = gen_file->getStatus(); + if (status == FileStatus::CONFLICTED) + { + item->setForeground(QBrush(QColor(255, 0, 0))); + } + else if (status == FileStatus::EXTERNALLY_MODIFIED) + { + item->setForeground(QBrush(QColor(255, 135, 0))); + } + + // Link the gen_files_ index to this item + item->setData(Qt::UserRole, QVariant(static_cast(i))); + + // Add actions to list + action_list_->addItem(item); + action_desc_.append(QString(gen_file->getDescription().c_str())); + } + + // Select the first item in the list so that a description is visible + action_list_->setCurrentRow(0); +} + +// ****************************************************************************************** +// Save configuration click event +// ****************************************************************************************** +void ConfigurationFilesWidget::savePackage() +{ + // Feedback + success_label_->hide(); + + // Reset the progress bar counter and GUI stuff + action_num_ = 0; + progress_bar_->setValue(0); + + if (!generatePackage()) + { + RCLCPP_ERROR_STREAM(setup_step_.getLogger(), "Failed to generate entire configuration package"); + return; + } + + // Alert user it completed successfully -------------------------------------------------- + progress_bar_->setValue(100); + success_label_->show(); + has_generated_pkg_ = true; +} + +// ****************************************************************************************** +// Save package using default path +// ****************************************************************************************** +bool ConfigurationFilesWidget::generatePackage() +{ + // Get path name + std::string package_path_s = stack_path_->getPath(); + // Trim whitespace from user input + boost::trim(package_path_s); + + // Check that a valid stack package name has been given + if (package_path_s.empty()) + { + QMessageBox::warning(this, "Error Generating", + "No package path provided. Please choose a directory location to " + "generate the MoveIt configuration files."); + return false; + } + + // Check setup assist deps + if (!checkDependencies()) + return false; // canceled + + // Check that all groups have components + if (!noGroupsEmpty()) + return false; // not ready + + // Make sure old package is correct package type and verify over write + if (setup_step_.isExistingConfig()) + { + // Check if the old package is a setup assistant package. If it is not, quit + if (!setup_step_.hasSetupAssistantFile()) + { + QMessageBox::warning( + this, "Incorrect Folder/Package", + QString("The chosen package location already exists but was not previously created using this MoveIt Setup " + "Assistant. " + "If this is a mistake, add the missing file: ") + .append(SETUP_ASSISTANT_FILE.c_str())); + return false; + } + + // Confirm overwrite + if (QMessageBox::question(this, "Confirm Package Update", + QString("Are you sure you want to overwrite this existing package with updated " + "configurations?
    ") + .append(package_path_s.c_str()) + .append(""), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) + { + return false; // abort + } + } + + setup_step_.setGenerationTime(); + + // Begin to create files and folders ---------------------------------------------------------------------- + std::filesystem::path absolute_path; + + for (auto& gen_file : setup_step_.getGeneratedFiles()) + { + // Check if we should skip this file + if (!setup_step_.shouldGenerate(gen_file)) + { + continue; + } + absolute_path = gen_file->getPath(); + + // Create the absolute path + RCLCPP_DEBUG_STREAM(setup_step_.getLogger(), "Creating file " << absolute_path.string()); + + // Run the generate function + if (!gen_file->write()) + { + // Error occurred + QMessageBox::critical(this, "Error Generating File", + QString("Failed to generate folder or file: '") + .append(gen_file->getRelativePath().c_str()) + .append("' at location:\n") + .append(absolute_path.c_str())); + return false; + } + updateProgress(); // Increment and update GUI + } + + return true; +} + +// ****************************************************************************************** +// Quit the program because we are done +// ****************************************************************************************** +void ConfigurationFilesWidget::exitSetupAssistant() +{ + if (has_generated_pkg_ || QMessageBox::question(this, "Exit Setup Assistant", + QString("Are you sure you want to exit the MoveIt Setup Assistant?"), + QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Ok) + { + QApplication::quit(); + } +} + +// ****************************************************************************************** +// Check that no group is empty (without links/joints/etc) +// ****************************************************************************************** +bool ConfigurationFilesWidget::noGroupsEmpty() +{ + // Loop through all groups + std::string invalid_group = setup_step_.getInvalidGroupName(); + if (!invalid_group.empty()) + { + // This group has no contents, bad + QMessageBox::warning( + this, "Empty Group", + QString("The planning group '") + .append(invalid_group.c_str()) + .append("' is empty and has no subcomponents associated with it (joints/links/chains/subgroups). You must " + "edit or remove this planning group before this configuration package can be saved.")); + return false; + } + + return true; // good +} +} // namespace core +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::core::ConfigurationFilesWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen.cpp new file mode 100644 index 0000000000..ff6827d6ff --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen.cpp @@ -0,0 +1,81 @@ +/********************************************************************* + * 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 +{ +namespace core +{ +void StartScreen::onInit() +{ + package_settings_ = config_data_->get("package_settings"); + srdf_config_ = config_data_->get("srdf"); + urdf_config_ = config_data_->get("urdf"); +} + +void StartScreen::loadURDFFile(const std::filesystem::path& urdf_file_path, const std::string& xacro_args) +{ + urdf_config_->loadFromPath(urdf_file_path, xacro_args); + srdf_config_->updateRobotModel(); +} + +std::filesystem::path StartScreen::getURDFPath() +{ + return urdf_config_->getURDFPath(); +} + +std::string StartScreen::getXacroArgs() +{ + return urdf_config_->getXacroArgs(); +} + +std::filesystem::path StartScreen::getPackagePath() +{ + return package_settings_->getPackagePath(); +} + +void StartScreen::loadExisting(const std::filesystem::path& package_path) +{ + package_settings_->loadExisting(package_path); +} + +bool StartScreen::isXacroFile() +{ + return urdf_config_->isXacroFile(); +} +} // namespace core +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp new file mode 100644 index 0000000000..f5bc5f47f8 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/src/start_screen_widget.cpp @@ -0,0 +1,491 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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: Dave Coleman */ + +// Qt +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// SA +#include "moveit_setup_core_plugins/start_screen_widget.hpp" +// C +#include // for reading in urdf +#include + +namespace moveit_setup +{ +namespace core +{ +// ****************************************************************************************** +// Start screen user interface for MoveIt Configuration Assistant +// ****************************************************************************************** +void StartScreenWidget::onInit() +{ + // Basic widget container + QVBoxLayout* layout = new QVBoxLayout(this); + + // Horizontal layout splitter + QHBoxLayout* hlayout = new QHBoxLayout(); + // Left side of screen + QVBoxLayout* left_layout = new QVBoxLayout(); + // Right side of screen + QVBoxLayout* right_layout = new QVBoxLayout(); + + // Right Image Area ---------------------------------------------- + right_image_ = new QImage(); + right_image_label_ = new QLabel(this); + auto image_path = getSharePath("moveit_setup_assistant") / "resources/MoveIt_Setup_Assistant2.png"; + + if (right_image_->load(image_path.c_str())) + { + right_image_label_->setPixmap(QPixmap::fromImage(*right_image_)); + right_image_label_->setMinimumHeight(384); // size of right_image_label_ + } + else + { + RCLCPP_ERROR_STREAM(setup_step_.getLogger(), "FAILED TO LOAD " << image_path); + } + + right_layout->addWidget(right_image_label_); + right_layout->setAlignment(right_image_label_, Qt::AlignRight | Qt::AlignTop); + + // Top Label Area --------------------------------------------------- + HeaderWidget* header = new HeaderWidget( + "MoveIt Setup Assistant", + "These tools will assist you in creating a Semantic Robot Description Format (SRDF) file, various yaml " + "configuration and many roslaunch files for utilizing all aspects of MoveIt functionality.", + this); + layout->addWidget(header); + + // Select Mode Area ------------------------------------------------- + select_mode_ = new SelectModeWidget(this); + connect(select_mode_->btn_new_, SIGNAL(clicked()), this, SLOT(showNewOptions())); + connect(select_mode_->btn_exist_, SIGNAL(clicked()), this, SLOT(showExistingOptions())); + left_layout->addWidget(select_mode_); + + // Path Box Area ---------------------------------------------------- + + // Stack Path Dialog + stack_path_ = new LoadPathArgsWidget("Load MoveIt Configuration Package", + "Specify the package name or path of an existing MoveIt configuration package " + "to be edited for your robot. Example package name: panda_moveit_config", + "optional xacro arguments:", this, true); // directory + // user needs to select option before this is shown + stack_path_->hide(); + stack_path_->setArgs(""); + connect(stack_path_, SIGNAL(pathChanged(QString)), this, SLOT(onPackagePathChanged(QString))); + left_layout->addWidget(stack_path_); + + // URDF File Dialog + urdf_file_ = new LoadPathArgsWidget( + "Load a URDF or COLLADA Robot Model", + "Specify the location of an existing Universal Robot Description Format or COLLADA file for your robot", + "optional xacro arguments:", this, false, true); // no directory, load only + // user needs to select option before this is shown + urdf_file_->hide(); + urdf_file_->setArgs(""); + connect(urdf_file_, SIGNAL(pathChanged(QString)), this, SLOT(onUrdfPathChanged(QString))); + left_layout->addWidget(urdf_file_); + + // Load settings box --------------------------------------------- + QHBoxLayout* load_files_layout = new QHBoxLayout(); + + progress_bar_ = new QProgressBar(this); + progress_bar_->setMaximum(100); + progress_bar_->setMinimum(0); + progress_bar_->hide(); + load_files_layout->addWidget(progress_bar_); + + btn_load_ = new QPushButton("&Load Files", this); + btn_load_->setMinimumWidth(180); + btn_load_->setMinimumHeight(40); + btn_load_->hide(); + load_files_layout->addWidget(btn_load_); + load_files_layout->setAlignment(btn_load_, Qt::AlignRight); + connect(btn_load_, SIGNAL(clicked()), this, SLOT(loadFilesClick())); + + // Next step instructions + next_label_ = new QLabel(this); + QFont next_label_font(QFont().defaultFamily(), 11, QFont::Bold); + next_label_->setFont(next_label_font); + next_label_->setText("Success! Use the left navigation pane to continue."); + next_label_->hide(); // only show once the files have been loaded. + + // Final Layout Setup --------------------------------------------- + // Alignment + layout->setAlignment(Qt::AlignTop); + hlayout->setAlignment(Qt::AlignTop); + left_layout->setAlignment(Qt::AlignTop); + right_layout->setAlignment(Qt::AlignTop); + + // Stretch + left_layout->setSpacing(10); + + // Attach Layouts + hlayout->addLayout(left_layout); + hlayout->addLayout(right_layout); + layout->addLayout(hlayout); + + // Vertical Spacer + layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding)); + + // Attach bottom layout + layout->addWidget(next_label_); + layout->setAlignment(next_label_, Qt::AlignRight); + layout->addLayout(load_files_layout); + + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); + this->setLayout(layout); + + // Debug mode: auto load the configuration file by clicking button after a timeout + if (debug_) + { + // select_mode_->btn_exist_->click(); + + QTimer* update_timer = new QTimer(this); + update_timer->setSingleShot(true); // only run once + connect(update_timer, SIGNAL(timeout()), btn_load_, SLOT(click())); + update_timer->start(100); + } +} + +// ****************************************************************************************** +// Destructor +// ****************************************************************************************** +StartScreenWidget::~StartScreenWidget() +{ + delete right_image_; // does not have a parent passed to it +} + +void StartScreenWidget::focusGiven() +{ + std::filesystem::path pkg_path = setup_step_.getPackagePath(); + if (!pkg_path.empty()) + { + stack_path_->setPath(pkg_path); + select_mode_->btn_exist_->click(); + return; + } + + std::filesystem::path urdf_path = setup_step_.getURDFPath(); + if (!urdf_path.empty()) + { + urdf_file_->setPath(urdf_path); + select_mode_->btn_new_->click(); + } +} + +// ****************************************************************************************** +// Show options for creating a new configuration package +// ****************************************************************************************** +void StartScreenWidget::showNewOptions() +{ + // Do GUI stuff + select_mode_->btn_exist_->setChecked(false); + select_mode_->btn_new_->setChecked(true); + select_mode_->widget_instructions_->hide(); + urdf_file_->show(); + stack_path_->hide(); + btn_load_->show(); + + // Remember choice + create_new_package_ = true; +} + +// ****************************************************************************************** +// Show options for editing an existing configuration package +// ****************************************************************************************** +void StartScreenWidget::showExistingOptions() +{ + // Do GUI stuff + select_mode_->btn_exist_->setChecked(true); + select_mode_->btn_new_->setChecked(false); + select_mode_->widget_instructions_->hide(); + urdf_file_->hide(); + stack_path_->show(); + btn_load_->show(); + + // Remember choice + create_new_package_ = false; +} + +// ****************************************************************************************** +// Load files to parameter server - CLICK +// ****************************************************************************************** +void StartScreenWidget::loadFilesClick() +{ + // Disable start screen GUI components from being changed + urdf_file_->setDisabled(true); + // srdf_file_->setDisabled(true); + stack_path_->setDisabled(true); + select_mode_->setDisabled(true); + btn_load_->setDisabled(true); + progress_bar_->show(); + + bool result; + + // Decide if this is a new config package, or loading an old one + if (create_new_package_) + { + result = loadNewFiles(); + } + else + { + result = loadExistingFiles(); + } + + // Check if there was a failure loading files + if (!result) + { + // Renable components + urdf_file_->setDisabled(false); + // srdf_file_->setDisabled(false); + stack_path_->setDisabled(false); + select_mode_->setDisabled(false); + btn_load_->setDisabled(false); + progress_bar_->hide(); + } + else + { + // Hide the logo image so that other screens can resize the rviz thing properly + right_image_label_->hide(); + } +} + +void StartScreenWidget::onPackagePathChanged(const QString& /*path*/) +{ + if (!loadPackageSettings(false)) + return; + // set xacro args from loaded settings + stack_path_->setArgs(QString::fromStdString(setup_step_.getXacroArgs())); +} + +void StartScreenWidget::onUrdfPathChanged(const QString& path) +{ + setup_step_.loadURDFFile(path.toStdString(), urdf_file_->getArgs().toStdString()); + urdf_file_->setArgsEnabled(setup_step_.isXacroFile()); +} + +bool StartScreenWidget::loadPackageSettings(bool show_warnings) +{ + // Get the package path + std::filesystem::path package_path_input = stack_path_->getPath(); + + try + { + setup_step_.loadExisting(package_path_input); + return true; + } + catch (const std::runtime_error& e) + { + if (show_warnings) + QMessageBox::warning(this, "Error Loading Files", e.what()); + return false; + } +} + +// ****************************************************************************************** +// Load existing package files +// ****************************************************************************************** +bool StartScreenWidget::loadExistingFiles() +{ + try + { + // Progress Indicator + progress_bar_->setValue(10); + QApplication::processEvents(); + + if (!loadPackageSettings(true)) + return false; + } + catch (const std::runtime_error& e) + { + QMessageBox::warning(this, "Error Loading SRDF", QString(e.what())); + RCLCPP_ERROR(setup_step_.getLogger(), "%s", e.what()); + return false; + } + + // Progress Indicator + progress_bar_->setValue(100); + QApplication::processEvents(); + + next_label_->show(); // only show once the files have been loaded + Q_EMIT dataUpdated(); + + RCLCPP_INFO(setup_step_.getLogger(), "Loading Setup Assistant Complete"); + + Q_EMIT advanceRequest(); + return true; // success! +} + +// ****************************************************************************************** +// Load chosen files for creating new package +// ****************************************************************************************** +bool StartScreenWidget::loadNewFiles() +{ + // Get URDF file path + std::filesystem::path urdf_path = urdf_file_->getPath(); + + // Check that box is filled out + if (urdf_path.empty()) + { + QMessageBox::warning(this, "Error Loading Files", "No robot model file specified"); + return false; + } + + // Check that this file exits + if (!std::filesystem::is_regular_file(urdf_path)) + { + QMessageBox::warning(this, "Error Loading Files", + QString("Unable to locate the URDF file: ").append(urdf_path.c_str())); + return false; + } + + // Progress Indicator + progress_bar_->setValue(20); + QApplication::processEvents(); + + // use xacro args from GUI + std::string xacro_args = urdf_file_->getArgs().toStdString(); + try + { + setup_step_.loadURDFFile(urdf_path, xacro_args); + } + catch (const std::runtime_error& e) + { + QMessageBox::warning(this, "Error Loading URDF", QString(e.what())); + return false; + } + + // Progress Indicator + progress_bar_->setValue(50); + QApplication::processEvents(); + + // DONE LOADING -------------------------------------------------------------------------- + + // Call a function that enables navigation + Q_EMIT dataUpdated(); + + // Progress Indicator + progress_bar_->setValue(70); + QApplication::processEvents(); + + // Progress Indicator + progress_bar_->setValue(100); + QApplication::processEvents(); + + next_label_->show(); // only show once the files have been loaded + + RCLCPP_INFO(setup_step_.getLogger(), "Loading Setup Assistant Complete"); + return true; // success! +} + +// ****************************************************************************************** +// ****************************************************************************************** +// Class for selecting which mode +// ****************************************************************************************** +// ****************************************************************************************** + +// ****************************************************************************************** +// Create the widget +// ****************************************************************************************** +SelectModeWidget::SelectModeWidget(QWidget* parent) : QFrame(parent) +{ + // Set frame graphics + setFrameShape(QFrame::StyledPanel); + setFrameShadow(QFrame::Raised); + setLineWidth(1); + setMidLineWidth(0); + + // Basic widget container + QVBoxLayout* layout = new QVBoxLayout(this); + + // Horizontal layout splitter + QHBoxLayout* hlayout = new QHBoxLayout(); + + // Widget Title + QLabel* widget_title = new QLabel(this); + widget_title->setText("Create new or edit existing?"); + QFont widget_title_font(QFont().defaultFamily(), 12, QFont::Bold); + widget_title->setFont(widget_title_font); + layout->addWidget(widget_title); + layout->setAlignment(widget_title, Qt::AlignTop); + + // Widget Instructions + widget_instructions_ = new QLabel(this); + widget_instructions_->setAlignment(Qt::AlignLeft | Qt::AlignTop); + widget_instructions_->setWordWrap(true); + widget_instructions_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + widget_instructions_->setText( + "All settings for MoveIt are stored in the MoveIt configuration package. Here you have the option to create a " + "new configuration package or load an existing one. Note: changes to a MoveIt configuration package outside this " + "Setup Assistant are likely to be overwritten by this tool."); + + layout->addWidget(widget_instructions_); + layout->setAlignment(widget_instructions_, Qt::AlignTop); + + // New Button + btn_new_ = new QPushButton(this); + btn_new_->setText("Create &New MoveIt\nConfiguration Package"); + hlayout->addWidget(btn_new_); + + // Exist Button + btn_exist_ = new QPushButton(this); + btn_exist_->setText("&Edit Existing MoveIt\nConfiguration Package"); + btn_exist_->setCheckable(true); + hlayout->addWidget(btn_exist_); + + // Add horizontal layer to vertical layer + layout->addLayout(hlayout); + setLayout(layout); + btn_new_->setCheckable(true); +} + +} // namespace core +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::core::StartScreenWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt new file mode 100644 index 0000000000..b9f386ab75 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.10.2) +project(moveit_setup_framework) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_visualization REQUIRED) +find_package(Qt5Core REQUIRED) +find_package(Qt5Widgets REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(srdfdom REQUIRED) +find_package(urdf REQUIRED) + +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +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 + src/rviz_panel.cpp + src/data_warehouse.cpp + src/templates.cpp + src/utilities.cpp + src/xml_syntax_highlighter.cpp + ${MOC_FILES} +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + moveit_core + moveit_ros_planning + moveit_ros_visualization + pluginlib + Qt5Core + Qt5Widgets + rclcpp + rviz_common + rviz_rendering + srdfdom + urdf +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() +install(DIRECTORY include/ + DESTINATION include +) +install(FILES moveit_setup_framework_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY templates DESTINATION share/${PROJECT_NAME}) + +ament_export_targets(export_moveit_setup_framework HAS_LIBRARY_TARGET) +install(TARGETS ${PROJECT_NAME} + EXPORT export_moveit_setup_framework + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) +ament_export_include_directories(include) +ament_export_dependencies(rclcpp) +ament_export_dependencies(Qt5Core) +ament_export_dependencies(Qt5Widgets) +ament_export_dependencies(moveit_common) +ament_export_dependencies(moveit_core) +ament_export_dependencies(moveit_ros_planning) +ament_export_dependencies(moveit_ros_visualization) +ament_export_dependencies(rviz_common) +ament_export_dependencies(rviz_rendering) +pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) + +ament_package() diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp new file mode 100644 index 0000000000..14eac2f768 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp @@ -0,0 +1,159 @@ +/********************************************************************* + * 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 +#include + +#include + +namespace moveit_setup +{ +class DataWarehouse; +// NB: We don't use DataWarehousePtr here because it gets complicated with this abstract usage + +MOVEIT_CLASS_FORWARD(SetupConfig); // Defines SetupConfigPtr, ConstPtr, WeakPtr... etc + +/** + * @brief where all the data for each part of the configuration is stored. + */ +class SetupConfig +{ +public: + /** + * @brief Called after construction to initialize the step + * @param config_data Pointer to all the other configs + * @param parent_node Shared pointer to the parent node + * @param name + */ + void initialize(std::shared_ptr config_data, const rclcpp::Node::SharedPtr& parent_node, + const std::string& name) + { + config_data_ = config_data; + parent_node_ = parent_node; + name_ = name; + logger_ = std::make_shared(parent_node->get_logger().get_child(name)); + onInit(); + } + + /** + * @brief Overridable initialization method + */ + virtual void onInit() + { + } + + /** + * @brief The name for this part of the configuration + */ + const std::string& getName() + { + return name_; + } + + /** + * @brief Return true if this part of the configuration is completely set up. + */ + virtual bool isConfigured() const + { + return false; + } + + /** + * @brief Loads the configuration from an existing MoveIt configuration. + * + * The data can be loaded directly from files in the configuration + * via the package path. + * + * Certain other pieces of "meta" information may be stored in the .setup_assistant + * yaml file in the root of the configuration. If there is a node in that file that + * matches this config's name, it is passed in as an argument. + * + * @param package_path The path to the root folder of the configuration. + */ + virtual void loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& /*node*/) + { + } + + /** + * @brief Optionally save "meta" information for saving in the .setup_assistant yaml file + */ + virtual YAML::Node saveToYaml() const + { + return YAML::Node(); + } + + /** + * @brief Collect the files generated by this configuration and add them to the vector + * + * @param[in] package_path the path to the root of the config package + * @param[in] last_gen_time The time (if any) when the config package was last generated + * @parma[out] files Where to put the new generated files + */ + virtual void collectFiles(const std::filesystem::path& /*package_path*/, const GeneratedTime& /*last_gen_time*/, + std::vector& /*files*/) + { + } + + /** + * @brief Collect the package dependencies generated by this configuration + * + * @param[out] packages Names of ROS packages + */ + virtual void collectDependencies(std::set& /*packages*/) const + { + } + + /** + * @brief Collect key/value pairs for use in templates + * + * @param[out] variables Where to put the new Variables + */ + virtual void collectVariables(std::vector& /*variables*/) + { + } + +protected: + std::shared_ptr config_data_; + rclcpp::Node::SharedPtr parent_node_; + std::string name_; + std::shared_ptr logger_; +}; +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.hpp new file mode 100644 index 0000000000..128d7dc6c3 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.hpp @@ -0,0 +1,233 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, Inc. + * 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace moveit_setup +{ +static const std::string SETUP_ASSISTANT_FILE = ".setup_assistant"; + +class PackageSettingsConfig : public SetupConfig +{ +public: + /** + * @brief Overridden method to load THIS config's data variables. + * + * Not to be confused with loadExisting which is a PackageSettingsConfig-specific + * method for loading ALL configs and their data. + */ + void loadPrevious(const std::filesystem::path& package_path, const YAML::Node& node) override; + YAML::Node saveToYaml() const override; + + /** + * @brief Method for loading the contents of the .setup_assistant file into all the configs + * + * @param package_path_or_name Either the path to the MoveIt config package folder OR the name of the package. + */ + void loadExisting(const std::string& package_path_or_name); + + const std::filesystem::path& getPackagePath() const + { + return config_pkg_path_; + } + + const std::string& getPackageName() const + { + return new_package_name_; + } + + void setPackagePath(const std::filesystem::path& package_path); + + void setPackageName(const std::string& package_name); + + const GeneratedTime& getGenerationTime() const + { + return config_pkg_generated_timestamp_; + } + + bool isConfigured() const override + { + return !config_pkg_path_.empty(); + } + + bool hasValidName() const; + + bool hasValidEmail() const; + + class GeneratedSettings : public YamlGeneratedFile + { + public: + GeneratedSettings(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + PackageSettingsConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + std::filesystem::path getRelativePath() const override + { + return SETUP_ASSISTANT_FILE; + } + + std::string getDescription() const override + { + return "MoveIt Setup Assistant's hidden settings file. You should not need to edit this file."; + } + + bool hasChanges() const override + { + // We write this file on any changes + return true; + } + + bool writeYaml(YAML::Emitter& emitter) override; + + protected: + PackageSettingsConfig& parent_; + }; + + class GeneratedPackageXML : public TemplatedGeneratedFile + { + public: + GeneratedPackageXML(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + PackageSettingsConfig& parent) + : TemplatedGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + bool hasChanges() const override + { + return parent_.author_info_changed_; + } + + std::filesystem::path getRelativePath() const override + { + return "package.xml"; + } + + std::filesystem::path getTemplatePath() const override + { + // Note: we call the file package.xml.template so that it isn't automatically indexed by rosprofile + return getSharePath("moveit_setup_framework") / "templates" / "package.xml.template"; + } + + std::string getDescription() const override + { + return "Defines a ROS package"; + } + + protected: + PackageSettingsConfig& parent_; + }; + + class GeneratedCMake : public TemplatedGeneratedFile + { + public: + using TemplatedGeneratedFile::TemplatedGeneratedFile; + + bool hasChanges() const override + { + return false; // Generally doesn't change + } + + std::filesystem::path getRelativePath() const override + { + return "CMakeLists.txt"; + } + + std::filesystem::path getTemplatePath() const override + { + return getSharePath("moveit_setup_framework") / "templates" / "CMakeLists.txt"; + } + + std::string getDescription() const override + { + return "CMake build system configuration file"; + } + }; + + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) override + { + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + files.push_back(std::make_shared(package_path, last_gen_time)); + } + + void collectVariables(std::vector& variables) override; + + void loadDependencies(); + + std::string getAuthorName() const + { + return author_name_; + } + std::string getAuthorEmail() const + { + return author_email_; + } + void setAuthorName(const std::string& name) + { + author_name_ = name; + } + void setAuthorEmail(const std::string& email) + { + author_email_ = email; + } + + void setGenerationTime(); + +protected: + /// Loaded configuration package path - if an existing package was loaded, holds that path + std::filesystem::path config_pkg_path_; + + /// Name of the new package that is being (or going) to be generated, based on user specified save path + std::string new_package_name_{ "unnamed_moveit_config" }; + + /// Name of the author of this config + std::string author_name_; + + /// Email of the author of this config + std::string author_email_; + + bool author_info_changed_{ false }; + + /// Timestamp when configuration package was generated, if it was previously generated + GeneratedTime config_pkg_generated_timestamp_; + + std::set package_dependencies_; +}; +} // namespace moveit_setup 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 new file mode 100644 index 0000000000..9039a9bf3c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp @@ -0,0 +1,304 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, Inc. + * 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 +#include // for getting kinematic model +#include // for writing srdf data + +namespace moveit_setup +{ +// 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, +}; + +static const std::string JOINT_LIMITS_FILE = "config/joint_limits.yaml"; +static const std::string CARTESIAN_LIMITS_FILE = "config/cartesian_limits.yaml"; + +class SRDFConfig : public SetupConfig +{ +public: + void onInit() override; + + bool isConfigured() const override + { + return robot_model_ != nullptr; + } + + void loadPrevious(const std::filesystem::path& package_path, const YAML::Node& node) override; + YAML::Node saveToYaml() const override; + + /// Load SRDF File + void loadSRDFFile(const std::filesystem::path& package_path, const std::filesystem::path& relative_path); + void loadSRDFFile(const std::filesystem::path& srdf_file_path, + const std::vector& xacro_args = std::vector()); + + moveit::core::RobotModelPtr getRobotModel() const + { + return robot_model_; + } + + /// Provide a shared planning scene + planning_scene::PlanningScenePtr getPlanningScene(); + + /// 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; + + void clearCollisionData() + { + srdf_.no_default_collision_links_.clear(); + srdf_.enabled_collision_pairs_.clear(); + srdf_.disabled_collision_pairs_.clear(); + } + + std::vector& getDisabledCollisions() + { + return srdf_.disabled_collision_pairs_; + } + + std::vector& getEndEffectors() + { + return srdf_.end_effectors_; + } + + std::vector& getGroups() + { + 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); + + std::vector getJointNames(const std::string& group_name, bool include_multi_dof = true, + bool include_passive = true); + + class GeneratedSRDF : public GeneratedFile + { + public: + GeneratedSRDF(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, SRDFConfig& parent) + : GeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + std::filesystem::path getRelativePath() const override + { + return parent_.srdf_pkg_relative_path_; + } + + std::string getDescription() const override + { + return "SRDF (Semantic Robot Description Format) is a " + "representation of semantic information about robots. This format is intended to represent " + "information about the robot that is not in the URDF file, but it is useful for a variety of " + "applications. The intention is to include information that has a semantic aspect to it."; + } + + bool hasChanges() const override + { + return parent_.changes_ > 0; + } + + bool write() override + { + std::filesystem::path path = getPath(); + createParentFolders(path); + return parent_.write(path); + } + + protected: + SRDFConfig& parent_; + }; + + class GeneratedJointLimits : public YamlGeneratedFile + { + public: + GeneratedJointLimits(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + SRDFConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + std::filesystem::path getRelativePath() const override + { + return JOINT_LIMITS_FILE; + } + + std::string getDescription() const override + { + return "Contains additional information about joints that appear in your planning groups that is not " + "contained in the URDF, as well as allowing you to set maximum and minimum limits for velocity " + "and acceleration than those contained in your URDF. This information is used by our trajectory " + "filtering system to assign reasonable velocities and timing for the trajectory before it is " + "passed to the robot's controllers."; + } + + bool hasChanges() const override + { + return false; // Can't be changed just yet + } + + bool writeYaml(YAML::Emitter& emitter) override; + + protected: + SRDFConfig& parent_; + }; + + class GeneratedCartesianLimits : public TemplatedGeneratedFile + { + public: + using TemplatedGeneratedFile::TemplatedGeneratedFile; + + std::filesystem::path getRelativePath() const override + { + return CARTESIAN_LIMITS_FILE; + } + + std::filesystem::path getTemplatePath() const override + { + return getSharePath("moveit_setup_framework") / "templates" / CARTESIAN_LIMITS_FILE; + } + + std::string getDescription() const override + { + return "Cartesian velocity for planning in the workspace." + "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian " + "planning requests scaled by the velocity scaling factor of an individual planning request."; + } + + bool hasChanges() const override + { + return false; + } + }; + + void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + std::vector& files) override + { + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + files.push_back(std::make_shared(package_path, last_gen_time)); + } + + void collectVariables(std::vector& variables) override; + + bool write(const std::filesystem::path& path) + { + return srdf_.writeSRDF(path); + } + + std::filesystem::path getPath() const + { + return srdf_path_; + } + + unsigned long getChangeMask() const + { + return changes_; + } + +protected: + void getRelativePath(); + void loadURDFModel(); + + // ****************************************************************************************** + // SRDF Data + // ****************************************************************************************** + /// Full file-system path to srdf + std::filesystem::path srdf_path_; + + /// Path relative to loaded configuration package + std::filesystem::path srdf_pkg_relative_path_; + + /// SRDF Data and Writer + srdf::SRDFWriter srdf_; + + std::shared_ptr urdf_model_; + + moveit::core::RobotModelPtr robot_model_; + + /// Shared planning scene + planning_scene::PlanningScenePtr planning_scene_; + + // bitfield of changes (composed of InformationFields) + unsigned long changes_; +}; +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp new file mode 100644 index 0000000000..3402cf59a4 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.hpp @@ -0,0 +1,130 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, Inc. + * 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. + *********************************************************************/ + +#pragma once + +#include +#include // for testing a valid urdf is loaded + +namespace moveit_setup +{ +class URDFConfig : public SetupConfig +{ +public: + URDFConfig() + { + urdf_model_ = std::make_shared(); + } + + void onInit() override; + + void loadPrevious(const std::filesystem::path& package_path, const YAML::Node& node) override; + YAML::Node saveToYaml() const override; + + /// Load URDF File + void loadFromPath(const std::filesystem::path& urdf_file_path, const std::string& xacro_args = ""); + void loadFromPath(const std::filesystem::path& urdf_file_path, const std::vector& xacro_args); + void loadFromPackage(const std::filesystem::path& package_name, const std::filesystem::path& relative_path, + const std::string& xacro_args = ""); + + const urdf::Model& getModel() const + { + return *urdf_model_; + } + + const std::shared_ptr& getModelPtr() const + { + return urdf_model_; + } + + std::string getURDFPackageName() const + { + return urdf_pkg_name_; + } + + std::string getURDFContents() const + { + return urdf_string_; + } + + std::filesystem::path getURDFPath() const + { + return urdf_path_; + } + + std::string getXacroArgs() const + { + return xacro_args_; + } + + bool isConfigured() const override; + + bool isXacroFile() const; + + void collectDependencies(std::set& packages) const override; + + void collectVariables(std::vector& variables) override; + + std::string getRobotName() const + { + return urdf_model_->getName(); + } + +protected: + void setPackageName(); + void load(); + + /// Full file-system path to urdf + std::filesystem::path urdf_path_; + + /// Name of package containing urdf (note: this may be empty b/c user may not have urdf in pkg) + std::string urdf_pkg_name_; + + /// Path relative to urdf package (note: this may be same as urdf_path_) + std::filesystem::path urdf_pkg_relative_path_; + + /// Flag indicating whether the URDF was loaded from .xacro format + bool urdf_from_xacro_; + + /// xacro arguments in two different formats + std::string xacro_args_; + std::vector xacro_args_vec_; + + /// URDF robot model + std::shared_ptr urdf_model_; + + /// URDF robot model string + std::string urdf_string_; +}; +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.hpp new file mode 100644 index 0000000000..1be13b186c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.hpp @@ -0,0 +1,132 @@ +/********************************************************************* + * 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 +#include +#include + +#pragma once + +namespace moveit_setup +{ +MOVEIT_CLASS_FORWARD(DataWarehouse); // Defines DataWarehousePtr, ConstPtr, WeakPtr... etc + +/** + * @brief Container for all of the `SetupConfig` object singletons + */ +class DataWarehouse : public std::enable_shared_from_this +{ +public: + DataWarehouse(const rclcpp::Node::SharedPtr& parent_node); + + /** + * @brief Get the singleton for a given config name and class + * + * @param config_name The name of the SetupConfig + * @param config_class The string used to load the class via pluginlib. If empty, the registered type is used. + * @throws std::runtime_error If the name is not registered to a type + * @returns Shared pointer to the generic SetupConfig object + */ + SetupConfigPtr get(const std::string& config_name, std::string config_class = ""); + + /** + * @brief Get the specific singleton for a given config name and class + * + * Unlike the non-templated version of this method, this returns + * the SetupConfig pointer cast to the specific config type. + * + * @param config_name The name of the SetupConfig + * @param config_class The string used to load the class via pluginlib. If empty, the registered type is used. + * @throws std::runtime_error If the name is not registered to a type + * @returns Shared pointer to the specific SetupConfig object + */ + template + std::shared_ptr get(const std::string& config_name, const std::string& config_class = "") + { + return std::static_pointer_cast(get(config_name, config_class)); + } + + /** + * @brief Get all of the registered configs that match the given config_class + * + * @param config_class The string representing the class name + * @returns Map of shared pointers from config names to singleton config + */ + template + std::unordered_map> getAll() + { + std::unordered_map> matches; + for (const auto& pair : registered_types_) + { + SetupConfigPtr uncast_ptr = get(pair.first); + std::shared_ptr ptr = std::dynamic_pointer_cast(uncast_ptr); + if (!ptr) + { + continue; + } + + matches[pair.first] = ptr; + } + return matches; + } + + /** + * @brief Associates a class_name with the given name. Makes calls to get more succinct. + */ + void registerType(const std::string& config_name, const std::string& config_class); + + /** + * @brief Returns a list of the SetupConfig for which isConfigured is true + */ + std::vector getConfigured(); + + /** + * @brief Returns a list of config_names that have registered types associated with them + */ + std::vector getRegisteredNames() const; + + /// Is this application in debug mode? + bool debug{ false }; + +protected: + rclcpp::Node::SharedPtr parent_node_; + pluginlib::ClassLoader config_loader_; + std::unordered_map configs_; // mapping from name to config + std::unordered_map registered_types_; // mapping from name to config class type +}; +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp new file mode 100644 index 0000000000..0798d21297 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp @@ -0,0 +1,158 @@ +/********************************************************************* + * 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 +#include +#include +#include +#include + +namespace moveit_setup +{ +/** + * @brief Status associated with each GeneratedFile + */ +enum class FileStatus +{ + NEW, // The file does not exist in the configuration package + UNCHANGED, // The file exists and would be the same as the generated file + CHANGED, // The file exists, but a new version will be written + EXTERNALLY_MODIFIED, // The file exists and was externally modified + CONFLICTED // The file exists, was externally modified and there are changes to be written +}; + +MOVEIT_CLASS_FORWARD(GeneratedFile); // Defines GeneratedFilePtr, ConstPtr, WeakPtr... etc + +/** + * @brief Container for the logic for a single file to appear in MoveIt configuration package. + */ +class GeneratedFile : public std::enable_shared_from_this +{ +public: + GeneratedFile(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time) + : package_path_(package_path), last_gen_time_(last_gen_time) + { + } + + /** + * @brief Returns the path relative to the configuration package root + */ + virtual std::filesystem::path getRelativePath() const = 0; + + /** + * @brief Returns an English description of this file's purpose. + */ + virtual std::string getDescription() const = 0; + + /** + * @brief Returns true if this file will have changes when it is written to file + */ + virtual bool hasChanges() const = 0; + + /** + * @brief Writes the file to disk + */ + virtual bool write() = 0; + + /** + * @brief Returns the fully qualified path to this file + */ + std::filesystem::path getPath() const + { + return package_path_ / getRelativePath(); + } + + FileStatus getStatus() const + { + std::filesystem::path full_path = getPath(); + if (!std::filesystem::is_regular_file(full_path) || last_gen_time_ == GeneratedTime()) + { + return FileStatus::NEW; + } + GeneratedTime mod_time = std::filesystem::last_write_time(full_path); + if (mod_time > last_gen_time_ + TIME_MOD_TOLERANCE || mod_time < last_gen_time_ - TIME_MOD_TOLERANCE) + { + return hasChanges() ? FileStatus::CONFLICTED : FileStatus::EXTERNALLY_MODIFIED; + } + else + { + return hasChanges() ? FileStatus::CHANGED : FileStatus::UNCHANGED; + } + } + +protected: + static constexpr GeneratedTime::duration TIME_MOD_TOLERANCE = std::chrono::seconds(10); + + std::filesystem::path package_path_; + const GeneratedTime& last_gen_time_; +}; + +class YamlGeneratedFile : public GeneratedFile +{ +public: + using GeneratedFile::GeneratedFile; + + bool write() override + { + YAML::Emitter emitter; + bool ret = writeYaml(emitter); + if (!ret) + { + return false; + } + + std::filesystem::path file_path = getPath(); + createParentFolders(file_path); + std::ofstream output_stream(file_path, std::ios_base::trunc); + if (!output_stream.good()) + { + return false; + } + + output_stream << emitter.c_str(); + output_stream.close(); + + return true; // file created successfully + } + + virtual bool writeYaml(YAML::Emitter& emitter) = 0; +}; + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_time.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_time.hpp new file mode 100644 index 0000000000..fa2d078b0b --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_time.hpp @@ -0,0 +1,115 @@ +/********************************************************************* + * 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 +{ +/** + * This file contains a bunch of conversions for converting + * std::filesystem::file_time_type (a.k.a. the return type from std::filesystem::last_write_time) + * to other useful types and back + */ +using GeneratedTime = std::filesystem::file_time_type; + +/** + * @brief Convert a time_point from an arbitrary clock to GeneratedTime + */ +template +inline GeneratedTime convertTime(const typename CLOCK::time_point& t0) +{ + return std::chrono::time_point_cast(t0 - CLOCK::now() + GeneratedTime::clock::now()); +} + +/** + * @brief Convert GeneratedTime to a time_point from an arbitrary clock + */ +template +inline typename CLOCK::time_point convertTime(const GeneratedTime& t0) +{ + return std::chrono::time_point_cast(t0 - GeneratedTime::clock::now() + CLOCK::now()); +} + +/** + * @brief Convert an integral epoch from an arbitrary clock to GeneratedTime + */ +template +inline GeneratedTime convertTime(long int epoch) +{ + auto t0 = CLOCK::from_time_t(epoch); + return convertTime(t0); +} + +/** + * @brief Convert an integral epoch to GeneratedTime (using system_clock) + */ +inline GeneratedTime fromEpoch(long int epoch) +{ + return convertTime(epoch); +} + +/** + * @brief Convert a GeneratedTime to a std::time_t (using system_clock) + */ +template +inline std::time_t toStandardTime(const GeneratedTime& t) +{ + return CLOCK::to_time_t(convertTime(t)); +} + +/** + * @brief Convert a GeneratedTime to an integral epoch (using system_clock) + */ +inline long int toEpoch(const GeneratedTime& t) +{ + return toStandardTime(t); +} + +/** + * @brief Convert a GeneratedTime to a std::string (using system_clock) + */ +inline std::string toString(const GeneratedTime& t) +{ + std::time_t tt = toStandardTime(t); + char buff[20]; + strftime(buff, 20, "%Y-%m-%d %H:%M:%S", localtime(&tt)); + return std::string(buff); +} + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.hpp similarity index 90% rename from moveit_setup_assistant/src/widgets/double_list_widget.h rename to moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.hpp index 61fffc8ec2..755e877f43 100644 --- a/moveit_setup_assistant/src/widgets/double_list_widget.h +++ 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 { 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 diff --git a/moveit_setup_assistant/src/widgets/header_widget.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/helper_widgets.hpp similarity index 98% rename from moveit_setup_assistant/src/widgets/header_widget.h rename to moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/helper_widgets.hpp index f78e5ceace..36cb0a96f0 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.h +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/helper_widgets.hpp @@ -41,7 +41,7 @@ class QLabel; class QLineEdit; -namespace moveit_setup_assistant +namespace moveit_setup { // ****************************************************************************************** // ****************************************************************************************** @@ -118,4 +118,4 @@ class LoadPathArgsWidget : public LoadPathWidget void setArgs(const QString& args); void setArgsEnabled(bool enabled = true); }; -} // namespace moveit_setup_assistant +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.hpp new file mode 100644 index 0000000000..1de5e3c022 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.hpp @@ -0,0 +1,141 @@ +/********************************************************************* + * 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 +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_setup +{ +// Used for loading kinematic model +static const std::string ROBOT_DESCRIPTION = "robot_description"; +static const std::string MOVEIT_ROBOT_STATE = "moveit_robot_state"; + +class RVizPanel : public QWidget, public rviz_common::WindowManagerInterface +{ + Q_OBJECT +public: + RVizPanel(QWidget* parent, rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr node_abstraction, + DataWarehousePtr config_data); + ~RVizPanel() override; + + bool isReadyForInitialization() const + { + return rviz_render_panel_ == nullptr && getRobotModel() != nullptr; + } + void initialize(); + void updateFixedFrame(); + + QWidget* getParentWindow() override + { + return parent_; + } + + rviz_common::PanelDockWidget* addPane(const QString& /*name*/, QWidget* /*pane*/, + Qt::DockWidgetArea /*area*/ = Qt::LeftDockWidgetArea, + bool /*floating*/ = true) override + { + // Stub for now...just to define the WindowManagerInterface methods + return nullptr; + } + + void setStatus(const QString& /*message*/) override + { + // Stub for now...just to define the WindowManagerInterface methods + } + +public Q_SLOTS: + /** + * Highlight a link of the robot + * + * @param link_name name of link to highlight + */ + void highlightLink(const std::string& link_name, const QColor& color) + { + Q_EMIT highlightLinkSignal(link_name, color); + } + + /** + * Highlight a robot group + */ + void highlightGroup(const std::string& group_name) + { + Q_EMIT highlightGroupSignal(group_name); + } + + /** + * Unhighlight all links of a robot + */ + void unhighlightAll() + { + Q_EMIT unhighlightAllSignal(); + } + +Q_SIGNALS: + // Protected event handlers + void highlightLinkSignal(const std::string& link_name, const QColor& color); + void highlightGroupSignal(const std::string& group_name); + void unhighlightAllSignal(); + +protected Q_SLOTS: + void highlightLinkEvent(const std::string& link_name, const QColor& color); + void highlightGroupEvent(const std::string& group_name); + void unhighlightAllEvent(); + +protected: + moveit::core::RobotModelPtr getRobotModel() const; + + QWidget* parent_; + rviz_common::RenderPanel* rviz_render_panel_{ nullptr }; + rviz_common::VisualizationManager* rviz_manager_{ nullptr }; + moveit_rviz_plugin::RobotStateDisplay* robot_state_display_{ nullptr }; + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr node_abstraction_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr logger_; + + DataWarehousePtr config_data_; +}; +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/setup_step_widget.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/setup_step_widget.hpp new file mode 100644 index 0000000000..f32e7b5ad3 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/setup_step_widget.hpp @@ -0,0 +1,118 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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: Dave Coleman */ + +#pragma once +#include +#include +#include +#include + +namespace moveit_setup +{ +/** + * @brief The GUI code for one SetupStep + */ +class SetupStepWidget : public QWidget +{ + Q_OBJECT +public: + /** + * @brief Called after construction to initialize the step + * @param parent_node Shared pointer to the parent node + * @param parent_widget Pointer to the parent gui element + * @param rviz_panel Pointer to the shared rviz panel + * @param config_data All the data + */ + void initialize(const rclcpp::Node::SharedPtr& parent_node, QWidget* parent_widget, RVizPanel* rviz_panel, + const DataWarehousePtr& config_data) + { + getSetupStep().initialize(parent_node, config_data); + setParent(parent_widget); + rviz_panel_ = rviz_panel; + debug_ = config_data->debug; + onInit(); + } + + virtual void onInit() + { + } + + /** + * @brief function called when widget is activated, allows to update/initialize GUI + */ + virtual void focusGiven() + { + } + + /** + * @brief function called when widget loses focus, although switching away can be rejected + * + * @return If the widget should not be switched away from, return false + */ + virtual bool focusLost() + { + return true; // accept switching by default + } + + /** + * @brief Return a reference to the SetupStep object + */ + virtual SetupStep& getSetupStep() = 0; + + bool isReady() + { + return getSetupStep().isReady(); + } + + // ****************************************************************************************** + // Emitted Signal Functions + // ****************************************************************************************** + +Q_SIGNALS: + /// When the underlying data has been updated (which can cause other steps to become "Ready") + void dataUpdated(); + + /// When this signal is received, the GUI should attempt to advance to the next step. + void advanceRequest(); + + /// Event for when the current screen is in modal view. Disables the left navigation + void setModalMode(bool isModal); + +protected: + RVizPanel* rviz_panel_; + bool debug_; +}; +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/tools/xml_syntax_highlighter.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.hpp similarity index 97% rename from moveit_setup_assistant/src/tools/xml_syntax_highlighter.h rename to moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.hpp index cd48b70238..0ec322cc2e 100644 --- a/moveit_setup_assistant/src/tools/xml_syntax_highlighter.h +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.hpp @@ -40,6 +40,8 @@ #include #include +namespace moveit_setup +{ /** XML SyntaxHighlighter allowing nested highlighting of XML tags */ class XmlSyntaxHighlighter : public QSyntaxHighlighter { @@ -63,3 +65,5 @@ class XmlSyntaxHighlighter : public QSyntaxHighlighter Rules::const_iterator highlight(Rules::const_iterator active, QStringRef text, int start, bool search_end, int& end); }; + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.hpp new file mode 100644 index 0000000000..79e05ca37f --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.hpp @@ -0,0 +1,94 @@ +/********************************************************************* + * 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 +{ +/** + * @brief Contains all of the non-GUI code necessary for doing one "screen" worth of setup. + */ +class SetupStep +{ +public: + /** + * @brief Called after construction to initialize the step + * @param parent_node Shared pointer to the parent node + */ + void initialize(const rclcpp::Node::SharedPtr& parent_node, const DataWarehousePtr& config_data) + { + parent_node_ = parent_node; + config_data_ = config_data; + logger_ = std::make_shared(parent_node->get_logger().get_child(getName())); + onInit(); + } + + /** + * @brief Overridable initialization method + */ + virtual void onInit() + { + } + + /** + * @brief Returns the name of the setup step + */ + virtual std::string getName() const = 0; + + /** + * @brief Return true if the data necessary to proceed with this step has been configured + */ + virtual bool isReady() const + { + return true; + } + + /** + * @brief Makes a namespaced logger for this step available to the widget + */ + const rclcpp::Logger& getLogger() const + { + return *logger_; + } + +protected: + DataWarehousePtr config_data_; + rclcpp::Node::SharedPtr parent_node_; + std::shared_ptr logger_; +}; +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp new file mode 100644 index 0000000000..fb90a75703 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp @@ -0,0 +1,75 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, Inc. + * 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 +{ +/** + * @brief Simple Key/value pair for templates + */ +struct TemplateVariable +{ + TemplateVariable(const std::string& key, const std::string& value) : key(key), value(value) + { + } + + std::string key; + std::string value; +}; + +/** + * @brief Specialization of GeneratedFile for generating a text file from a template + */ +class TemplatedGeneratedFile : public GeneratedFile +{ +public: + // Use same constructor + using GeneratedFile::GeneratedFile; + + /** + * @brief Returns the full path to the template file + */ + virtual std::filesystem::path getTemplatePath() const = 0; + + bool write() override; + + static std::vector variables_; +}; + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp new file mode 100644 index 0000000000..3fc15b301f --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp @@ -0,0 +1,130 @@ +/********************************************************************* + * 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 +#include + +namespace moveit_setup +{ +/** + * @brief Return a path for the given package's share folder + */ +inline std::filesystem::path getSharePath(const std::string& package_name) +{ + return std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)); +} + +/** + * @brief Create folders (recursively) + * @return false if the path was not a directory and was unable to create the directory(s) + */ +inline bool createFolders(const std::filesystem::path& output_path) +{ + return std::filesystem::is_directory(output_path) || std::filesystem::create_directories(output_path); +} + +/** + * @brief Create parent folders (recursively) + * @return false if the path was not a directory and was unable to create the directory(s) + */ +inline bool createParentFolders(const std::filesystem::path& file_path) +{ + return createFolders(file_path.parent_path()); +} + +/** + * determine the package name containing a given file path + * @param path to a file + * @param package_name holds the ros package name if found + * @param relative_filepath holds the relative path of the file to the package root + * @return whether the file belongs to a known package + */ +bool extractPackageNameFromPath(const std::filesystem::path& path, std::string& package_name, + std::filesystem::path& relative_filepath); + +/** + * @brief Simple structure for easy xml creation + */ +struct XMLAttribute +{ + const char* name; + const char* value; + bool required = false; +}; + +/** + * @brief Insert a new XML element with a given tag, attributes, and text + * + * If a corresponding element already exists (and has required attribute values), it is just reused. + * + * @param doc The XMLDocument, used for creating new elements + * @param element The tag inside of which the new tag should be inserted + * @param tag The name of the tag + * @param attributes Attribute name/value pairs to be created/overwritten + * @param text If not null, text value to insert inside the new tag + * @returns The new or existing element + */ +tinyxml2::XMLElement* uniqueInsert(tinyxml2::XMLDocument& doc, tinyxml2::XMLElement& element, const char* tag, + const std::vector& attributes = {}, const char* text = nullptr); + +// Formerly "parse" +template +inline bool getYamlProperty(const YAML::Node& node, const std::string& key, T& storage, const T& default_value = T()) +{ + const YAML::Node& n = node[key]; + bool valid = n.IsDefined(); + storage = valid ? n.as() : default_value; + return valid; +} + +inline bool getYamlProperty(const YAML::Node& node, const std::string& key, std::filesystem::path& storage, + const std::string& default_value = "") +{ + std::string storage_s; + bool ret = getYamlProperty(node, key, storage_s, default_value); + if (ret) + { + storage = storage_s; + } + return ret; +} + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/moveit_setup_framework_plugins.xml b/moveit_setup_assistant/moveit_setup_framework/moveit_setup_framework_plugins.xml new file mode 100644 index 0000000000..4749ba8a8a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/moveit_setup_framework_plugins.xml @@ -0,0 +1,11 @@ + + + Data for the URDF + + + Data for the SRDF + + + Data for general settings for the MoveIt config package + + diff --git a/moveit_setup_assistant/moveit_setup_framework/package.xml b/moveit_setup_assistant/moveit_setup_framework/package.xml new file mode 100644 index 0000000000..beca642695 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/package.xml @@ -0,0 +1,32 @@ + + + + moveit_setup_framework + 2.5.1 + C++ Interface for defining setup steps for MoveIt Setup Assistant + David V. Lu!! + BSD + David V. Lu!! + + ament_cmake + ament_index_cpp + moveit_common + moveit_core + moveit_ros_planning + moveit_ros_visualization + pluginlib + rclcpp + rviz_common + rviz_rendering + srdfdom + urdf + + ament_lint_auto + ament_clang_format + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/moveit_setup_assistant/moveit_setup_framework/src/data_warehouse.cpp b/moveit_setup_assistant/moveit_setup_framework/src/data_warehouse.cpp new file mode 100644 index 0000000000..67da780f9e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/src/data_warehouse.cpp @@ -0,0 +1,101 @@ +/********************************************************************* + * 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 +#include + +namespace moveit_setup +{ +DataWarehouse::DataWarehouse(const rclcpp::Node::SharedPtr& parent_node) + : parent_node_(parent_node), config_loader_("moveit_setup_framework", "moveit_setup::SetupConfig") +{ + registerType("urdf", "moveit_setup::URDFConfig"); + registerType("srdf", "moveit_setup::SRDFConfig"); + registerType("package_settings", "moveit_setup::PackageSettingsConfig"); +} + +SetupConfigPtr DataWarehouse::get(const std::string& config_name, std::string config_class) +{ + if (config_class.empty()) + { + auto it = registered_types_.find(config_name); + if (it == registered_types_.end()) + { + throw std::runtime_error(config_name + " does not have a registered type in the data warehouse"); + } + config_class = it->second; + } + auto it = configs_.find(config_name); + if (it != configs_.end()) + { + return it->second; + } + auto config = config_loader_.createSharedInstance(config_class); + config->initialize(shared_from_this(), parent_node_, config_name); + configs_[config_name] = config; + return config; +} + +void DataWarehouse::registerType(const std::string& config_name, const std::string& config_class) +{ + registered_types_[config_name] = config_class; +} + +std::vector DataWarehouse::getConfigured() +{ + std::vector configs; + for (const auto& kv : configs_) + { + if (kv.second->isConfigured()) + { + configs.push_back(kv.second); + } + } + return configs; +} + +std::vector DataWarehouse::getRegisteredNames() const +{ + std::vector names; + for (const auto& type_pair : registered_types_) + { + names.push_back(type_pair.first); + } + return names; +} +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/double_list_widget.cpp b/moveit_setup_assistant/moveit_setup_framework/src/double_list_widget.cpp similarity index 95% rename from moveit_setup_assistant/src/widgets/double_list_widget.cpp rename to moveit_setup_assistant/moveit_setup_framework/src/double_list_widget.cpp index 583e9e2ed4..2288090be0 100644 --- a/moveit_setup_assistant/src/widgets/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 { // ****************************************************************************************** // // ****************************************************************************************** -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,15 @@ void DoubleListWidget::previewSelected(const QList& selected) Q_EMIT(previewSelected(selected_vector)); } -} // namespace moveit_setup_assistant +std::vector DoubleListWidget::getSelectedValues() const +{ + std::vector values; + values.reserve(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 diff --git a/moveit_setup_assistant/src/widgets/header_widget.cpp b/moveit_setup_assistant/moveit_setup_framework/src/helper_widgets.cpp similarity index 98% rename from moveit_setup_assistant/src/widgets/header_widget.cpp rename to moveit_setup_assistant/moveit_setup_framework/src/helper_widgets.cpp index 0d00303db4..2d98f71950 100644 --- a/moveit_setup_assistant/src/widgets/header_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/helper_widgets.cpp @@ -34,7 +34,7 @@ /* Author: Dave Coleman */ -#include "header_widget.h" +#include "moveit_setup_framework/qt/helper_widgets.hpp" #include #include #include @@ -42,7 +42,7 @@ #include #include -namespace moveit_setup_assistant +namespace moveit_setup { // ****************************************************************************************** // ****************************************************************************************** @@ -230,4 +230,4 @@ void LoadPathArgsWidget::setArgsEnabled(bool enabled) { args_->setEnabled(enabled); } -} // namespace moveit_setup_assistant +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/src/package_settings_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/package_settings_config.cpp new file mode 100644 index 0000000000..f68f8aaabb --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/src/package_settings_config.cpp @@ -0,0 +1,224 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, Inc. + * 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. + *********************************************************************/ +#include +#include +#include + +namespace moveit_setup +{ +const std::unordered_map + BACKWARDS_KEY_LOOKUP({ { "urdf", "URDF" }, { "srdf", "SRDF" }, { "package_settings", "CONFIG" } }); + +const std::regex MAIL_REGEX("\\b[A-Z0-9._%+-]+@[A-Z0-9.-]+\\.[A-Z]{2,63}\\b", std::regex::icase); + +void PackageSettingsConfig::loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node) +{ + getYamlProperty(node, "author_name", author_name_); + getYamlProperty(node, "author_email", author_email_); + unsigned long int timestamp_i; + getYamlProperty(node, "generated_timestamp", timestamp_i); + config_pkg_generated_timestamp_ = fromEpoch(timestamp_i); +} + +YAML::Node PackageSettingsConfig::saveToYaml() const +{ + YAML::Node node; + node["author_name"] = author_name_; + node["author_email"] = author_email_; + node["generated_timestamp"] = toEpoch(config_pkg_generated_timestamp_); + return node; +} + +void PackageSettingsConfig::setPackagePath(const std::filesystem::path& package_path) +{ + config_pkg_path_ = package_path; +} + +void PackageSettingsConfig::setPackageName(const std::string& package_name) +{ + new_package_name_ = package_name; +} + +void PackageSettingsConfig::loadExisting(const std::string& package_path_or_name) +{ + if (package_path_or_name.empty()) + { + throw std::runtime_error("Please specify a configuration package path to load."); + } + // Check if it is a path that exists + if (std::filesystem::is_directory(package_path_or_name)) + { + // they inputted a full path + setPackagePath(package_path_or_name); + } + else + { + // Determine the path from a name + /* TODO(dlu): Ideally, the package path is in source so that when we write back to it, + * the changes will be reflected and then we can check them into git. + * However, there's no easy way to determine the source folder from C++. + * You could run colcon list -p --packages-select $PACKAGE_NAME but the + * results are dependent on what folder you are in and opening an external + * process is messy. For now, we just use the share path and rely on the user + * to write back to the proper directory in the ConfigurationFiles step + */ + auto share_dir = getSharePath(package_path_or_name); + + // check that the folder exists + if (!std::filesystem::is_directory(share_dir)) + { + throw std::runtime_error("The specified path is not a directory or is not accessible."); + } + + setPackagePath(share_dir); + } + + // Load the package name from the package.xml + std::filesystem::path relative_path; // we don't use this output value + extractPackageNameFromPath(config_pkg_path_, new_package_name_, relative_path); + + // Load Config Yaml + std::filesystem::path config_path = config_pkg_path_ / SETUP_ASSISTANT_FILE; + if (!std::filesystem::is_regular_file(config_path)) + { + throw std::runtime_error("The chosen package location exists but was not created using MoveIt Setup Assistant. " + "If this is a mistake, provide the missing file: " + + config_path.string()); + } + + std::ifstream input_stream(config_path); + if (!input_stream.good()) + { + throw std::runtime_error("Unable to open file for reading " + config_path.string()); + } + + // Begin parsing + try + { + const YAML::Node& doc = YAML::Load(input_stream); + // Get title node + const YAML::Node& title_node = doc["moveit_setup_assistant_config"]; + + for (const std::string& name : config_data_->getRegisteredNames()) + { + std::string yaml_key = name; + /* Generally speaking, we use each config's name as the key to read the yaml from + * + * However, for backwards compatibility, we also allow for the three legacy keys, which we translate here. + * If the name is found in BACKWARDS_KEY_LOOKUP and the legacy key is present in the yaml dictionary, + * we load using the legacy key instead. + */ + auto backwards_match = BACKWARDS_KEY_LOOKUP.find(name); + if (backwards_match != BACKWARDS_KEY_LOOKUP.end() && title_node[backwards_match->second].IsDefined()) + { + yaml_key = backwards_match->second; + } + + // We load the previous regardless of whether the title_node[yaml_key] is actually defined + config_data_->get(name)->loadPrevious(config_pkg_path_, title_node[yaml_key]); + } + } + catch (YAML::ParserException& e) // Catch errors, translate to runtime_error + { + throw std::runtime_error("Error parsing " + config_path.string() + ": " + e.what()); + } +} + +bool PackageSettingsConfig::GeneratedSettings::writeYaml(YAML::Emitter& emitter) +{ + emitter << YAML::BeginMap; + // Output every available planner --------------------------------------------------- + emitter << YAML::Key << "moveit_setup_assistant_config"; + emitter << YAML::Value << YAML::BeginMap; + + for (const auto& config : parent_.config_data_->getConfigured()) + { + YAML::Node node = config->saveToYaml(); + if (!node.size()) + { + continue; + } + emitter << YAML::Key << config->getName(); + emitter << YAML::Value << node; + } + emitter << YAML::EndMap; + return true; +} + +void PackageSettingsConfig::loadDependencies() +{ + package_dependencies_.clear(); + for (const auto& config : config_data_->getConfigured()) + { + config->collectDependencies(package_dependencies_); + } +} + +void PackageSettingsConfig::collectVariables(std::vector& variables) +{ + variables.push_back(TemplateVariable("GENERATED_PACKAGE_NAME", new_package_name_)); + + // TODO: Add new variables for other fields existing in the package.xml + // i.e. read the version so that the version is not overwritten + variables.push_back(TemplateVariable("AUTHOR_NAME", author_name_)); + variables.push_back(TemplateVariable("AUTHOR_EMAIL", author_email_)); + + std::stringstream deps; + for (const auto& dependency : package_dependencies_) + { + deps << " " << dependency << "\n"; + } + variables.push_back(TemplateVariable("OTHER_DEPENDENCIES", deps.str())); +} + +bool PackageSettingsConfig::hasValidName() const +{ + // Make sure there is something that isn't just whitespace + return author_name_.find_first_not_of(' ') != std::string::npos; +} + +bool PackageSettingsConfig::hasValidEmail() const +{ + return std::regex_match(author_email_, MAIL_REGEX); +} + +void PackageSettingsConfig::setGenerationTime() +{ + config_pkg_generated_timestamp_ = GeneratedTime::clock::now(); +} + +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::PackageSettingsConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/moveit_setup_framework/src/rviz_panel.cpp b/moveit_setup_assistant/moveit_setup_framework/src/rviz_panel.cpp new file mode 100644 index 0000000000..e1f5298ffd --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/src/rviz_panel.cpp @@ -0,0 +1,220 @@ +/********************************************************************* + * 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 +#include +#include "rviz_rendering/render_window.hpp" + +namespace moveit_setup +{ +RVizPanel::RVizPanel(QWidget* parent, rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr node_abstraction, + DataWarehousePtr config_data) + : QWidget(parent) + , parent_(parent) + , node_abstraction_(node_abstraction) + , node_(node_abstraction_.lock()->get_raw_node()) + , config_data_(config_data) +{ + logger_ = std::make_shared(node_->get_logger().get_child("RVizPanel")); + + connect(this, SIGNAL(highlightLinkSignal(const std::string&, const QColor&)), this, + SLOT(highlightLinkEvent(const std::string&, const QColor&))); + connect(this, SIGNAL(highlightGroupSignal(const std::string&)), this, SLOT(highlightGroupEvent(const std::string&))); + connect(this, SIGNAL(unhighlightAllSignal()), this, SLOT(unhighlightAllEvent())); +} + +void RVizPanel::initialize() +{ + // Create rviz frame + rviz_render_panel_ = new rviz_common::RenderPanel(); + rviz_render_panel_->setMinimumWidth(200); + rviz_render_panel_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + + QApplication::processEvents(); + rviz_render_panel_->getRenderWindow()->initialize(); + + rviz_manager_ = + new rviz_common::VisualizationManager(rviz_render_panel_, node_abstraction_, this, node_->get_clock()); + rviz_render_panel_->initialize(rviz_manager_); + rviz_manager_->initialize(); + rviz_manager_->startUpdate(); + + // Create the MoveIt Rviz Plugin and attach to display + robot_state_display_ = new moveit_rviz_plugin::RobotStateDisplay(); + robot_state_display_->setName("Robot State"); + + rviz_manager_->addDisplay(robot_state_display_, true); + + // Set the fixed and target frame + updateFixedFrame(); + + // Set the topic on which the moveit_msgs::msg::PlanningScene messages are received + robot_state_display_->subProp("Robot State Topic")->setValue(QString::fromStdString(MOVEIT_ROBOT_STATE)); + + // Set robot description + robot_state_display_->subProp("Robot Description")->setValue(QString::fromStdString(ROBOT_DESCRIPTION)); + robot_state_display_->setVisible(true); + // Zoom into robot + rviz_common::ViewController* view = rviz_manager_->getViewManager()->getCurrent(); + view->subProp("Distance")->setValue(4.0f); + + // Add Rviz to Planning Groups Widget + QVBoxLayout* rviz_layout = new QVBoxLayout(); + rviz_layout->addWidget(rviz_render_panel_); + setLayout(rviz_layout); + + // visual / collision buttons + auto btn_layout = new QHBoxLayout(); + rviz_layout->addLayout(btn_layout); + + QCheckBox* btn; + btn_layout->addWidget(btn = new QCheckBox("visual"), 0); + btn->setChecked(true); + connect(btn, &QCheckBox::toggled, + [this](bool checked) { robot_state_display_->subProp("Visual Enabled")->setValue(checked); }); + + btn_layout->addWidget(btn = new QCheckBox("collision"), 1); + btn->setChecked(false); + connect(btn, &QCheckBox::toggled, + [this](bool checked) { robot_state_display_->subProp("Collision Enabled")->setValue(checked); }); +} +RVizPanel::~RVizPanel() +{ + if (rviz_manager_ != nullptr) + rviz_manager_->removeAllDisplays(); + if (rviz_render_panel_ != nullptr) + delete rviz_render_panel_; + if (rviz_manager_ != nullptr) + delete rviz_manager_; +} + +moveit::core::RobotModelPtr RVizPanel::getRobotModel() const +{ + auto urdf = config_data_->get("urdf"); + + if (!urdf->isConfigured()) + { + return nullptr; + } + + auto srdf = config_data_->get("srdf"); + + return srdf->getRobotModel(); +} + +void RVizPanel::updateFixedFrame() +{ + auto rm = getRobotModel(); + if (rm && rviz_manager_ && robot_state_display_) + { + std::string frame = rm->getModelFrame(); + rviz_manager_->setFixedFrame(QString::fromStdString(frame)); + robot_state_display_->reset(); + robot_state_display_->setVisible(true); + } +} + +// ****************************************************************************************** +// Highlight a robot link +// ****************************************************************************************** +void RVizPanel::highlightLinkEvent(const std::string& link_name, const QColor& color) +{ + auto rm = getRobotModel(); + if (!rm) + return; + const moveit::core::LinkModel* lm = rm->getLinkModel(link_name); + if (!lm->getShapes().empty()) // skip links with no geometry + robot_state_display_->setLinkColor(link_name, color); +} + +// ****************************************************************************************** +// Highlight a robot group +// ****************************************************************************************** +void RVizPanel::highlightGroupEvent(const std::string& group_name) +{ + auto rm = getRobotModel(); + if (!rm) + return; + // Highlight the selected planning group by looping through the links + if (!rm->hasJointModelGroup(group_name)) + return; + + const moveit::core::JointModelGroup* joint_model_group = rm->getJointModelGroup(group_name); + if (joint_model_group) + { + const std::vector& link_models = joint_model_group->getLinkModels(); + // Iterate through the links + for (std::vector::const_iterator link_it = link_models.begin(); + link_it < link_models.end(); ++link_it) + highlightLink((*link_it)->getName(), QColor(255, 0, 0)); + } +} + +// ****************************************************************************************** +// Unhighlight all robot links +// ****************************************************************************************** +void RVizPanel::unhighlightAllEvent() +{ + auto rm = getRobotModel(); + if (!rm) + return; + // Get the names of the all links robot + const std::vector& links = rm->getLinkModelNamesWithCollisionGeometry(); + + // Quit if no links found + if (links.empty()) + { + return; + } + + // check if rviz is ready + if (!rviz_manager_ || !robot_state_display_) + { + return; + } + + // Iterate through the links + for (std::vector::const_iterator link_it = links.begin(); link_it < links.end(); ++link_it) + { + if ((*link_it).empty()) + continue; + + robot_state_display_->unsetLinkColor(*link_it); + } +} + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp new file mode 100644 index 0000000000..9058aea1c0 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp @@ -0,0 +1,322 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, Inc. + * 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. + *********************************************************************/ +#include +#include +#include +#include + +namespace moveit_setup +{ +void SRDFConfig::onInit() +{ + parent_node_->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + changes_ = 0L; +} + +void SRDFConfig::loadPrevious(const std::filesystem::path& package_path, const YAML::Node& node) +{ + if (!getYamlProperty(node, "relative_path", srdf_pkg_relative_path_)) + { + throw std::runtime_error("cannot find relative_path property in SRDF"); + } + loadSRDFFile(package_path, srdf_pkg_relative_path_); +} + +YAML::Node SRDFConfig::saveToYaml() const +{ + YAML::Node node; + node["relative_path"] = srdf_pkg_relative_path_.string(); + return node; +} + +void SRDFConfig::loadURDFModel() +{ + if (urdf_model_ != nullptr) + { + return; + } + + auto urdf_config = config_data_->get("urdf"); + urdf_model_ = urdf_config->getModelPtr(); + srdf_.robot_name_ = urdf_model_->getName(); + parent_node_->set_parameter(rclcpp::Parameter("robot_description_semantic", srdf_.getSRDFString())); +} + +void SRDFConfig::loadSRDFFile(const std::filesystem::path& package_path, const std::filesystem::path& relative_path) +{ + srdf_pkg_relative_path_ = relative_path; + loadSRDFFile(std::filesystem::path(package_path) / relative_path); +} +// ****************************************************************************************** +// Load SRDF File to Parameter Server +// ****************************************************************************************** +void SRDFConfig::loadSRDFFile(const std::filesystem::path& srdf_file_path, const std::vector& xacro_args) +{ + srdf_path_ = srdf_file_path; + + loadURDFModel(); + + std::string srdf_string; + if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_path_, xacro_args)) + { + throw std::runtime_error("SRDF file not found: " + srdf_path_.string()); + } + + // Verify that file is in correct format / not an XACRO by loading into robot model + if (!srdf_.initString(*urdf_model_, srdf_string)) + { + throw std::runtime_error("SRDF file not a valid semantic robot description model."); + } + + // Set parameter + parent_node_->set_parameter(rclcpp::Parameter("robot_description_semantic", srdf_string)); + + updateRobotModel(); + + RCLCPP_INFO_STREAM(*logger_, "Robot semantic model successfully loaded."); +} + +void SRDFConfig::getRelativePath() +{ + if (!srdf_pkg_relative_path_.empty()) + { + return; + } + srdf_pkg_relative_path_ = std::filesystem::path("config") / (urdf_model_->getName() + ".srdf"); +} + +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_ = std::filesystem::path("config") / (urdf_model_->getName() + ".srdf"); + srdf_.robot_name_ = urdf_model_->getName(); + changes_ |= OTHER; + } + changes_ |= changed_information; + + // Reset the planning scene + planning_scene_.reset(); +} + +// ****************************************************************************************** +// Provide a shared planning scene +// ****************************************************************************************** +planning_scene::PlanningScenePtr SRDFConfig::getPlanningScene() +{ + if (!planning_scene_) + { + // make sure kinematic model exists + getRobotModel(); + + // Allocate an empty planning scene + planning_scene_ = std::make_shared(robot_model_); + } + return planning_scene_; +} + +std::vector SRDFConfig::getLinkNames() const +{ + std::vector names; + for (const moveit::core::LinkModel* link_model : robot_model_->getLinkModels()) + { + names.push_back(link_model->getName()); + } + 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::POSES); + return; + } + } +} + +std::vector SRDFConfig::getJointNames(const std::string& group_name, bool include_multi_dof, + bool include_passive) +{ + std::vector names; + const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name); + + // Iterate through the joints + for (const moveit::core::JointModel* joint : joint_model_group->getActiveJointModels()) + { + if (!include_multi_dof && joint->getVariableCount() > 1) + { + continue; + } + else if (!include_passive && joint->isPassive()) + { + continue; + } + names.push_back(joint->getName()); + } + return names; +} + +void SRDFConfig::collectVariables(std::vector& variables) +{ + variables.push_back(TemplateVariable("ROBOT_NAME", srdf_.robot_name_)); + variables.push_back(TemplateVariable("ROBOT_ROOT_LINK", robot_model_->getRootLinkName())); + variables.push_back(TemplateVariable("PLANNING_FRAME", robot_model_->getModelFrame())); +} + +/** + * \brief Custom std::set comparator, used for sorting the joint_limits.yaml file into alphabetical order + * \param jm1 - a pointer to the first joint model to compare + * \param jm2 - a pointer to the second joint model to compare + * \return bool of alphabetical sorting comparison + */ +struct JointModelCompare +{ + bool operator()(const moveit::core::JointModel* jm1, const moveit::core::JointModel* jm2) const + { + return jm1->getName() < jm2->getName(); + } +}; + +bool SRDFConfig::GeneratedJointLimits::writeYaml(YAML::Emitter& emitter) +{ + emitter << YAML::Comment("joint_limits.yaml allows the dynamics properties specified in the URDF " + "to be overwritten or augmented as needed"); + emitter << YAML::Newline; + + emitter << YAML::BeginMap; + + emitter << YAML::Comment("For beginners, we downscale velocity and acceleration limits.") << YAML::Newline; + emitter << YAML::Comment("You can always specify higher scaling factors (<= 1.0) in your motion requests."); + emitter << YAML::Comment("Increase the values below to 1.0 to always move at maximum speed."); + emitter << YAML::Key << "default_velocity_scaling_factor"; + emitter << YAML::Value << "0.1"; + + emitter << YAML::Key << "default_acceleration_scaling_factor"; + emitter << YAML::Value << "0.1"; + + emitter << YAML::Newline << YAML::Newline; + emitter << YAML::Comment("Specific joint properties can be changed with the keys " + "[max_position, min_position, max_velocity, max_acceleration]") + << YAML::Newline; + emitter << YAML::Comment("Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]"); + + emitter << YAML::Key << "joint_limits"; + emitter << YAML::Value << YAML::BeginMap; + + // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name + std::set joints; + + // Loop through groups + for (srdf::Model::Group& group : parent_.srdf_.groups_) + { + // Get list of associated joints + const moveit::core::JointModelGroup* joint_model_group = parent_.getRobotModel()->getJointModelGroup(group.name_); + + const std::vector& joint_models = joint_model_group->getJointModels(); + + // Iterate through the joints + for (const moveit::core::JointModel* joint_model : joint_models) + { + // Check that this joint only represents 1 variable. + if (joint_model->getVariableCount() == 1) + joints.insert(joint_model); + } + } + + // Add joints to yaml file, if no more than 1 dof + for (const moveit::core::JointModel* joint : joints) + { + emitter << YAML::Key << joint->getName(); + emitter << YAML::Value << YAML::BeginMap; + + const moveit::core::VariableBounds& b = joint->getVariableBounds()[0]; + + // Output property + emitter << YAML::Key << "has_velocity_limits"; + if (b.velocity_bounded_) + emitter << YAML::Value << "true"; + else + emitter << YAML::Value << "false"; + + // Output property + emitter << YAML::Key << "max_velocity"; + emitter << YAML::Value << std::min(fabs(b.max_velocity_), fabs(b.min_velocity_)); + + // Output property + emitter << YAML::Key << "has_acceleration_limits"; + if (b.acceleration_bounded_) + emitter << YAML::Value << "true"; + else + emitter << YAML::Value << "false"; + + // Output property + emitter << YAML::Key << "max_acceleration"; + emitter << YAML::Value << std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_)); + + emitter << YAML::EndMap; + } + + emitter << YAML::EndMap; + return true; // file created successfully +} + +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::SRDFConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp new file mode 100644 index 0000000000..6fc39d0671 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp @@ -0,0 +1,97 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, Inc. + * 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 string find and replace in templates + +namespace moveit_setup +{ +std::vector TemplatedGeneratedFile::variables_; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_setup.templates"); + +bool TemplatedGeneratedFile::write() +{ + std::filesystem::path template_path = getTemplatePath(); + + // Error check file + if (!std::filesystem::is_regular_file(template_path)) + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to find template file " << template_path.string()); + return false; + } + + // Load file + std::ifstream template_stream(template_path); + if (!template_stream.good()) // File not found + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to load file " << template_path.string()); + return false; + } + + // Load the file to a string using an efficient memory allocation technique + std::string template_string; + template_stream.seekg(0, std::ios::end); + template_string.reserve(template_stream.tellg()); + template_stream.seekg(0, std::ios::beg); + template_string.assign((std::istreambuf_iterator(template_stream)), std::istreambuf_iterator()); + template_stream.close(); + + // Replace keywords in string ------------------------------------------------------------ + for (const auto& variable : variables_) + { + std::string key_with_brackets = "[" + variable.key + "]"; + boost::replace_all(template_string, key_with_brackets, variable.value); + } + + // Save string to new location ----------------------------------------------------------- + std::filesystem::path file_path = getPath(); + createParentFolders(file_path); + + std::ofstream output_stream(file_path, std::ios_base::trunc); + if (!output_stream.good()) + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to open file for writing " << file_path.string()); + return false; + } + + output_stream << template_string.c_str(); + output_stream.close(); + + return true; // file created successfully +} + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp new file mode 100644 index 0000000000..290c9e8a2a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp @@ -0,0 +1,197 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, Inc. + * 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. + *********************************************************************/ + +#include +#include +#include +#include + +namespace moveit_setup +{ +void URDFConfig::onInit() +{ + parent_node_->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); +} + +void URDFConfig::loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node) +{ + if (!getYamlProperty(node, "package", urdf_pkg_name_)) + { + throw std::runtime_error("cannot find package property in URDF"); + } + + if (!getYamlProperty(node, "relative_path", urdf_pkg_relative_path_)) + { + throw std::runtime_error("cannot find relative_path property in URDF"); + } + + getYamlProperty(node, "xacro_args", xacro_args_); + loadFromPackage(urdf_pkg_name_, urdf_pkg_relative_path_, xacro_args_); +} + +YAML::Node URDFConfig::saveToYaml() const +{ + YAML::Node node; + node["package"] = urdf_pkg_name_; + node["relative_path"] = urdf_pkg_relative_path_.string(); + if (!xacro_args_.empty()) + { + node["xacro_args"] = xacro_args_; + } + return node; +} + +void URDFConfig::loadFromPath(const std::filesystem::path& urdf_file_path, const std::string& xacro_args) +{ + urdf_path_ = urdf_file_path; + xacro_args_ = xacro_args; + xacro_args_vec_ = { xacro_args_ }; + setPackageName(); + load(); +} + +void URDFConfig::loadFromPath(const std::filesystem::path& urdf_file_path, const std::vector& xacro_args) +{ + urdf_path_ = urdf_file_path; + xacro_args_vec_ = xacro_args; + xacro_args_ = boost::algorithm::join(xacro_args_vec_, " "); + setPackageName(); + load(); +} + +void URDFConfig::setPackageName() +{ + bool package_found = extractPackageNameFromPath(urdf_path_, urdf_pkg_name_, urdf_pkg_relative_path_); + if (!package_found) + { + urdf_pkg_name_ = ""; + urdf_pkg_relative_path_ = urdf_path_; // just the absolute path + } + else + { + // Check that ROS can find the package + const std::filesystem::path robot_desc_pkg_path = getSharePath(urdf_pkg_name_); + + if (robot_desc_pkg_path.empty()) + { + RCLCPP_WARN(*logger_, + "Package Not Found In ROS Workspace. ROS was unable to find the package name '%s'" + " within the ROS workspace. This may cause issues later.", + urdf_pkg_name_.c_str()); + } + } +} + +void URDFConfig::loadFromPackage(const std::filesystem::path& package_name, const std::filesystem::path& relative_path, + const std::string& xacro_args) +{ + urdf_pkg_name_ = package_name; + urdf_pkg_relative_path_ = relative_path; + xacro_args_ = xacro_args; + + urdf_path_ = getSharePath(urdf_pkg_name_) / relative_path; + load(); +} + +void URDFConfig::load() +{ + RCLCPP_DEBUG_STREAM(*logger_, "URDF Package Name: " << urdf_pkg_name_); + RCLCPP_DEBUG_STREAM(*logger_, "URDF Package Path: " << urdf_pkg_relative_path_); + + if (!rdf_loader::RDFLoader::loadXmlFileToString(urdf_string_, urdf_path_, xacro_args_vec_)) + { + throw std::runtime_error("URDF/COLLADA file not found: " + urdf_path_.string()); + } + + if (urdf_string_.empty() && rdf_loader::RDFLoader::isXacroFile(urdf_path_)) + { + throw std::runtime_error("Running xacro failed.\nPlease check console for errors."); + } + + // Verify that file is in correct format / not an XACRO by loading into robot model + if (!urdf_model_->initString(urdf_string_)) + { + throw std::runtime_error("URDF/COLLADA file is not a valid robot model."); + } + urdf_from_xacro_ = rdf_loader::RDFLoader::isXacroFile(urdf_path_); + + // Set parameter + parent_node_->set_parameter(rclcpp::Parameter("robot_description", urdf_string_)); + + RCLCPP_INFO_STREAM(*logger_, "Loaded " << urdf_model_->getName() << " robot model."); +} + +bool URDFConfig::isXacroFile() const +{ + return rdf_loader::RDFLoader::isXacroFile(urdf_path_); +} + +bool URDFConfig::isConfigured() const +{ + return urdf_model_->getRoot() != nullptr; +} + +void URDFConfig::collectDependencies(std::set& packages) const +{ + packages.insert(urdf_pkg_name_); +} + +void URDFConfig::collectVariables(std::vector& variables) +{ + std::string urdf_location; + if (urdf_pkg_name_.empty()) + { + urdf_location = urdf_path_; + } + else + { + urdf_location = "$(find " + urdf_pkg_name_ + ")/" + urdf_pkg_relative_path_.string(); + } + + variables.push_back(TemplateVariable("URDF_LOCATION", urdf_location)); + + if (urdf_from_xacro_) + { + variables.push_back( + TemplateVariable("URDF_LOAD_ATTRIBUTE", "command=\"xacro " + xacro_args_ + " '" + urdf_location + "'\"")); + } + else + { + variables.push_back(TemplateVariable("URDF_LOAD_ATTRIBUTE", "textfile=\"" + urdf_location + "\"")); + } +} +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::URDFConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp b/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp new file mode 100644 index 0000000000..51e207f0d8 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/src/utilities.cpp @@ -0,0 +1,131 @@ +/********************************************************************* + * 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 +{ +bool extractPackageNameFromPath(const std::filesystem::path& path, std::string& package_name, + std::filesystem::path& relative_filepath) +{ + std::filesystem::path sub_path = path; // holds the directory less one folder + if (std::filesystem::is_regular_file(sub_path)) + { + relative_filepath = sub_path.filename(); // holds the path after the sub_path + sub_path = sub_path.parent_path(); + } + else + { + relative_filepath = sub_path; + } + + // truncate path step by step and check if it contains a package.xml + while (!sub_path.empty()) + { + if (std::filesystem::is_regular_file(sub_path / "package.xml")) + { + // Search the string in the package.xml file + // This works assuming the package name is entered as PACKAGE_NAME + // Default package name to folder name + package_name = sub_path.filename().string(); + tinyxml2::XMLDocument package_xml_file; + auto is_open = package_xml_file.LoadFile((sub_path / "package.xml").c_str()); + if (is_open == tinyxml2::XML_SUCCESS) + { + auto name_potential = + package_xml_file.FirstChildElement("package")->FirstChildElement("name")->FirstChild()->ToText()->Value(); + if (name_potential) + { + // Change package name if we have non-empty potential, else it defaults + package_name = name_potential; + } + } + return true; + } + relative_filepath = sub_path.filename() / relative_filepath; + sub_path = sub_path.parent_path(); + } + // No package name found, we must be outside ROS + return false; +} + +bool hasRequiredAttributes(const tinyxml2::XMLElement& e, const std::vector& attributes) +{ + for (const auto& attr : attributes) + { + if (!attr.required) + continue; // attribute not required + const char* value = e.Attribute(attr.name); + if (value && strcmp(attr.value, value) == 0) + continue; // attribute has required value + else + return false; + } + return true; +}; + +tinyxml2::XMLElement* uniqueInsert(tinyxml2::XMLDocument& doc, tinyxml2::XMLElement& element, const char* tag, + const std::vector& attributes, const char* text) +{ + // search for existing element with required tag name and attributes + tinyxml2::XMLElement* result = element.FirstChildElement(tag); + while (result && !hasRequiredAttributes(*result, attributes)) + result = result->NextSiblingElement(tag); + + if (!result) // if not yet present, create new element + { + result = doc.NewElement(tag); + element.InsertEndChild(result); + } + + // set (not-yet existing) attributes + for (const auto& attr : attributes) + { + if (!result->Attribute(attr.name)) + result->SetAttribute(attr.name, attr.value); + } + + // insert text if required + if (text && !result->GetText()) + { + tinyxml2::XMLText* text_el = doc.NewText(text); + result->InsertEndChild(text_el); + } + + return result; +} + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/tools/xml_syntax_highlighter.cpp b/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp similarity index 97% rename from moveit_setup_assistant/src/tools/xml_syntax_highlighter.cpp rename to moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp index 60703f8da8..50994498ae 100644 --- a/moveit_setup_assistant/src/tools/xml_syntax_highlighter.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/xml_syntax_highlighter.cpp @@ -34,9 +34,11 @@ /* Author: Robert Haschke */ -#include "xml_syntax_highlighter.h" +#include #include +namespace moveit_setup +{ XmlSyntaxHighlighter::XmlSyntaxHighlighter(QTextDocument* parent) : QSyntaxHighlighter(parent) { } @@ -122,3 +124,5 @@ void XmlSyntaxHighlighter::highlightBlock(const QString& text) active = highlight(active, QStringRef(&text, 0, text.size()), 0, active != rules.cend(), unused); setCurrentBlockState(active != rules.cend() ? active->first : -1); } + +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt new file mode 100644 index 0000000000..4fa149b7c2 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.10.2) +project([GENERATED_PACKAGE_NAME]) + +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml b/moveit_setup_assistant/moveit_setup_framework/templates/config/cartesian_limits.yaml similarity index 58% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml rename to moveit_setup_assistant/moveit_setup_framework/templates/config/cartesian_limits.yaml index 7df72f693b..c1e8d321d9 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/cartesian_limits.yaml +++ b/moveit_setup_assistant/moveit_setup_framework/templates/config/cartesian_limits.yaml @@ -1,5 +1,5 @@ cartesian_limits: - max_trans_vel: 1 + max_trans_vel: 1.0 max_trans_acc: 2.25 - max_trans_dec: -5 + max_trans_dec: -5.0 max_rot_vel: 1.57 diff --git a/moveit_setup_assistant/moveit_setup_framework/templates/package.xml.template b/moveit_setup_assistant/moveit_setup_framework/templates/package.xml.template new file mode 100644 index 0000000000..4862444bd3 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/templates/package.xml.template @@ -0,0 +1,38 @@ + + + + [GENERATED_PACKAGE_NAME] + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the [ROBOT_NAME] with the MoveIt Motion Planning Framework + + [AUTHOR_NAME] + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + [AUTHOR_NAME] + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + +[OTHER_DEPENDENCIES] + + + ament_cmake + + diff --git a/moveit_setup_assistant/test/moveit_config_data_test.cpp b/moveit_setup_assistant/moveit_setup_framework/test/moveit_config_data_test.cpp similarity index 100% rename from moveit_setup_assistant/test/moveit_config_data_test.cpp rename to moveit_setup_assistant/moveit_setup_framework/test/moveit_config_data_test.cpp diff --git a/moveit_setup_assistant/moveit_setup_simulation/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_simulation/CMakeLists.txt new file mode 100644 index 0000000000..d37bbc795a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_simulation/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.10.2) +project(moveit_setup_simulation) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(moveit_setup_framework REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) + +# Instruct CMake to run moc automatically when needed. +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +qt5_wrap_cpp(MOC_FILES + include/moveit_setup_simulation/simulation_widget.hpp +) + +add_library(${PROJECT_NAME} + src/simulation.cpp + src/simulation_widget.cpp + ${MOC_FILES} +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + ament_index_cpp + moveit_setup_framework + pluginlib + rclcpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY templates DESTINATION share/${PROJECT_NAME}) + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(FILES moveit_setup_framework_plugins.xml + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) +pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) + +ament_package() diff --git a/moveit_setup_assistant/moveit_setup_simulation/COLCON_IGNORE b/moveit_setup_assistant/moveit_setup_simulation/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation.hpp b/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation.hpp new file mode 100644 index 0000000000..f00bae3473 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation.hpp @@ -0,0 +1,110 @@ +/********************************************************************* + * 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 +{ +namespace simulation +{ +class Simulation : public SetupStep +{ +public: + std::string getName() const override + { + return "Simulation"; + } + + void onInit() override; + + bool isReady() const override + { + return false; + } + + std::filesystem::path getURDFPath() const + { + return urdf_config_->getURDFPath(); + } + + std::string getURDFPackageName() const + { + return urdf_config_->getURDFPackageName(); + } + + std::string getURDFContents() const + { + return urdf_config_->getURDFContents(); + } + + /** + * \brief Helper function to get the controller that is controlling the joint + * \return controller type + */ + std::string getJointHardwareInterface(const std::string& joint_name); + + /** + * \brief Parses the existing urdf and constructs a string from it with the elements required by gazebo simulator + * added + * \return gazebo compatible urdf or empty if error encountered + */ + std::string getGazeboCompatibleURDF(); + + bool outputGazeboURDFFile(const std::filesystem::path& file_path); + + /** + * @brief Check if the given xml is valid + * @param[in] new_urdf_contents The string of xml to check + * @param[out] error_row The row of the error + * @param[out] error_description The description + * @returns True if valid, false otherwise + */ + bool isValidXML(const std::string& new_urdf_contents, int& error_row, std::string& error_description) const; + +protected: + std::shared_ptr urdf_config_; + + /// Gazebo URDF robot model string + // NOTE: Created when the robot urdf is not compatible with Gazebo. + std::string gazebo_urdf_string_; + + /// Whether a new Gazebo URDF is created + bool save_gazebo_urdf_; +}; +} // namespace simulation +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.h b/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation_widget.hpp similarity index 84% rename from moveit_setup_assistant/src/widgets/simulation_widget.h rename to moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation_widget.hpp index d6f1e28113..57e88b3ea0 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.h +++ b/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation_widget.hpp @@ -41,20 +41,19 @@ class QTextEdit; class QPushButton; // 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 +{ +namespace simulation { // ****************************************************************************************** // ****************************************************************************************** // Class for showing changes needed to help user bring his robot into gazebo simulation // ****************************************************************************************** // ****************************************************************************************** -class SimulationWidget : public SetupScreenWidget +class SimulationWidget : public SetupStepWidget { Q_OBJECT @@ -63,13 +62,14 @@ class SimulationWidget : public SetupScreenWidget // Public Functions // ****************************************************************************************** - SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; void focusGiven() override; bool focusLost() override; -private: - /// Generate Gazebo-compatible URDF, starting from original URDF - std::string generateGazeboCompatibleURDF() const; + SetupStep& getSetupStep() override + { + return setup_step_; + } private Q_SLOTS: // ****************************************************************************************** @@ -94,8 +94,8 @@ private Q_SLOTS: QPushButton* btn_open_; QLabel* copy_urdf_; - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + Simulation setup_step_; }; -} // namespace moveit_setup_assistant +} // namespace simulation +} // namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_simulation/moveit_setup_framework_plugins.xml b/moveit_setup_assistant/moveit_setup_simulation/moveit_setup_framework_plugins.xml new file mode 100644 index 0000000000..ca5c8ac17c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_simulation/moveit_setup_framework_plugins.xml @@ -0,0 +1,17 @@ + + + Configures launches to generate + + + Configures the sensor suite + + + Data for Launch Files + + + Data for perception step + + + Configures the simulation + + diff --git a/moveit_setup_assistant/moveit_setup_simulation/package.xml b/moveit_setup_assistant/moveit_setup_simulation/package.xml new file mode 100644 index 0000000000..3fa92a3551 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_simulation/package.xml @@ -0,0 +1,26 @@ + + + + moveit_setup_simulation + 2.5.1 + MoveIt Setup Steps for Simulation + David V. Lu!! + BSD + David V. Lu!! + + ament_cmake + + ament_index_cpp + moveit_setup_framework + pluginlib + rclcpp + + ament_lint_auto + ament_clang_format + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/moveit_setup_assistant/moveit_setup_simulation/src/simulation.cpp b/moveit_setup_assistant/moveit_setup_simulation/src/simulation.cpp new file mode 100644 index 0000000000..94baf044bb --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_simulation/src/simulation.cpp @@ -0,0 +1,201 @@ +/********************************************************************* + * 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 +{ +namespace simulation +{ +void Simulation::onInit() +{ + urdf_config_ = config_data_->get("urdf"); +} + +// ****************************************************************************************** +// Helper function to get the controller that is controlling the joint +// ****************************************************************************************** +std::string Simulation::getJointHardwareInterface(const std::string& joint_name) +{ + /** TODO: Need to port this - may require depending on moveit_setup_controllers ControllerConfig + + for (ControllerConfig& ros_control_config : controller_configs_) + { + std::vector::iterator joint_it = + std::find(ros_control_config.joints_.begin(), ros_control_config.joints_.end(), joint_name); + if (joint_it != ros_control_config.joints_.end()) + { + if (ros_control_config.type_.substr(0, 8) == "position") + return "hardware_interface/PositionJointInterface"; + else if (ros_control_config.type_.substr(0, 8) == "velocity") + return "hardware_interface/VelocityJointInterface"; + // As of writing this, available joint command interfaces are position, velocity and effort. + else + return "hardware_interface/EffortJointInterface"; + } + } + */ + // If the joint was not found in any controller return EffortJointInterface + return "hardware_interface/EffortJointInterface"; +} + +std::string printXML(const tinyxml2::XMLDocument& doc) +{ + tinyxml2::XMLPrinter printer; + doc.Accept(&printer); + return printer.CStr(); +} + +// ****************************************************************************************** +// Writes a Gazebo compatible robot URDF to gazebo_compatible_urdf_string_ +// ****************************************************************************************** +std::string Simulation::getGazeboCompatibleURDF() +{ + using tinyxml2::XMLElement; + + tinyxml2::XMLDocument doc; + std::string urdf_string = urdf_config_->getURDFContents(); + doc.Parse(urdf_string.c_str()); + auto root = doc.RootElement(); + + // Normalize original urdf_string_ + std::string orig_urdf = printXML(doc); + + // Map existing SimpleTransmission elements to their joint name + std::map transmission_elements; + for (XMLElement* element = root->FirstChildElement("transmission"); element != nullptr; + element = element->NextSiblingElement(element->Value())) + { + auto type_tag = element->FirstChildElement("type"); + auto joint_tag = element->FirstChildElement("joint"); + if (!type_tag || !type_tag->GetText() || !joint_tag || !joint_tag->Attribute("name")) + continue; // ignore invalid tags + if (std::string(type_tag->GetText()) == "transmission_interface/SimpleTransmission") + transmission_elements[element->FirstChildElement("joint")->Attribute("name")] = element; + } + + // Loop through Link and Joint elements and add Gazebo tags if not present + for (XMLElement* element = root->FirstChildElement(); element != nullptr; element = element->NextSiblingElement()) + { + const std::string tag_name(element->Value()); + if (tag_name == "link" && element->FirstChildElement("collision")) + { + XMLElement* inertial = uniqueInsert(doc, *element, "inertial"); + uniqueInsert(doc, *inertial, "mass", { { "value", "0.1" } }); + uniqueInsert(doc, *inertial, "origin", { { "xyz", "0 0 0" }, { "rpy", "0 0 0" } }); + uniqueInsert(doc, *inertial, "inertia", + { { "ixx", "0.03" }, + { "iyy", "0.03" }, + { "izz", "0.03" }, + { "ixy", "0.0" }, + { "ixz", "0.0" }, + { "iyz", "0.0" } }); + } + else if (tag_name == "joint") + { + const char* joint_type = element->Attribute("type"); + const char* joint_name = element->Attribute("name"); + if (!joint_type || !joint_name || strcmp(joint_type, "fixed") == 0) + continue; // skip invalid or fixed joints + + // find existing or create new transmission element for this joint + XMLElement* transmission; + auto it = transmission_elements.find(joint_name); + if (it != transmission_elements.end()) + transmission = it->second; + else + { + transmission = doc.NewElement("transmission"); + root->InsertEndChild(transmission); + transmission->SetAttribute("name", (std::string("trans_") + joint_name).c_str()); + } + + uniqueInsert(doc, *transmission, "type", {}, "transmission_interface/SimpleTransmission"); + + std::string hw_interface = getJointHardwareInterface(joint_name); + auto* joint = uniqueInsert(doc, *transmission, "joint", { { "name", joint_name } }); + uniqueInsert(doc, *joint, "hardwareInterface", {}, hw_interface.c_str()); + + auto actuator_name = joint_name + std::string("_motor"); + auto* actuator = uniqueInsert(doc, *transmission, "actuator", { { "name", actuator_name.c_str() } }); + uniqueInsert(doc, *actuator, "hardwareInterface", {}, hw_interface.c_str()); + uniqueInsert(doc, *actuator, "mechanicalReduction", {}, "1"); + } + } + + // Add gazebo_ros_control plugin which reads the transmission tags + XMLElement* gazebo = uniqueInsert(doc, *root, "gazebo"); + XMLElement* plugin = uniqueInsert(doc, *gazebo, "plugin", { { "name", "gazebo_ros_control", true } }); + uniqueInsert(doc, *plugin, "robotNamespace", {}, "/"); + + // generate new URDF + std::string new_urdf = printXML(doc); + // and return it when there are changes + return orig_urdf == new_urdf ? std::string() : new_urdf; +} + +// ****************************************************************************************** +// Output Gazebo URDF file +// ****************************************************************************************** +bool Simulation::outputGazeboURDFFile(const std::filesystem::path& file_path) +{ + std::ofstream os(file_path, std::ios_base::trunc); + if (!os.good()) + { + RCLCPP_ERROR_STREAM(*logger_, "Unable to open file for writing " << file_path); + return false; + } + + os << gazebo_urdf_string_.c_str() << std::endl; + os.close(); + + return true; +} + +bool Simulation::isValidXML(const std::string& new_urdf_contents, int& error_row, std::string& error_description) const +{ + tinyxml2::XMLDocument doc; + doc.Parse(new_urdf_contents.c_str()); + if (doc.Error()) + { + error_row = doc.ErrorLineNum(); + error_description = doc.ErrorStr(); + } + return !doc.Error(); +} + +} // namespace simulation +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/simulation_widget.cpp b/moveit_setup_assistant/moveit_setup_simulation/src/simulation_widget.cpp similarity index 59% rename from moveit_setup_assistant/src/widgets/simulation_widget.cpp rename to moveit_setup_assistant/moveit_setup_simulation/src/simulation_widget.cpp index 95fa4c3957..981658b161 100644 --- a/moveit_setup_assistant/src/widgets/simulation_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_simulation/src/simulation_widget.cpp @@ -34,10 +34,9 @@ /* Author: Mohamad Ayman */ // SA -#include "simulation_widget.h" -#include "header_widget.h" -#include "../tools/xml_syntax_highlighter.h" -#include +#include +#include +#include // Qt #include @@ -53,13 +52,14 @@ #include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace simulation { // ****************************************************************************************** // Constructor // ****************************************************************************************** -SimulationWidget::SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +void SimulationWidget::onInit() { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -67,11 +67,10 @@ SimulationWidget::SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& c // Top Header Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget("Simulate With Gazebo", - "The following tool will auto-generate the URDF changes needed " - "for Gazebo compatibility with ROSControl and MoveIt. The " - "needed changes are shown in green.", - this); + auto header = new HeaderWidget("Simulate With Gazebo", + "The following tool will auto-generate the URDF changes needed for Gazebo " + "compatibility with ROSControl and MoveIt. The needed changes are shown in green.", + this); layout->addWidget(header); layout->addSpacerItem(new QSpacerItem(1, 8, QSizePolicy::Fixed, QSizePolicy::Fixed)); @@ -132,8 +131,7 @@ void SimulationWidget::focusGiven() return; // nothing to do simulation_text_->setVisible(true); - std::string text = generateGazeboCompatibleURDF(); - config_data_->gazebo_urdf_string_ = text; + std::string text = setup_step_.getGazeboCompatibleURDF(); simulation_text_->document()->setPlainText(QString::fromStdString(text)); @@ -152,7 +150,7 @@ void SimulationWidget::focusGiven() btn_overwrite_->setDisabled(config_data_->urdf_from_xacro_); QString tooltip; if (btn_overwrite_->isEnabled()) - tooltip = tr("Overwrite URDF in original location:\n").append(config_data_->urdf_path_.c_str()); + tooltip = tr("Overwrite URDF in original location:\n").append(setup_step_.getURDFPath().c_str()); else tooltip = tr("Cannot overwrite original, xacro-based URDF"); btn_overwrite_->setToolTip(tooltip); @@ -169,23 +167,25 @@ bool SimulationWidget::focusLost() return true; // saving is disabled anyway // validate XML - TiXmlDocument doc; - auto urdf = simulation_text_->document()->toPlainText().toStdString(); - doc.Parse(urdf.c_str(), nullptr, TIXML_ENCODING_UTF8); - if (doc.Error()) + std::string urdf = simulation_text_->document()->toPlainText().toStdString(); + int error_row; + std::string error_description; + + if (setup_step_.isValidXML(urdf, error_row, error_description)) + { + config_data_->gazebo_urdf_string_ = std::move(urdf); + return true; + } + else { QTextCursor cursor = simulation_text_->textCursor(); cursor.movePosition(QTextCursor::Start); - cursor.movePosition(QTextCursor::Down, QTextCursor::MoveAnchor, doc.ErrorRow()); - cursor.movePosition(QTextCursor::Right, QTextCursor::MoveAnchor, doc.ErrorCol()); + cursor.movePosition(QTextCursor::Down, QTextCursor::MoveAnchor, error_row); simulation_text_->setTextCursor(cursor); - QMessageBox::warning(this, tr("Gazebo URDF"), tr("Error parsing XML:\n").append(doc.ErrorDesc())); + QMessageBox::warning(this, tr("Gazebo URDF"), tr("Error parsing XML:\n").append(error_description.c_str())); simulation_text_->setFocus(Qt::OtherFocusReason); return false; // reject switching } - else - config_data_->gazebo_urdf_string_ = std::move(urdf); - return true; } // ****************************************************************************************** @@ -196,7 +196,7 @@ void SimulationWidget::overwriteURDF() if (!focusLost()) // validate XML return; - if (!config_data_->outputGazeboURDFFile(config_data_->urdf_path_)) + if (!setup_step_.outputGazeboURDFFile(config_data_->urdf_path_)) QMessageBox::warning(this, "Gazebo URDF", tr("Failed to save to ").append(config_data_->urdf_path_.c_str())); else // Display success message QMessageBox::information(this, "Overwriting Successful", @@ -209,7 +209,7 @@ void SimulationWidget::overwriteURDF() void SimulationWidget::openURDF() { - QProcess::startDetached(qgetenv("EDITOR"), { config_data_->urdf_path_.c_str() }); + QProcess::startDetached(qgetenv("EDITOR"), { setup_step_.getURDFPath().c_str() }); } // ****************************************************************************************** @@ -220,89 +220,8 @@ void SimulationWidget::copyURDF() simulation_text_->selectAll(); simulation_text_->copy(); } +} // namespace simulation +} // namespace moveit_setup -// Generate a Gazebo-compatible robot URDF -std::string SimulationWidget::generateGazeboCompatibleURDF() const -{ - TiXmlDocument doc; - doc.Parse(config_data_->urdf_string_.c_str(), nullptr, TIXML_ENCODING_UTF8); - auto root = doc.RootElement(); - - // Normalize original urdf_string_ - TiXmlPrinter orig_urdf; - doc.Accept(&orig_urdf); - - // Map existing SimpleTransmission elements to their joint name - std::map transmission_elements; - for (TiXmlElement* element = root->FirstChildElement("transmission"); element != nullptr; - element = element->NextSiblingElement(element->Value())) - { - auto type_tag = element->FirstChildElement("type"); - auto joint_tag = element->FirstChildElement("joint"); - if (!type_tag || !type_tag->GetText() || !joint_tag || !joint_tag->Attribute("name")) - continue; // ignore invalid tags - if (std::string(type_tag->GetText()) == "transmission_interface/SimpleTransmission") - transmission_elements[element->FirstChildElement("joint")->Attribute("name")] = element; - } - - // Loop through Link and Joint elements and add Gazebo tags if not present - for (TiXmlElement* element = root->FirstChildElement(); element != nullptr; element = element->NextSiblingElement()) - { - const std::string tag_name(element->Value()); - if (tag_name == "link" && element->FirstChildElement("collision")) - { - TiXmlElement* inertial = uniqueInsert(*element, "inertial"); - uniqueInsert(*inertial, "mass", { { "value", "0.1" } }); - uniqueInsert(*inertial, "origin", { { "xyz", "0 0 0" }, { "rpy", "0 0 0" } }); - uniqueInsert(*inertial, "inertia", - { { "ixx", "0.03" }, - { "iyy", "0.03" }, - { "izz", "0.03" }, - { "ixy", "0.0" }, - { "ixz", "0.0" }, - { "iyz", "0.0" } }); - } - else if (tag_name == "joint") - { - const char* joint_type = element->Attribute("type"); - const char* joint_name = element->Attribute("name"); - if (!joint_type || !joint_name || strcmp(joint_type, "fixed") == 0) - continue; // skip invalid or fixed joints - - // find existing or create new transmission element for this joint - TiXmlElement* transmission; - auto it = transmission_elements.find(joint_name); - if (it != transmission_elements.end()) - transmission = it->second; - else - { - transmission = root->InsertEndChild(TiXmlElement("transmission"))->ToElement(); - transmission->SetAttribute("name", std::string("trans_") + joint_name); - } - - uniqueInsert(*transmission, "type", {}, "transmission_interface/SimpleTransmission"); - - std::string hw_interface = config_data_->getJointHardwareInterface(joint_name); - auto* joint = uniqueInsert(*transmission, "joint", { { "name", joint_name } }); - uniqueInsert(*joint, "hardwareInterface", {}, hw_interface.c_str()); - - auto actuator_name = joint_name + std::string("_motor"); - auto* actuator = uniqueInsert(*transmission, "actuator", { { "name", actuator_name.c_str() } }); - uniqueInsert(*actuator, "hardwareInterface", {}, hw_interface.c_str()); - uniqueInsert(*actuator, "mechanicalReduction", {}, "1"); - } - } - - // Add gazebo_ros_control plugin which reads the transmission tags - TiXmlElement* gazebo = uniqueInsert(*root, "gazebo"); - TiXmlElement* plugin = uniqueInsert(*gazebo, "plugin", { { "name", "gazebo_ros_control", true } }); - uniqueInsert(*plugin, "robotNamespace", {}, "/"); - - // generate new URDF - TiXmlPrinter new_urdf; - doc.Accept(&new_urdf); - // and return it when there are changes - return orig_urdf.Str() == new_urdf.Str() ? std::string() : new_urdf.Str(); -} - -} // namespace moveit_setup_assistant +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::simulation::SimulationWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch b/moveit_setup_assistant/moveit_setup_simulation/templates/launch/demo_gazebo.launch similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo_gazebo.launch rename to moveit_setup_assistant/moveit_setup_simulation/templates/launch/demo_gazebo.launch diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch b/moveit_setup_assistant/moveit_setup_simulation/templates/launch/gazebo.launch similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gazebo.launch rename to moveit_setup_assistant/moveit_setup_simulation/templates/launch/gazebo.launch diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt new file mode 100644 index 0000000000..822211001e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 3.10.2) +project(moveit_setup_srdf_plugins) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(moveit_setup_framework REQUIRED) +find_package(pluginlib REQUIRED) + +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 +) + +add_library(${PROJECT_NAME} + src/collision_linear_model.cpp + src/collision_matrix_model.cpp + 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 + ${MOC_FILES} +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + moveit_setup_framework + pluginlib +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(FILES moveit_setup_framework_plugins.xml + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) +pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) + +ament_package() diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_linear_model.hpp similarity index 93% rename from moveit_setup_assistant/src/tools/collision_linear_model.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_linear_model.hpp index 08e28151f5..a4d634cc45 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_linear_model.hpp @@ -41,11 +41,15 @@ #include #ifndef Q_MOC_RUN -#include +#include #endif -#include "collision_matrix_model.h" +#include +namespace moveit_setup +{ +namespace srdf_setup +{ class CollisionLinearModel : public QAbstractProxyModel { Q_OBJECT @@ -66,7 +70,7 @@ class CollisionLinearModel : public QAbstractProxyModel QModelIndex index(int row, int column, const QModelIndex& parent = QModelIndex()) const override; QModelIndex parent(const QModelIndex& child) const override; QVariant data(const QModelIndex& index, int role) const override; - moveit_setup_assistant::DisabledReason reason(int row) const; + DisabledReason reason(int row) const; bool setData(const QModelIndex& index, const QVariant& value, int role) override; void setEnabled(const QItemSelection& selection, bool value); @@ -99,3 +103,5 @@ private Q_SLOTS: QVector sort_columns_; // sorting history QVector sort_orders_; // corresponding sort orders }; +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/tools/collision_matrix_model.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.hpp similarity index 84% rename from moveit_setup_assistant/src/tools/collision_matrix_model.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.hpp index 7a724e852e..efb9943a6c 100644 --- a/moveit_setup_assistant/src/tools/collision_matrix_model.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.hpp @@ -39,22 +39,24 @@ #include #ifndef Q_MOC_RUN -#include +#include #endif -class QItemSelection; - +#include +namespace moveit_setup +{ +namespace srdf_setup +{ class CollisionMatrixModel : public QAbstractTableModel { Q_OBJECT public: - CollisionMatrixModel(moveit_setup_assistant::LinkPairMap& pairs, const std::vector& names, - QObject* parent = nullptr); + CollisionMatrixModel(LinkPairMap& pairs, const std::vector& names, QObject* parent = nullptr); int rowCount(const QModelIndex& parent = QModelIndex()) const override; int columnCount(const QModelIndex& parent = QModelIndex()) const override; QVariant data(const QModelIndex& index, int role = Qt::DisplayRole) const override; QVariant headerData(int section, Qt::Orientation orientation, int role) const override; - moveit_setup_assistant::DisabledReason reason(const QModelIndex& index) const; + DisabledReason reason(const QModelIndex& index) const; // for editing Qt::ItemFlags flags(const QModelIndex& index) const override; @@ -66,15 +68,17 @@ public Q_SLOTS: void setFilterRegExp(const QString& filter); private: - moveit_setup_assistant::LinkPairMap::iterator item(const QModelIndex& index); - moveit_setup_assistant::LinkPairMap::const_iterator item(const QModelIndex& index) const + LinkPairMap::iterator item(const QModelIndex& index); + LinkPairMap::const_iterator item(const QModelIndex& index) const { return const_cast(this)->item(index); } private: - moveit_setup_assistant::LinkPairMap& pairs; + LinkPairMap& pairs; const std::vector std_names; // names of links QList q_names; // names of links QList visual_to_index; // map from visual index to actual index }; +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.hpp similarity index 97% rename from moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.hpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.hpp index f7f41b1b66..846b99cc5c 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/compute_default_collisions.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.hpp @@ -43,7 +43,9 @@ namespace planning_scene MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc } -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { /** * \brief Reasons for disabling link pairs. Append "in collision" for understanding. @@ -108,4 +110,5 @@ const std::string disabledReasonToString(DisabledReason reason); * \return reason as struct */ DisabledReason disabledReasonFromString(const std::string& reason); -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup 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 new file mode 100644 index 0000000000..402d6d2e30 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp @@ -0,0 +1,104 @@ +/********************************************************************* + * 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 + +// less operation for two CollisionPairs +struct CollisionPairLess +{ + bool operator()(const srdf::Model::CollisionPair& left, const srdf::Model::CollisionPair& right) const + { + // use std::pair's operator<: (left.link1_, left.link2_) < (right.link1_, right.link2_) + return left.link1_ < right.link1_ || (!(right.link1_ < left.link1_) && left.link2_ < right.link2_); + } +}; + +namespace moveit_setup +{ +namespace srdf_setup +{ +class DefaultCollisions : public SRDFStep +{ +public: + std::string getName() const override + { + return "Self-Collisions"; + } + + std::vector getCollidingLinks(); + + /** + * @brief Output Link Pairs to SRDF Format + */ + void linkPairsToSRDF(); + + /** + * @brief Output Link Pairs to SRDF Format; sorted; with optional filter + * @param skip_mask mask of shifted DisabledReason values that will be skipped + */ + void linkPairsToSRDFSorted(size_t skip_mask = 0); + + /** + * @brief Load Link Pairs from SRDF Format + */ + void linkPairsFromSRDF(); + + LinkPairMap& getLinkPairs() + { + return link_pairs_; + } + + // For Threaded Operations + void startGenerationThread(unsigned int num_trials, double min_frac, bool verbose = true); + void cancelGenerationThread(); + void joinGenerationThread(); + int getThreadProgress() const; + +protected: + void generateCollisionTable(unsigned int num_trials, double min_frac, bool verbose); + + /// main storage of link pair data + LinkPairMap link_pairs_; + + // For threaded operations + boost::thread worker_; + unsigned int progress_; +}; +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions_widget.hpp similarity index 76% rename from moveit_setup_assistant/src/widgets/default_collisions_widget.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions_widget.hpp index 4eb4ae8e53..09c093cc64 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions_widget.hpp @@ -35,41 +35,38 @@ /* Author: Dave Coleman */ #pragma once + +#include +#include + #include -class QAbstractItemModel; -class QAction; -class QButtonGroup; -class QCheckBox; -class QGroupBox; -class QHeaderView; -class QItemSelection; -class QItemSelectionModel; -class QLabel; -class QLineEdit; -class QProgressBar; -class QPushButton; -class QRadioButton; -class QSlider; -class QSpinBox; -class QTableView; -class QVBoxLayout; - -#ifndef Q_MOC_RUN -#include -#include -#include -#endif - -#include "setup_screen_widget.h" // a base class for screens in the setup assistant - -namespace moveit_setup_assistant +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_setup +{ +namespace srdf_setup { class MonitorThread; /** * \brief User interface for editing the default collision matrix list in an SRDF */ -class DefaultCollisionsWidget : public SetupScreenWidget +class DefaultCollisionsWidget : public SetupStepWidget { Q_OBJECT @@ -83,23 +80,14 @@ class DefaultCollisionsWidget : public SetupScreenWidget // ****************************************************************************************** // Public Functions // ****************************************************************************************** + void onInit() override; - /** - * \brief User interface for editing the default collision matrix list in an SRDF - * \param urdf_file String srdf file location. It will create a new file or will edit an existing one - */ - DefaultCollisionsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); ~DefaultCollisionsWidget() override; - /** - * \brief Output Link Pairs to SRDF Format - */ - void linkPairsToSRDF(); - - /** - * \brief Load Link Pairs from SRDF Format - */ - void linkPairsFromSRDF(); + SetupStep& getSetupStep() override + { + return setup_step_; + } private Q_SLOTS: @@ -194,23 +182,12 @@ private Q_SLOTS: // ****************************************************************************************** MonitorThread* worker_; - /// main storage of link pair data - moveit_setup_assistant::LinkPairMap link_pairs_; - - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; + DefaultCollisions setup_step_; // ****************************************************************************************** // Private Functions // ****************************************************************************************** - /** - * \brief The thread that is called to allow the GUI to update. Calls an external function to do calcs - * \param collision_progress A shared pointer between 3 threads to allow progress bar to update. See declaration - * location for more details and warning. - */ - void generateCollisionTable(unsigned int* collision_progress); - /** * \brief Helper function to disable parts of GUI during computation * \param disable A command @@ -233,14 +210,14 @@ private Q_SLOTS: }; /** - * \brief QThread to monitor progress of a std::thread + * \brief QThread to monitor progress of the setup step */ class MonitorThread : public QThread { Q_OBJECT public: - MonitorThread(const std::function& f, QProgressBar* progress_bar = nullptr); + MonitorThread(DefaultCollisions& setup_step, QProgressBar* progress_bar = nullptr); void run() override; void cancel() { @@ -255,8 +232,8 @@ class MonitorThread : public QThread void progress(int /*_t1*/); private: - std::thread worker_; - unsigned int progress_; + DefaultCollisions& setup_step_; bool canceled_; }; -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup 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..3c9bf7ac83 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors.hpp @@ -0,0 +1,93 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +class EndEffectors : public SuperSRDFStep +{ +public: + std::string getName() const override + { + return "End Effectors"; + } + + std::vector& getContainer() override + { + return srdf_config_->getEndEffectors(); + } + + InformationFields getInfoField() const override + { + return 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 srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.hpp similarity index 88% rename from moveit_setup_assistant/src/widgets/end_effectors_widget.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.hpp index 6b776d6e57..ae7548fe14 100644 --- a/moveit_setup_assistant/src/widgets/end_effectors_widget.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.hpp @@ -37,22 +37,21 @@ #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 +{ +namespace srdf_setup { -class EndEffectorsWidget : public SetupScreenWidget +class EndEffectorsWidget : public SetupStepWidget { Q_OBJECT @@ -61,11 +60,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; + SetupStep& getSetupStep() override + { + return setup_step_; + } + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -116,9 +120,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 +135,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 +177,5 @@ private Q_SLOTS: void edit(const std::string& name); }; -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.hpp similarity index 90% rename from moveit_setup_assistant/src/widgets/group_edit_widget.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.hpp index 0cf37207c7..1fd68421a7 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.hpp @@ -37,16 +37,16 @@ #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 +{ +namespace srdf_setup { class GroupEditWidget : public QWidget { @@ -58,10 +58,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 +121,11 @@ 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 srdf_setup +} // namespace moveit_setup 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..eafb337ce5 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_meta_config.hpp @@ -0,0 +1,127 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +// 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 SetupConfig +{ +public: + bool isConfigured() const override; + void loadPrevious(const std::filesystem::path& 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 YamlGeneratedFile + { + public: + GeneratedGroupMetaConfig(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time, + GroupMetaConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + std::filesystem::path 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::filesystem::path& package_path, const GeneratedTime& 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::filesystem::path& 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 srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp similarity index 77% rename from moveit_setup_assistant/src/widgets/kinematic_chain_widget.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp index 03e3a90cac..0926a869df 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp @@ -37,42 +37,36 @@ #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 +{ +namespace srdf_setup { 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, 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 +107,18 @@ 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_; + RVizPanel* rviz_panel_; // ****************************************************************************************** // Private Functions // ****************************************************************************************** }; -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/xml_manipulation.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.hpp similarity index 66% rename from moveit_setup_assistant/include/moveit/setup_assistant/tools/xml_manipulation.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.hpp index 05017f502f..6e47201abe 100644 --- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/xml_manipulation.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.hpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2022, Bielefeld University, Inc. + * Copyright (c) 2021, PickNik Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * 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 Bielefeld University nor the names of its + * * 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. * @@ -32,28 +32,39 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Robert Haschke */ - +/* Author: David V. Lu!! */ #pragma once -#include -#include -namespace moveit_setup_assistant +#include + +namespace moveit_setup { -struct Attribute +namespace srdf_setup { - const char* name; - const char* value; - bool required = false; -}; +class PassiveJoints : public SRDFStep +{ +public: + std::string getName() const override + { + return "Passive Joints"; + } -/** Insert a new XML element with given tag, attributes and text value - * - * If a corresponding element already exists (and has required attribute values), it is just reused. - * All attributes are created or overwritten with given values. - * A text value is created or overwritten with given value (if not NULL). - * The element is returned */ -TiXmlElement* uniqueInsert(TiXmlElement& element, const char* tag, const std::vector& attributes = {}, - const char* text = nullptr); + /** + * @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; -} // namespace moveit_setup_assistant + std::string getChildOfJoint(const std::string& joint_name) const + { + return srdf_config_->getChildOfJoint(joint_name); + } + + void setPassiveJoints(const std::vector& passive_joints); +}; +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.hpp similarity index 85% rename from moveit_setup_assistant/src/widgets/passive_joints_widget.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.hpp index 1eced6e34b..dc974b5337 100644 --- a/moveit_setup_assistant/src/widgets/passive_joints_widget.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.hpp @@ -37,16 +37,15 @@ #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 +{ +namespace srdf_setup { -class DoubleListWidget; -class PassiveJointsWidget : public SetupScreenWidget +class PassiveJointsWidget : public SetupStepWidget { Q_OBJECT @@ -55,11 +54,16 @@ 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; + SetupStep& getSetupStep() override + { + return setup_step_; + } + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -81,11 +85,11 @@ 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 srdf_setup +} // namespace moveit_setup 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..f62880ae94 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp @@ -0,0 +1,153 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +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(); + } + + InformationFields getInfoField() const override + { + return 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 srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.hpp similarity index 87% rename from moveit_setup_assistant/src/widgets/planning_groups_widget.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.hpp index ef860eddbd..240b25b19d 100644 --- a/moveit_setup_assistant/src/widgets/planning_groups_widget.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.hpp @@ -37,27 +37,25 @@ #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 +{ +namespace srdf_setup { -class DoubleListWidget; -class KinematicChainWidget; -class GroupEditWidget; - // Custom Type enum GroupType { @@ -73,7 +71,7 @@ enum GroupType // CLASS // ****************************************************************************************** // ****************************************************************************************** -class PlanningGroupsWidget : public SetupScreenWidget +class PlanningGroupsWidget : public SetupStepWidget { Q_OBJECT @@ -82,13 +80,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; + SetupStep& getSetupStep() override + { + return setup_step_; + } + private Q_SLOTS: // ****************************************************************************************** @@ -166,8 +169,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 +190,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 +203,6 @@ private Q_SLOTS: /// Switch to current groups view void showMainScreen(); }; -} // namespace moveit_setup_assistant // ****************************************************************************************** // ****************************************************************************************** @@ -219,7 +217,7 @@ class PlanGroupType PlanGroupType() { } - PlanGroupType(srdf::Model::Group* group, const moveit_setup_assistant::GroupType type); + PlanGroupType(srdf::Model::Group* group, const GroupType type); virtual ~PlanGroupType() { ; @@ -227,7 +225,10 @@ class PlanGroupType srdf::Model::Group* group_; - moveit_setup_assistant::GroupType type_; + GroupType type_; }; -Q_DECLARE_METATYPE(PlanGroupType); +} // namespace srdf_setup +} // namespace moveit_setup + +Q_DECLARE_METATYPE(moveit_setup::srdf_setup::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..5d909c3666 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses.hpp @@ -0,0 +1,125 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +// 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 + */ + void publishState(const moveit::core::RobotState& robot_state); + + /** + * @brief Check if the given robot state is in collision + */ + bool checkSelfCollision(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 srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.hpp similarity index 84% rename from moveit_setup_assistant/src/widgets/robot_poses_widget.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.hpp index dcc8377be4..dd31befca9 100644 --- a/moveit_setup_assistant/src/widgets/robot_poses_widget.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.hpp @@ -37,26 +37,25 @@ #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 +{ +namespace srdf_setup { -class RobotPosesWidget : public SetupScreenWidget +class RobotPosesWidget : public SetupStepWidget { Q_OBJECT @@ -65,11 +64,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; + SetupStep& getSetupStep() override + { + return setup_step_; + } + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -133,40 +137,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 +187,9 @@ private Q_SLOTS: /** * Show the robot in the current pose */ - void showPose(srdf::Model::GroupState* pose); + void showPose(const srdf::Model::GroupState& pose); + + void updateStateAndCollision(const moveit::core::RobotState& robot_state); }; // ****************************************************************************************** @@ -279,7 +265,8 @@ private Q_SLOTS: // ****************************************************************************************** }; -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup // Declare std::string as metatype so we can use it in a signal Q_DECLARE_METATYPE(std::string) diff --git a/moveit_setup_assistant/src/tools/rotated_header_view.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/rotated_header_view.hpp similarity index 95% rename from moveit_setup_assistant/src/tools/rotated_header_view.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/rotated_header_view.hpp index 41e371b441..dd34323fba 100644 --- a/moveit_setup_assistant/src/tools/rotated_header_view.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/rotated_header_view.hpp @@ -38,7 +38,9 @@ #include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { class RotatedHeaderView : public QHeaderView { @@ -48,4 +50,5 @@ class RotatedHeaderView : public QHeaderView QSize sectionSizeFromContents(int logicalIndex) const override; int sectionSizeHint(int logicalIndex) const; }; -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup 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..3032b8fa4b --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/srdf_step.hpp @@ -0,0 +1,181 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +/** + * @brief Setup Step that contains the SRDFConfig + */ +class SRDFStep : public 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 + * + * Assuming T is a type that has a name_ field, this provides the following operations on the container + * in which the name_ field is kept unique. + * * find + * * create + * * rename + * * remove + * * get + */ +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 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 srdf_setup +} // namespace moveit_setup 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 new file mode 100644 index 0000000000..68372e2288 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints.hpp @@ -0,0 +1,77 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +class VirtualJoints : public SuperSRDFStep +{ +public: + std::string getName() const override + { + return "Virtual Joints"; + } + + std::vector& getContainer() override + { + return srdf_config_->getVirtualJoints(); + } + + InformationFields getInfoField() const override + { + return VIRTUAL_JOINTS; + } + + void onInit() override; + + 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(); + } + +protected: + std::shared_ptr urdf_config_; +}; +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints_widget.hpp similarity index 79% rename from moveit_setup_assistant/src/widgets/virtual_joints_widget.h rename to moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints_widget.hpp index 75bb0d072e..95735f5f4d 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.h +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints_widget.hpp @@ -36,24 +36,20 @@ #pragma once -// Qt -class QLabel; -class QLineEdit; -class QPushButton; -class QTableWidget; -class QStackedWidget; -class QComboBox; - -// SA -#ifndef Q_MOC_RUN -#include -#endif - -#include "setup_screen_widget.h" // a base class for screens in the setup assistant - -namespace moveit_setup_assistant +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_setup { -class VirtualJointsWidget : public SetupScreenWidget +namespace srdf_setup +{ +class VirtualJointsWidget : public SetupStepWidget { Q_OBJECT @@ -61,12 +57,19 @@ class VirtualJointsWidget : public SetupScreenWidget // ****************************************************************************************** // Public Functions // ****************************************************************************************** + SetupStep& getSetupStep() override + { + return setup_step_; + } - VirtualJointsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data); + void onInit() override; /// Received when this widget is chosen from the navigation menu void focusGiven() override; +protected: + VirtualJoints setup_step_; + // ****************************************************************************************** // Qt Components // ****************************************************************************************** @@ -110,23 +113,10 @@ private Q_SLOTS: /// Cancel changes void cancelEditing(); -Q_SIGNALS: - - // ****************************************************************************************** - // Emitted Signals - // ****************************************************************************************** - - /// Event sent when this widget updated the root joint, which changes the frame of reference for the model - void referenceFrameChanged(); - private: // ****************************************************************************************** // Variables // ****************************************************************************************** - - /// Contains all the configuration data for the setup assistant - moveit_setup_assistant::MoveItConfigDataPtr config_data_; - /// Original name of vjoint currently being edited. This is used to find the element in the vector std::string current_edit_vjoint_; @@ -134,14 +124,6 @@ private Q_SLOTS: // 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::VirtualJoint* findVJointByName(const std::string& name); - /** * Create the main list view of vjoints for robot * @@ -182,4 +164,5 @@ private Q_SLOTS: void edit(const std::string& name); }; -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup 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 new file mode 100644 index 0000000000..094d6cca30 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/moveit_setup_framework_plugins.xml @@ -0,0 +1,26 @@ + + + Step where you can setup the virtual joints in your SRDF + + + Step to ignore self-colisions + + + Step to configure planning groups + + + Step to configure robot controllers + + + 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/package.xml b/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml new file mode 100644 index 0000000000..b62aee17c4 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml @@ -0,0 +1,24 @@ + + + + moveit_setup_srdf_plugins + 2.5.1 + SRDF-based plugins for MoveIt Setup Assistant + David V. Lu!! + BSD + David V. Lu!! + + ament_cmake + + moveit_setup_framework + pluginlib + + ament_lint_auto + ament_clang_format + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/moveit_setup_assistant/src/tools/collision_linear_model.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp similarity index 97% rename from moveit_setup_assistant/src/tools/collision_linear_model.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp index eb07f855bb..0018752460 100644 --- a/moveit_setup_assistant/src/tools/collision_linear_model.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp @@ -34,15 +34,16 @@ /* Author: Robert Haschke */ -#include "collision_linear_model.h" -#include "collision_matrix_model.h" +#include +#include #include #include #include - -using namespace moveit_setup_assistant; - +namespace moveit_setup +{ +namespace srdf_setup +{ CollisionLinearModel::CollisionLinearModel(CollisionMatrixModel* src, QObject* parent) : QAbstractProxyModel(parent) { setSourceModel(src); @@ -239,7 +240,7 @@ void SortFilterProxyModel::setShowAll(bool show_all) bool SortFilterProxyModel::filterAcceptsRow(int source_row, const QModelIndex& source_parent) const { CollisionLinearModel* m = qobject_cast(sourceModel()); - if (!(show_all_ || m->reason(source_row) <= moveit_setup_assistant::ALWAYS || + if (!(show_all_ || m->reason(source_row) <= ALWAYS || m->data(m->index(source_row, 2), Qt::CheckStateRole) == Qt::Checked)) return false; // not accepted due to check state @@ -305,3 +306,5 @@ void SortFilterProxyModel::sort(int column, Qt::SortOrder order) QSortFilterProxyModel::sort(column, Qt::AscendingOrder); endResetModel(); } +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/tools/collision_matrix_model.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_matrix_model.cpp similarity index 74% rename from moveit_setup_assistant/src/tools/collision_matrix_model.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_matrix_model.cpp index eb35a20110..6d8ed29559 100644 --- a/moveit_setup_assistant/src/tools/collision_matrix_model.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_matrix_model.cpp @@ -34,7 +34,7 @@ /* Author: Robert Haschke */ -#include "collision_matrix_model.h" +#include #include #include #include @@ -44,30 +44,31 @@ #include #include -using namespace moveit_setup_assistant; - -/// Boost mapping of reasons for disabling a link pair to strings -static const std::unordered_map LONG_REASONS_TO_STRING = +namespace moveit_setup +{ +namespace srdf_setup +{ +/// Mapping of reasons for disabling a link pair to strings +static const std::unordered_map LONG_REASONS_TO_STRING = boost::assign::map_list_of // clang-format off - ( moveit_setup_assistant::NEVER, "Never in Collision" ) - ( moveit_setup_assistant::DEFAULT, "Collision by Default" ) - ( moveit_setup_assistant::ADJACENT, "Adjacent Links" ) - ( moveit_setup_assistant::ALWAYS, "Always in Collision" ) - ( moveit_setup_assistant::USER, "User Disabled" ) - ( moveit_setup_assistant::NOT_DISABLED, ""); // clang-format on - -/// Boost mapping of reasons to a background color -static const std::unordered_map LONG_REASONS_TO_BRUSH = + ( NEVER, "Never in Collision" ) + ( DEFAULT, "Collision by Default" ) + ( ADJACENT, "Adjacent Links" ) + ( ALWAYS, "Always in Collision" ) + ( USER, "User Disabled" ) + ( NOT_DISABLED, ""); // clang-format on + +/// Mapping of reasons to a background color +static const std::unordered_map LONG_REASONS_TO_BRUSH = boost::assign::map_list_of // clang-format off - ( moveit_setup_assistant::NEVER, QBrush(QColor("lightgreen")) ) - ( moveit_setup_assistant::DEFAULT, QBrush(QColor("lightpink")) ) - ( moveit_setup_assistant::ADJACENT, QBrush(QColor("powderblue")) ) - ( moveit_setup_assistant::ALWAYS, QBrush(QColor("tomato")) ) - ( moveit_setup_assistant::USER, QBrush(QColor("yellow")) ) - ( moveit_setup_assistant::NOT_DISABLED, QBrush()); // clang-format on - -CollisionMatrixModel::CollisionMatrixModel(moveit_setup_assistant::LinkPairMap& pairs, - const std::vector& names, QObject* parent) + ( NEVER, QBrush(QColor("lightgreen")) ) + ( DEFAULT, QBrush(QColor("lightpink")) ) + ( ADJACENT, QBrush(QColor("powderblue")) ) + ( ALWAYS, QBrush(QColor("tomato")) ) + ( USER, QBrush(QColor("yellow")) ) + ( NOT_DISABLED, QBrush()); // clang-format on + +CollisionMatrixModel::CollisionMatrixModel(LinkPairMap& pairs, const std::vector& names, QObject* parent) : QAbstractTableModel(parent), pairs(pairs), std_names(names) { int idx = 0; @@ -79,7 +80,7 @@ CollisionMatrixModel::CollisionMatrixModel(moveit_setup_assistant::LinkPairMap& } // return item in pairs map given a normalized index, use item(normalized(index)) -moveit_setup_assistant::LinkPairMap::iterator CollisionMatrixModel::item(const QModelIndex& index) +LinkPairMap::iterator CollisionMatrixModel::item(const QModelIndex& index) { int r = visual_to_index[index.row()], c = visual_to_index[index.column()]; if (r == c) @@ -107,7 +108,7 @@ QVariant CollisionMatrixModel::data(const QModelIndex& index, int role) const if (index.isValid() && index.row() == index.column() && role == Qt::BackgroundRole) return QApplication::palette().window(); - moveit_setup_assistant::LinkPairMap::const_iterator item = this->item(index); + LinkPairMap::const_iterator item = this->item(index); if (item == pairs.end()) return QVariant(); @@ -123,11 +124,11 @@ QVariant CollisionMatrixModel::data(const QModelIndex& index, int role) const return QVariant(); } -moveit_setup_assistant::DisabledReason CollisionMatrixModel::reason(const QModelIndex& index) const +DisabledReason CollisionMatrixModel::reason(const QModelIndex& index) const { - moveit_setup_assistant::LinkPairMap::const_iterator item = this->item(index); + LinkPairMap::const_iterator item = this->item(index); if (item == pairs.end()) - return moveit_setup_assistant::NOT_DISABLED; + return NOT_DISABLED; return item->second.reason; } @@ -135,7 +136,7 @@ bool CollisionMatrixModel::setData(const QModelIndex& index, const QVariant& val { if (role == Qt::CheckStateRole) { - moveit_setup_assistant::LinkPairMap::iterator item = this->item(index); + LinkPairMap::iterator item = this->item(index); if (item == pairs.end()) return false; @@ -146,12 +147,12 @@ bool CollisionMatrixModel::setData(const QModelIndex& index, const QVariant& val item->second.disable_check = new_value; // Handle USER Reasons: 1) pair is disabled by user - if (item->second.disable_check && item->second.reason == moveit_setup_assistant::NOT_DISABLED) - item->second.reason = moveit_setup_assistant::USER; + if (item->second.disable_check && item->second.reason == NOT_DISABLED) + item->second.reason = USER; // Handle USER Reasons: 2) pair was disabled by user and now is enabled (not checked) - else if (!item->second.disable_check && item->second.reason == moveit_setup_assistant::USER) - item->second.reason = moveit_setup_assistant::NOT_DISABLED; + else if (!item->second.disable_check && item->second.reason == USER) + item->second.reason = NOT_DISABLED; QModelIndex mirror = this->index(index.column(), index.row()); Q_EMIT dataChanged(index, index); @@ -219,3 +220,5 @@ Qt::ItemFlags CollisionMatrixModel::flags(const QModelIndex& index) const f |= Qt::ItemIsUserCheckable; return f; } +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp similarity index 99% rename from moveit_setup_assistant/src/tools/compute_default_collisions.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp index 86b0f110c1..bb345d8a78 100644 --- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp @@ -35,19 +35,21 @@ /* Author: Dave Coleman */ #include -#include +#include #include // for statistics at end #include #include #include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { // ****************************************************************************************** // Custom Types, Enums and Structs // ****************************************************************************************** -// Boost mapping of reasons for disabling a link pair to strings +// Mapping of reasons for disabling a link pair to strings const std::unordered_map REASONS_TO_STRING = boost::assign::map_list_of(NEVER, "Never")( DEFAULT, "Default")(ADJACENT, "Adjacent")(ALWAYS, "Always")(USER, "User")(NOT_DISABLED, "Not Disabled"); @@ -668,4 +670,5 @@ DisabledReason disabledReasonFromString(const std::string& reason) return r; } -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup 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 new file mode 100644 index 0000000000..19969d98a1 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions.cpp @@ -0,0 +1,191 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +std::vector DefaultCollisions::getCollidingLinks() +{ + return srdf_config_->getPlanningScene()->getRobotModel()->getLinkModelNamesWithCollisionGeometry(); +} + +// Output Link Pairs to SRDF Format and update the collision matrix +// ****************************************************************************************** +void DefaultCollisions::linkPairsToSRDF() +{ + // reset the data in the SRDF Writer class + auto& disabled_list = srdf_config_->getDisabledCollisions(); + disabled_list.clear(); + + // Create temp disabled collision + srdf::Model::CollisionPair dc; + + // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format + for (LinkPairMap::const_iterator pair_it = link_pairs_.begin(); pair_it != link_pairs_.end(); ++pair_it) + { + // Only copy those that are actually disabled + if (pair_it->second.disable_check) + { + dc.link1_ = pair_it->first.first; + dc.link2_ = pair_it->first.second; + dc.reason_ = disabledReasonToString(pair_it->second.reason); + disabled_list.push_back(dc); + } + } + srdf_config_->updateRobotModel(COLLISIONS); // mark as changed +} + +void DefaultCollisions::linkPairsToSRDFSorted(size_t skip_mask) +{ + auto& disabled_list = srdf_config_->getDisabledCollisions(); + // Create temp disabled collision + srdf::Model::CollisionPair dc; + + std::set disabled_collisions; + for (auto& p : disabled_list) + { + if (p.link1_ >= p.link2_) + std::swap(p.link1_, p.link2_); // unify link1, link2 sorting + disabled_collisions.insert(p); + } + + // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format + for (const std::pair, LinkPairData>& link_pair : link_pairs_) + { + // Only copy those that are actually disabled + if (link_pair.second.disable_check) + { + if ((1 << link_pair.second.reason) & skip_mask) + continue; + + dc.link1_ = link_pair.first.first; + dc.link2_ = link_pair.first.second; + if (dc.link1_ >= dc.link2_) + std::swap(dc.link1_, dc.link2_); + dc.reason_ = disabledReasonToString(link_pair.second.reason); + + disabled_collisions.insert(dc); + } + } + + disabled_list.assign(disabled_collisions.begin(), disabled_collisions.end()); +} + +// ****************************************************************************************** +// Load Link Pairs from SRDF Format +// ****************************************************************************************** +void DefaultCollisions::linkPairsFromSRDF() +{ + // Clear all the previous data in the compute_default_collisions tool + link_pairs_.clear(); + + // Create new instance of planning scene using pointer + planning_scene::PlanningScenePtr scene = srdf_config_->getPlanningScene()->diff(); + + // Populate link_pairs_ list with every possible n choose 2 combination of links + computeLinkPairs(*scene, link_pairs_); + + // Create temp link pair data struct + LinkPairData link_pair_data; + std::pair link_pair; + + // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created + for (const auto& disabled_collision : srdf_config_->getDisabledCollisions()) + { + // Set the link names + link_pair.first = disabled_collision.link1_; + link_pair.second = disabled_collision.link2_; + if (link_pair.first >= link_pair.second) + std::swap(link_pair.first, link_pair.second); + + // Set the link meta data + link_pair_data.reason = disabledReasonFromString(disabled_collision.reason_); + link_pair_data.disable_check = true; // disable checking the collision btw the 2 links + + // Insert into map + link_pairs_[link_pair] = link_pair_data; + } +} + +void DefaultCollisions::startGenerationThread(unsigned int num_trials, double min_frac, bool verbose) +{ + progress_ = 0; + + // start worker thread + worker_ = + boost::thread([this, num_trials, min_frac, verbose] { generateCollisionTable(num_trials, min_frac, verbose); }); +} + +// ****************************************************************************************** +// The worker function to compute the collision matrix +// ****************************************************************************************** +void DefaultCollisions::generateCollisionTable(unsigned int num_trials, double min_frac, bool verbose) +{ + const bool include_never_colliding = true; + + // clear previously loaded collision matrix entries + srdf_config_->getPlanningScene()->getAllowedCollisionMatrixNonConst().clear(); + + // Find the default collision matrix - all links that are allowed to collide + link_pairs_ = computeDefaultCollisions(srdf_config_->getPlanningScene(), &progress_, include_never_colliding, + num_trials, min_frac, verbose); + + // End the progress bar loop + progress_ = 100; + + RCLCPP_INFO_STREAM(getLogger(), "Thread complete " << link_pairs_.size()); +} + +void DefaultCollisions::cancelGenerationThread() +{ + worker_.interrupt(); +} + +void DefaultCollisions::joinGenerationThread() +{ + worker_.join(); +} + +int DefaultCollisions::getThreadProgress() const +{ + return progress_; +} + +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions_widget.cpp similarity index 81% rename from moveit_setup_assistant/src/widgets/default_collisions_widget.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions_widget.cpp index ce3cd37044..2c28d2f018 100644 --- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions_widget.cpp @@ -34,47 +34,38 @@ /* Author: Dave Coleman */ -#include #include -#include -#include -#include -#include #include #include -#include -#include #include #include -#include -#include #include -#include -#include #include -#include -#include -#include "default_collisions_widget.h" -#include "header_widget.h" -#include "../tools/collision_matrix_model.h" -#include "../tools/collision_linear_model.h" -#include "../tools/rotated_header_view.h" -#include +#include +#include +#include +#include +#include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { // ****************************************************************************************** // User interface for editing the default collision matrix list in an SRDF // ****************************************************************************************** -DefaultCollisionsWidget::DefaultCollisionsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), model_(nullptr), selection_model_(nullptr), worker_(nullptr), config_data_(config_data) +void DefaultCollisionsWidget::onInit() { + model_ = nullptr; + selection_model_ = nullptr; + worker_ = nullptr; + // Basic widget container layout_ = new QVBoxLayout(this); // Top Label Area ------------------------------------------------ - HeaderWidget* header = new HeaderWidget( + auto header = new HeaderWidget( "Optimize Self-Collision Checking", "This searches for pairs of robot links that can safely be disabled from collision checking, decreasing motion " "planning time. These pairs are disabled when they are always in collision, never in collision, in collision in " @@ -234,8 +225,11 @@ void DefaultCollisionsWidget::startGeneratingCollisionTable() disableControls(true); btn_revert_->setEnabled(true); // allow to interrupt and revert + // Start actual worker thread + setup_step_.startGenerationThread(density_slider_->value() * 1000 + 1000, fraction_spinbox_->value() / 100.0); + // create a MonitorThread running generateCollisionTable() in a worker thread and monitoring the progress - worker_ = new MonitorThread([this](unsigned int* progress) { generateCollisionTable(progress); }, progress_bar_); + worker_ = new MonitorThread(setup_step_, progress_bar_); connect(worker_, SIGNAL(finished()), this, SLOT(finishGeneratingCollisionTable())); worker_->start(); // start after having finished() signal connected } @@ -254,42 +248,17 @@ void DefaultCollisionsWidget::finishGeneratingCollisionTable() // Hide the progress bar disableControls(false); // enable everything else - config_data_->changes |= MoveItConfigData::COLLISIONS; worker_->deleteLater(); worker_ = nullptr; } -// ****************************************************************************************** -// The worker function to compute the collision matrix -// ****************************************************************************************** -void DefaultCollisionsWidget::generateCollisionTable(unsigned int* collision_progress) -{ - unsigned int num_trials = density_slider_->value() * 1000 + 1000; // scale to trials amount - double min_frac = (double)fraction_spinbox_->value() / 100.0; - - const bool verbose = true; // Output benchmarking and statistics - const bool include_never_colliding = true; - - // clear previously loaded collision matrix entries - config_data_->getPlanningScene()->getAllowedCollisionMatrixNonConst().clear(); - - // Find the default collision matrix - all links that are allowed to collide - link_pairs_ = moveit_setup_assistant::computeDefaultCollisions( - config_data_->getPlanningScene(), collision_progress, include_never_colliding, num_trials, min_frac, verbose); - - // End the progress bar loop - *collision_progress = 100; - - ROS_INFO_STREAM("Thread complete " << link_pairs_.size()); -} - // ****************************************************************************************** // Displays data in the link_pairs_ data structure into a QtTableWidget // ****************************************************************************************** void DefaultCollisionsWidget::loadCollisionTable() { - CollisionMatrixModel* matrix_model = new CollisionMatrixModel( - link_pairs_, config_data_->getPlanningScene()->getRobotModel()->getLinkModelNamesWithCollisionGeometry()); + CollisionMatrixModel* matrix_model = + new CollisionMatrixModel(setup_step_.getLinkPairs(), setup_step_.getCollidingLinks()); QAbstractItemModel* model; if (view_mode_buttons_->checkedId() == MATRIX_MODE) @@ -554,7 +523,7 @@ void DefaultCollisionsWidget::showSections(QHeaderView* header, const QList void DefaultCollisionsWidget::revertChanges() { - linkPairsFromSRDF(); + setup_step_.linkPairsFromSRDF(); loadCollisionTable(); btn_revert_->setEnabled(false); // no changes to revert } @@ -671,77 +640,13 @@ void DefaultCollisionsWidget::checkedFilterChanged() m->setShowAll(collision_checkbox_->checkState() == Qt::Checked); } -// Output Link Pairs to SRDF Format and update the collision matrix -// ****************************************************************************************** -void DefaultCollisionsWidget::linkPairsToSRDF() -{ - // reset the data in the SRDF Writer class - config_data_->srdf_->disabled_collision_pairs_.clear(); - - // Create temp disabled collision - srdf::Model::CollisionPair dc; - - // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format - for (moveit_setup_assistant::LinkPairMap::const_iterator pair_it = link_pairs_.begin(); pair_it != link_pairs_.end(); - ++pair_it) - { - // Only copy those that are actually disabled - if (pair_it->second.disable_check) - { - dc.link1_ = pair_it->first.first; - dc.link2_ = pair_it->first.second; - dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason); - config_data_->srdf_->disabled_collision_pairs_.push_back(dc); - } - } - - // Update collision_matrix for robot pose's use - config_data_->loadAllowedCollisionMatrix(); -} - -// ****************************************************************************************** -// Load Link Pairs from SRDF Format -// ****************************************************************************************** -void DefaultCollisionsWidget::linkPairsFromSRDF() -{ - // Clear all the previous data in the compute_default_collisions tool - link_pairs_.clear(); - - // Create new instance of planning scene using pointer - planning_scene::PlanningScenePtr scene = config_data_->getPlanningScene()->diff(); - - // Populate link_pairs_ list with every possible n choose 2 combination of links - moveit_setup_assistant::computeLinkPairs(*scene, link_pairs_); - - // Create temp link pair data struct - moveit_setup_assistant::LinkPairData link_pair_data; - std::pair link_pair; - - // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created - for (const auto& disabled_collision : config_data_->srdf_->disabled_collision_pairs_) - { - // Set the link names - link_pair.first = disabled_collision.link1_; - link_pair.second = disabled_collision.link2_; - if (link_pair.first >= link_pair.second) - std::swap(link_pair.first, link_pair.second); - - // Set the link meta data - link_pair_data.reason = moveit_setup_assistant::disabledReasonFromString(disabled_collision.reason_); - link_pair_data.disable_check = true; // disable checking the collision btw the 2 links - - // Insert into map - link_pairs_[link_pair] = link_pair_data; - } -} - // ****************************************************************************************** // Preview whatever element is selected // ****************************************************************************************** void DefaultCollisionsWidget::previewSelectedMatrix(const QModelIndex& index) { // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); if (!index.isValid()) return; @@ -760,14 +665,14 @@ void DefaultCollisionsWidget::previewSelectedMatrix(const QModelIndex& index) uint check_state = model_->data(index, Qt::CheckStateRole).toUInt(); QColor color = (check_state == Qt::Checked) ? QColor(0, 255, 0) : QColor(255, 0, 0); - Q_EMIT highlightLink(first_link.toStdString(), color); - Q_EMIT highlightLink(second_link.toStdString(), color); + rviz_panel_->highlightLink(first_link.toStdString(), color); + rviz_panel_->highlightLink(second_link.toStdString(), color); } void DefaultCollisionsWidget::previewSelectedLinear(const QModelIndex& index) { // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); if (!index.isValid()) return; @@ -778,8 +683,8 @@ void DefaultCollisionsWidget::previewSelectedLinear(const QModelIndex& index) uint check_state = model_->data(model_->index(index.row(), 2), Qt::CheckStateRole).toUInt(); QColor color = (check_state == Qt::Checked) ? QColor(0, 255, 0) : QColor(255, 0, 0); - Q_EMIT highlightLink(first_link.toStdString(), color); - Q_EMIT highlightLink(second_link.toStdString(), color); + rviz_panel_->highlightLink(first_link.toStdString(), color); + rviz_panel_->highlightLink(second_link.toStdString(), color); } // ****************************************************************************************** @@ -788,7 +693,7 @@ void DefaultCollisionsWidget::previewSelectedLinear(const QModelIndex& index) void DefaultCollisionsWidget::focusGiven() { // Convert the SRDF data to LinkPairData format - linkPairsFromSRDF(); + setup_step_.linkPairsFromSRDF(); // Load the data to the table loadCollisionTable(); @@ -810,39 +715,39 @@ bool DefaultCollisionsWidget::focusLost() worker_->wait(); } - // Copy changes to srdf_writer object and config_data_->allowed_collision_matrix_ - linkPairsToSRDF(); + // Copy changes to srdf_writer object + setup_step_.linkPairsToSRDF(); return true; } -moveit_setup_assistant::MonitorThread::MonitorThread(const std::function& f, - QProgressBar* progress_bar) - : progress_(0), canceled_(false) +MonitorThread::MonitorThread(DefaultCollisions& setup_step, QProgressBar* progress_bar) + : setup_step_(setup_step), canceled_(false) { - // start worker thread - worker_ = std::thread([f, progress_ptr = &progress_] { f(progress_ptr); }); // connect progress bar for updates if (progress_bar) connect(this, SIGNAL(progress(int)), progress_bar, SLOT(setValue(int))); } -void moveit_setup_assistant::MonitorThread::run() +void MonitorThread::run() { // loop until worker thread is finished or cancel is requested - while (!canceled_ && progress_ < 100) + int thread_progress; + while (!canceled_ && (thread_progress = setup_step_.getThreadProgress()) < 100) { - Q_EMIT progress(progress_); + Q_EMIT progress(thread_progress); QThread::msleep(100); // sleep 100ms } // cancel worker thread if (canceled_) - worker_.interrupt(); + setup_step_.cancelGenerationThread(); + setup_step_.joinGenerationThread(); - worker_.join(); - - progress_ = 100; - Q_EMIT progress(progress_); + Q_EMIT progress(100); } -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::srdf_setup::DefaultCollisionsWidget, moveit_setup::SetupStepWidget) 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..df69346260 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors.cpp @@ -0,0 +1,66 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +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(END_EFFECTORS); +} + +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/end_effectors_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors_widget.cpp similarity index 79% rename from moveit_setup_assistant/src/widgets/end_effectors_widget.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/end_effectors_widget.cpp index 8366b42d74..70aed67e2b 100644 --- a/moveit_setup_assistant/src/widgets/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,26 @@ #include #include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { // ****************************************************************************************** // 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 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 +247,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 +271,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 +290,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 +321,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 +357,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 +371,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 +387,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 +436,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 +450,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 +457,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 +471,10 @@ 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())) - { - 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 +485,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 +510,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 +525,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 +538,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 +578,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 +598,8 @@ void EndEffectorsWidget::focusGiven() loadParentComboBox(); } -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::srdf_setup::EndEffectorsWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_edit_widget.cpp similarity index 82% rename from moveit_setup_assistant/src/widgets/group_edit_widget.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_edit_widget.cpp index 86944edcef..efa7423143 100644 --- a/moveit_setup_assistant/src/widgets/group_edit_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_edit_widget.cpp @@ -46,16 +46,16 @@ #include #include -#include "group_edit_widget.h" -#include // for loading all avail kinematic planners +#include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { // ****************************************************************************************** // // ****************************************************************************************** -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 +225,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 +262,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 +304,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"); - } - catch (pluginlib::PluginlibException& ex) - { - QMessageBox::warning(this, "Missing Kinematic Solvers", - "Exception while creating class loader for kinematic " - "solver plugins"); - ROS_ERROR_STREAM(ex.what()); - return; + classes = setup_step_.getKinematicPlanners(); } - - // Get classes - const std::vector& classes = loader->getDeclaredClasses(); - - // Warn if no plugins are found - if (classes.empty()) + catch (const std::runtime_error& ex) { - 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 +322,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()); } } @@ -370,9 +338,8 @@ void GroupEditWidget::selectKinematicsFile() } std::string package_name; - std::string relative_filename; - bool package_found = - config_data_->extractPackageNameFromPath(filename.toStdString(), package_name, relative_filename); + std::filesystem::path relative_filename; + bool package_found = extractPackageNameFromPath(filename.toStdString(), package_name, relative_filename); QString lookup_path = filename; if (package_found) @@ -382,4 +349,5 @@ void GroupEditWidget::selectKinematicsFile() kinematics_parameters_file_field_->setText(lookup_path); } -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup 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..3080d49851 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/group_meta_config.cpp @@ -0,0 +1,201 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +bool GroupMetaConfig::isConfigured() const +{ + return !group_meta_data_.empty(); +} + +// ****************************************************************************************** +// Input kinematics.yaml file +// ****************************************************************************************** +void GroupMetaConfig::loadPrevious(const std::filesystem::path& package_path, const YAML::Node& /* node */) +{ + std::filesystem::path 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::filesystem::path& file_path) +{ + // Load file + std::ifstream input_stream(file_path); + 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) +{ + // TODO: Put any additional parameters files into the ROS 2 launch files where they can be read + + // 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(TemplateVariable("KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK", kinematics_parameters_files_block)); +} + +} // namespace srdf_setup +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::srdf_setup::GroupMetaConfig, moveit_setup::SetupConfig) diff --git a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/kinematic_chain_widget.cpp similarity index 78% rename from moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/kinematic_chain_widget.cpp index 4b6b348cc0..63b77967c2 100644 --- a/moveit_setup_assistant/src/widgets/kinematic_chain_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/kinematic_chain_widget.cpp @@ -43,15 +43,17 @@ #include #include #include -#include "kinematic_chain_widget.h" +#include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { // ****************************************************************************************** // Constructor // ****************************************************************************************** -KinematicChainWidget::KinematicChainWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : QWidget(parent), config_data_(config_data) +KinematicChainWidget::KinematicChainWidget(QWidget* parent, RVizPanel* rviz_panel) + : QWidget(parent), rviz_panel_(rviz_panel) { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -138,19 +140,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 +156,17 @@ void KinematicChainWidget::setAvailable() // ****************************************************************************************** // // ****************************************************************************************** -void KinematicChainWidget::addLinktoTreeRecursive(const moveit::core::LinkModel* link, - const moveit::core::LinkModel* parent) +QTreeWidgetItem* KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const LinkNameTree& link) { - // Create new tree item - QTreeWidgetItem* new_item = new QTreeWidgetItem(link_tree_); + QTreeWidgetItem* new_item = new QTreeWidgetItem(parent); + new_item->setText(0, link.data.c_str()); - // Add item to tree - if (parent == nullptr) - { - new_item->setText(0, link->getName().c_str()); - link_tree_->addTopLevelItem(new_item); - } - else + for (const LinkNameTree& child : link.children) { - for (int i = 0; i < link_tree_->topLevelItemCount(); ++i) - { - if (addLinkChildRecursive(link_tree_->topLevelItem(i), link, parent->getName())) - { - break; - } - } + QTreeWidgetItem* new_child = addLinkChildRecursive(new_item, child); + new_item->addChild(new_child); } - 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) -{ - if (parent->text(0).toStdString() == parent_name) - { - QTreeWidgetItem* new_item = new QTreeWidgetItem(parent); - new_item->setText(0, link->getName().c_str()); - - parent->addChild(new_item); - return true; - } - else - { - for (int i = 0; i < parent->childCount(); ++i) - { - if (addLinkChildRecursive(parent->child(i), link, parent_name)) - { - return true; - } - } - } - - return false; + return new_item; } // ****************************************************************************************** @@ -267,7 +221,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 +230,8 @@ 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 srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/tools/xml_manipulation.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints.cpp similarity index 53% rename from moveit_setup_assistant/src/tools/xml_manipulation.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints.cpp index 55093ff9c7..2b7e8eb0c9 100644 --- a/moveit_setup_assistant/src/tools/xml_manipulation.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2022, Bielefeld University, Inc. + * Copyright (c) 2021, PickNik Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * 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 Bielefeld University nor the names of its + * * 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. * @@ -32,53 +32,54 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Robert Haschke */ +/* Author: David V. Lu!! */ -#include +#include -namespace moveit_setup_assistant +namespace moveit_setup { -namespace +namespace srdf_setup { -bool hasRequiredAttributes(const TiXmlElement& e, const std::vector& attributes) +std::vector PassiveJoints::getActiveJoints() const { - for (const auto& attr : attributes) + 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 (!attr.required) - continue; // attribute not required - const char* value = e.Attribute(attr.name); - if (value && strcmp(attr.value, value) == 0) - continue; // attribute has required value - else - return false; + if (model->getJointModel(joint)->getVariableCount() > 0) + { + active_joints.push_back(joint); + } } - return true; -}; -} // namespace + return active_joints; +} -TiXmlElement* uniqueInsert(TiXmlElement& element, const char* tag, const std::vector& attributes, - const char* text) +std::vector PassiveJoints::getPassiveJoints() const { - // search for existing element with required tag name and attributes - TiXmlElement* result = element.FirstChildElement(tag); - while (result && !hasRequiredAttributes(*result, attributes)) - result = result->NextSiblingElement(tag); - - if (!result) // if not yet present, create new element - result = element.InsertEndChild(TiXmlElement(tag))->ToElement(); - - // set (not-yet existing) attributes - for (const auto& attr : attributes) + std::vector passive_joints; + for (const srdf::Model::PassiveJoint& passive_joint : srdf_config_->getPassiveJoints()) { - if (!result->Attribute(attr.name)) - result->SetAttribute(attr.name, attr.value); + passive_joints.push_back(passive_joint.name_); } + return passive_joints; +} - // insert text if required - if (text && !result->GetText()) - result->InsertEndChild(TiXmlText(text)); - - return result; +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(PASSIVE_JOINTS); } -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/passive_joints_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints_widget.cpp similarity index 65% rename from moveit_setup_assistant/src/widgets/passive_joints_widget.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/passive_joints_widget.cpp index 85610b4404..e9b9b29aba 100644 --- a/moveit_setup_assistant/src/widgets/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,28 @@ #include #include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { // ****************************************************************************************** // 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 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 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 +88,18 @@ void PassiveJointsWidget::focusGiven() { joints_widget_->clearContents(); - // 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(); + std::vector active_joints = setup_step_.getActiveJoints(); - 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 +107,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 +116,24 @@ 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 srdf_setup +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::srdf_setup::PassiveJointsWidget, moveit_setup::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..e27adc2432 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp @@ -0,0 +1,448 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +// 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_setup::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 |= 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 |= 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 |= 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 |= POSES; + } + } + + group_meta_config_->renameGroup(old_group_name, new_group_name); + changes |= 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 |= 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 |= GROUP_CONTENTS; + subgroup_it = std::find(subgroups.begin(), subgroups.end(), group_name); + } + } + + group_meta_config_->deleteGroup(group_name); + + srdf_config_->updateRobotModel(changes); +} + +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(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(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(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(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 srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/planning_groups_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp similarity index 62% rename from moveit_setup_assistant/src/widgets/planning_groups_widget.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp index be7b6bf5a5..e225435b79 100644 --- a/moveit_setup_assistant/src/widgets/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,23 @@ #include #include -//// Cycle checking -#include -#include - -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { // 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 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 +97,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 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 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 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 +229,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 +240,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 +290,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 +299,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 +371,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 +388,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 +408,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 +460,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 +488,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 +516,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 +551,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 +601,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 +609,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 +619,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 +637,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) - { - 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 - } - } - } - - // 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 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_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) + // 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) { - // 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; - } + return; } - // 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 +688,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 +702,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_.setLinks(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 +716,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())) - { - QMessageBox::warning(this, "Error Saving", - "You must specify a link for both the base and tip, or leave both " - "blank."); - return; - } - - // Check that both given links are valid links, unless they are both blank - if (!tip.empty() && !base.empty()) + try { - // 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; - } + setup_step_.setChain(current_edit_group_, base, tip); } - - // clear the old data - searched_group->chains_.clear(); - - // Save the data if there is data to save - if (!tip.empty() && !base.empty()) + catch (const std::runtime_error& e) { - searched_group->chains_.push_back(std::pair(base, tip)); + QMessageBox::warning(this, "Error Saving", e.what()); + return; } // Switch to main screen @@ -943,10 +735,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 +742,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_) - { - // 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 : config_data_->srdf_->groups_) + try { - // 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; + setup_step_.setSubgroups(current_edit_group_, subgroups_widget_->getSelectedValues()); } - - // Check for cycles - bool has_cycle = false; - CycleDetector vis(has_cycle); - boost::depth_first_search(g, visitor(vis)); - - if (has_cycle) + catch (const std::runtime_error& e) { - // 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 +764,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,148 +782,54 @@ 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 = std::stod(kinematics_resolution); + meta_data.kinematics_solver_search_resolution_ = boost::lexical_cast(kinematics_resolution); } - catch (std::invalid_argument&) + catch (boost::bad_lexical_cast&) { QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics resolution to a double number."); return false; } // 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 = std::stod(kinematics_timeout); + meta_data.kinematics_solver_timeout_ = boost::lexical_cast(kinematics_timeout); } - catch (std::invalid_argument&) + catch (boost::bad_lexical_cast&) { QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics solver timeout to a double number."); return false; } // 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 +862,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 +879,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 +896,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 +913,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 +934,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 +980,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 +991,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 +1000,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 +1010,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 +1020,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,24 +1041,27 @@ 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 - // ****************************************************************************************** // ****************************************************************************************** // CLASS // ****************************************************************************************** // ****************************************************************************************** -PlanGroupType::PlanGroupType(srdf::Model::Group* group, const moveit_setup_assistant::GroupType type) - : group_(group), type_(type) +PlanGroupType::PlanGroupType(srdf::Model::Group* group, const GroupType type) : group_(group), type_(type) { } + +} // namespace srdf_setup +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::srdf_setup::PlanningGroupsWidget, moveit_setup::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..2d907c92b5 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp @@ -0,0 +1,163 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +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 +// ****************************************************************************************** +void 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); +} + +bool RobotPoses::checkSelfCollision(const moveit::core::RobotState& robot_state) +{ + // 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(POSES); +} + +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/robot_poses_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp similarity index 78% rename from moveit_setup_assistant/src/widgets/robot_poses_widget.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp index 9ee39e252e..06b73ab454 100644 --- a/moveit_setup_assistant/src/widgets/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,14 @@ #include #include -#include -#include -#include - -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { // ****************************************************************************************** // 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 +69,11 @@ 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 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 +88,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 +284,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 +308,32 @@ 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(); + updateStateAndCollision(robot_state); // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); // Highlight group - Q_EMIT highlightGroup(pose->group_); + rviz_panel_->highlightGroup(pose.group_); } // ****************************************************************************************** @@ -363,14 +341,14 @@ 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(); + updateStateAndCollision(robot_state); // Unhighlight all links - Q_EMIT unhighlightAll(); + rviz_panel_->unhighlightAll(); } // ****************************************************************************************** @@ -378,15 +356,16 @@ void RobotPosesWidget::showDefaultPose() // ****************************************************************************************** void RobotPosesWidget::playPoses() { + using namespace std::chrono_literals; + // 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); + rclcpp::sleep_for(50000000ns); // 0.05 s QApplication::processEvents(); - ros::Duration(0.45).sleep(); + rclcpp::sleep_for(450000000ns); // 0.45 s } } @@ -410,7 +389,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 +404,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 +425,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 +444,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 +471,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 +485,17 @@ 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(); + updateStateAndCollision(robot_state); // 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,20 @@ 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(); + updateStateAndCollision(robot_state); } -// ****************************************************************************************** -// Publish the current RobotState to Rviz -// ****************************************************************************************** -void RobotPosesWidget::publishJoints() +void RobotPosesWidget::updateStateAndCollision(const moveit::core::RobotState& robot_state) { - // 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()); + setup_step_.publishState(robot_state); + + // if in collision, show warning + // if no collision, hide warning + collision_warning_->setHidden(!setup_step_.checkSelfCollision(robot_state)); } // ****************************************************************************************** @@ -904,4 +811,8 @@ void SliderWidget::changeJointSlider() Q_EMIT jointValueChanged(joint_model_->getName(), value); } -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::srdf_setup::RobotPosesWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/src/tools/rotated_header_view.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/rotated_header_view.cpp similarity index 96% rename from moveit_setup_assistant/src/tools/rotated_header_view.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/rotated_header_view.cpp index dc6be7bcec..3ccfaf4e04 100644 --- a/moveit_setup_assistant/src/tools/rotated_header_view.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/rotated_header_view.cpp @@ -34,10 +34,12 @@ /* Author: Robert Haschke */ -#include "rotated_header_view.h" +#include #include -namespace moveit_setup_assistant +namespace moveit_setup +{ +namespace srdf_setup { RotatedHeaderView::RotatedHeaderView(Qt::Orientation orientation, QWidget* parent) : QHeaderView(orientation, parent) { @@ -126,4 +128,5 @@ int RotatedHeaderView::sectionSizeHint(int logicalIndex) const int hint = size.height(); return qMax(minimumSectionSize(), hint); } -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup 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 new file mode 100644 index 0000000000..8845b7e3bf --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints.cpp @@ -0,0 +1,58 @@ +/********************************************************************* + * 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 +{ +namespace srdf_setup +{ +void VirtualJoints::onInit() +{ + SuperSRDFStep::onInit(); + urdf_config_ = config_data_->get("urdf"); +} + +void VirtualJoints::setProperties(srdf::Model::VirtualJoint* vj, const std::string& parent_name, + const std::string& child_name, const std::string& joint_type) +{ + vj->parent_frame_ = parent_name; + vj->child_link_ = child_name; + vj->type_ = joint_type; + srdf_config_->updateRobotModel(VIRTUAL_JOINTS); +} +} // namespace srdf_setup +} // namespace moveit_setup diff --git a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints_widget.cpp similarity index 77% rename from moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp rename to moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints_widget.cpp index b5775c04d2..578cf40879 100644 --- a/moveit_setup_assistant/src/widgets/virtual_joints_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints_widget.cpp @@ -34,42 +34,32 @@ /* Author: Dave Coleman */ -// SA -#include "virtual_joints_widget.h" -#include "header_widget.h" +#include +#include // Qt #include -#include #include #include -#include -#include #include -#include -#include #include -#include #include -namespace moveit_setup_assistant +namespace moveit_setup { -// ****************************************************************************************** -// Constructor -// ****************************************************************************************** -VirtualJointsWidget::VirtualJointsWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) +namespace srdf_setup +{ +void VirtualJointsWidget::onInit() { // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); // Top Header Area ------------------------------------------------ - HeaderWidget* header = - new HeaderWidget("Define Virtual Joints", - "Create a virtual joint between the base robot link and an external frame of reference. " - "This allows to place the robot in the world or on a mobile platform.", - this); + auto header = new HeaderWidget("Define Virtual Joints", + "Create a virtual joint between the base robot link and an external frame of " + "reference. This allows to place the robot in the world or on a mobile platform.", + this); layout->addWidget(header); // Create contents screens --------------------------------------- @@ -242,7 +232,7 @@ void VirtualJointsWidget::showNewScreen() stacked_widget_->setCurrentIndex(1); // Announce that this widget is in modal mode - Q_EMIT isModal(true); + Q_EMIT setModalMode(true); } // ****************************************************************************************** @@ -285,7 +275,14 @@ void VirtualJointsWidget::edit(const std::string& name) current_edit_vjoint_ = name; // Find the selected in datastruture - srdf::Model::VirtualJoint* vjoint = findVJointByName(name); + srdf::Model::VirtualJoint* vjoint = setup_step_.find(name); + + // Check if vjoint was found + if (vjoint == nullptr) // not found + { + QMessageBox::critical(this, "Error Saving", "An internal error has occurred while saving. Quitting."); + QApplication::quit(); + } // Set vjoint name vjoint_name_field_->setText(vjoint->name_.c_str()); @@ -313,7 +310,7 @@ void VirtualJointsWidget::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); } // ****************************************************************************************** @@ -338,42 +335,11 @@ void VirtualJointsWidget::loadChildLinksComboBox() // Remove all old links child_link_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) - { - child_link_field_->addItem((*link_it)->getName().c_str()); - } -} - -// ****************************************************************************************** -// Find the associated data by name -// ****************************************************************************************** -srdf::Model::VirtualJoint* VirtualJointsWidget::findVJointByName(const std::string& name) -{ - // 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 : config_data_->srdf_->virtual_joints_) - { - if (virtual_joint.name_ == name) // string match - { - searched_group = &virtual_joint; // convert to pointer from iterator - break; // we are done searching - } - } - - // Check if vjoint was found - if (searched_group == nullptr) // not found + for (const auto& link_name : setup_step_.getLinkNames()) { - QMessageBox::critical(this, "Error Saving", "An internal error has occurred while saving. Quitting."); - QApplication::quit(); + child_link_field_->addItem(link_name.c_str()); } - - return searched_group; } // ****************************************************************************************** @@ -402,21 +368,11 @@ void VirtualJointsWidget::deleteSelected() } // Delete vjoint from vector - for (std::vector::iterator vjoint_it = config_data_->srdf_->virtual_joints_.begin(); - vjoint_it != config_data_->srdf_->virtual_joints_.end(); ++vjoint_it) - { - // check if this is the group we want to delete - if (vjoint_it->name_ == current_edit_vjoint_) // string match - { - config_data_->srdf_->virtual_joints_.erase(vjoint_it); - break; - } - } + setup_step_.remove(current_edit_vjoint_); // Reload main screen table loadDataTable(); - config_data_->changes |= MoveItConfigData::VIRTUAL_JOINTS; - Q_EMIT referenceFrameChanged(); + rviz_panel_->updateFixedFrame(); } // ****************************************************************************************** @@ -427,9 +383,8 @@ void VirtualJointsWidget::doneEditing() // Get a reference to the supplied strings const std::string vjoint_name = vjoint_name_field_->text().trimmed().toStdString(); const std::string parent_name = parent_name_field_->text().trimmed().toStdString(); - - // Used for editing existing groups - srdf::Model::VirtualJoint* searched_data = nullptr; + const std::string child_link = child_link_field_->currentText().trimmed().toStdString(); + const std::string joint_type = joint_type_field_->currentText().trimmed().toStdString(); // Check that name field is not empty if (vjoint_name.empty()) @@ -445,68 +400,29 @@ void VirtualJointsWidget::doneEditing() return; } - // Check if this is an existing vjoint - if (!current_edit_vjoint_.empty()) - { - // Find the group we are editing based on the goup name string - searched_data = findVJointByName(current_edit_vjoint_); - } - - // Check that the vjoint name is unique - for (const auto& virtual_joint : config_data_->srdf_->virtual_joints_) - { - if (virtual_joint.name_.compare(vjoint_name) == 0) // the names are the same - { - // is this our existing vjoint? check if vjoint pointers are same - if (&virtual_joint != searched_data) - { - QMessageBox::warning(this, "Error Saving", "A virtual joint already exists with that name!"); - return; - } - } - } - // Check that a joint type was selected - if (joint_type_field_->currentText().isEmpty()) + if (joint_type.empty()) { QMessageBox::warning(this, "Error Saving", "A joint type must be chosen!"); return; } // Check that a child link was selected - if (child_link_field_->currentText().isEmpty()) + if (child_link.empty()) { QMessageBox::warning(this, "Error Saving", "A child link must be chosen!"); return; } - config_data_->changes |= MoveItConfigData::VIRTUAL_JOINTS; - - // Save the new vjoint name or create the new vjoint ---------------------------- - bool is_new = false; - - if (searched_data == nullptr) // create new + try { - is_new = true; - searched_data = new srdf::Model::VirtualJoint(); + srdf::Model::VirtualJoint* vj = setup_step_.get(vjoint_name, current_edit_vjoint_); + setup_step_.setProperties(vj, parent_name, child_link, joint_type); } - - // Copy name data ---------------------------------------------------- - searched_data->name_ = vjoint_name; - searched_data->parent_frame_ = parent_name; - searched_data->child_link_ = child_link_field_->currentText().toStdString(); - searched_data->type_ = joint_type_field_->currentText().toStdString(); - - bool emit_frame_notice = false; - - // Insert new vjoints into group state vector -------------------------- - if (is_new) + catch (const std::runtime_error& e) { - if (searched_data->child_link_ == config_data_->getRobotModel()->getRootLinkName()) - emit_frame_notice = true; - config_data_->srdf_->virtual_joints_.push_back(*searched_data); - config_data_->updateRobotModel(); - delete searched_data; + QMessageBox::warning(this, "Error Saving", e.what()); + return; } // Finish up ------------------------------------------------------ @@ -518,13 +434,9 @@ void VirtualJointsWidget::doneEditing() stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode - Q_EMIT isModal(false); + Q_EMIT setModalMode(false); - // if the root frame changed for the robot, emit the signal - if (emit_frame_notice) - { - Q_EMIT referenceFrameChanged(); - } + rviz_panel_->updateFixedFrame(); } // ****************************************************************************************** @@ -536,7 +448,7 @@ void VirtualJointsWidget::cancelEditing() stacked_widget_->setCurrentIndex(0); // Announce that this widget is not in modal mode - Q_EMIT isModal(false); + Q_EMIT setModalMode(false); } // ****************************************************************************************** @@ -549,12 +461,14 @@ void VirtualJointsWidget::loadDataTable() data_table_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called data_table_->clearContents(); + const auto& virtual_joints = setup_step_.getContainer(); + // Set size of datatable - data_table_->setRowCount(config_data_->srdf_->virtual_joints_.size()); + data_table_->setRowCount(virtual_joints.size()); // Loop through every virtual joint int row = 0; - for (const auto& virtual_joint : config_data_->srdf_->virtual_joints_) + for (const auto& virtual_joint : virtual_joints) { // Create row elements QTableWidgetItem* data_name = new QTableWidgetItem(virtual_joint.name_.c_str()); @@ -587,7 +501,7 @@ void VirtualJointsWidget::loadDataTable() data_table_->resizeColumnToContents(3); // Show edit button if applicable - if (!config_data_->srdf_->virtual_joints_.empty()) + if (!virtual_joints.empty()) btn_edit_->show(); } @@ -606,4 +520,8 @@ void VirtualJointsWidget::focusGiven() loadChildLinksComboBox(); } -} // namespace moveit_setup_assistant +} // namespace srdf_setup +} // namespace moveit_setup + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(moveit_setup::srdf_setup::VirtualJointsWidget, moveit_setup::SetupStepWidget) diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp deleted file mode 100644 index 11cd70266d..0000000000 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ /dev/null @@ -1,262 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, Fraunhofer IPA - * 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 Fraunhofer IPA 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: Mathias Lüdtke */ - -#include -#include -#include -#include -#include - -namespace po = boost::program_options; - -bool loadSetupAssistantConfig(moveit_setup_assistant::MoveItConfigData& config_data, const std::string& pkg_path) -{ - static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); - if (!config_data.setPackagePath(pkg_path)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not set package path '" << pkg_path << "'"); - return false; - } - - std::string setup_assistant_path; - if (!config_data.getSetupAssistantYAMLPath(setup_assistant_path)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not resolve path to .setup_assistant"); - return false; - } - - if (!config_data.inputSetupAssistantYAML(setup_assistant_path)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not parse .setup_assistant file from '" << setup_assistant_path << "'"); - return false; - } - - config_data.createFullURDFPath(); // might fail at this point - - config_data.createFullSRDFPath(config_data.config_pkg_path_); // might fail at this point - - return true; -} - -bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old, - const std::vector& xacro_args) -{ - static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); - std::string urdf_string; - if (!rdf_loader::RDFLoader::loadXmlFileToString(urdf_string, config_data.urdf_path_, xacro_args)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not load URDF from '" << config_data.urdf_path_ << "'"); - return false; - } - if (!config_data.urdf_model_->initString(urdf_string)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not parse URDF from '" << config_data.urdf_path_ << "'"); - return false; - } - - std::string srdf_string; - if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, config_data.srdf_path_, xacro_args)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not load SRDF from '" << config_data.srdf_path_ << "'"); - return false; - } - if (!config_data.srdf_->initString(*config_data.urdf_model_, srdf_string)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not parse SRDF from '" << config_data.srdf_path_ << "'"); - return false; - } - - if (!keep_old) - { - config_data.srdf_->no_default_collision_links_.clear(); - config_data.srdf_->enabled_collision_pairs_.clear(); - config_data.srdf_->disabled_collision_pairs_.clear(); - } - - return true; -} - -moveit_setup_assistant::LinkPairMap compute(moveit_setup_assistant::MoveItConfigData& config_data, uint32_t trials, - double min_collision_fraction, bool verbose) -{ - // TODO: spin thread and print progress if verbose - unsigned int collision_progress; - return moveit_setup_assistant::computeDefaultCollisions(config_data.getPlanningScene(), &collision_progress, - trials > 0, trials, min_collision_fraction, verbose); -} - -// less operation for two CollisionPairs -struct CollisionPairLess -{ - bool operator()(const srdf::Model::CollisionPair& left, const srdf::Model::CollisionPair& right) const - { - // use std::pair's operator<: (left.link1_, left.link2_) < (right.link1_, right.link2_) - return left.link1_ < right.link1_ || (!(right.link1_ < left.link1_) && left.link2_ < right.link2_); - } -}; - -// Update collision pairs -void updateCollisionLinkPairs(std::vector& target_pairs, - const moveit_setup_assistant::LinkPairMap& source_pairs, size_t skip_mask) -{ - // remove duplicates - std::set filtered; - for (auto& p : target_pairs) - { - if (p.link1_ >= p.link2_) - std::swap(p.link1_, p.link2_); // unify link1, link2 sorting - filtered.insert(p); - } - - // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format - for (const auto& link_pair : source_pairs) - { - // Only copy those that are actually disabled - if (!link_pair.second.disable_check) - continue; - - // Filter out pairs matching the skip_mask - if ((1 << link_pair.second.reason) & skip_mask) - continue; - - srdf::Model::CollisionPair pair; - pair.link1_ = link_pair.first.first; - pair.link2_ = link_pair.first.second; - if (pair.link1_ >= pair.link2_) - std::swap(pair.link1_, pair.link2_); - pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason); - - filtered.insert(pair); - } - - target_pairs.assign(filtered.begin(), filtered.end()); -} - -int main(int argc, char* argv[]) -{ - std::string config_pkg_path; - std::string urdf_path; - std::string srdf_path; - std::string output_path; - - bool include_default = false, include_always = false, keep_old = false, verbose = false; - - double min_collision_fraction = 1.0; - - uint32_t never_trials = 0; - - // clang-format off - po::options_description desc("Allowed options"); - desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to MoveIt config package")( - "urdf", po::value(&urdf_path), "path to URDF ( or xacro)")("srdf", po::value(&srdf_path), - "path to SRDF ( or xacro)")( - "output", po::value(&output_path), - "output path for SRDF")("xacro-args", po::value >()->composing(), - "additional arguments for xacro")("default", po::bool_switch(&include_default), - "disable default colliding pairs")( - "always", po::bool_switch(&include_always), "disable always colliding pairs")("keep", po::bool_switch(&keep_old), - "keep disabled link from SRDF")( - "verbose", po::bool_switch(&verbose), "verbose output")("trials", po::value(&never_trials), - "number of trials for searching never colliding pairs")( - "min-collision-fraction", po::value(&min_collision_fraction), - "fraction of small sample size to determine links that are always colliding"); - // clang-format on - - po::positional_options_description pos_desc; - pos_desc.add("xacro-args", -1); - - po::variables_map vm; - po::store(po::command_line_parser(argc, argv).options(desc).positional(pos_desc).run(), vm); - po::notify(vm); - - if (vm.count("help")) - { - std::cout << desc << '\n'; - return 1; - } - - moveit_setup_assistant::MoveItConfigData config_data; - static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); - - if (!config_pkg_path.empty()) - { - if (!loadSetupAssistantConfig(config_data, config_pkg_path)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not load config at '" << config_pkg_path << "'"); - return 1; - } - } - else if (urdf_path.empty() || srdf_path.empty()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Please provide config package or URDF and SRDF path"); - return 1; - } - else if (rdf_loader::RDFLoader::isXacroFile(srdf_path) && output_path.empty()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Please provide a different output file for SRDF xacro input file"); - return 1; - } - - // overwrite config paths if applicable - if (!urdf_path.empty()) - config_data.urdf_path_ = urdf_path; - if (!srdf_path.empty()) - config_data.srdf_path_ = srdf_path; - - std::vector xacro_args; - if (vm.count("xacro-args")) - xacro_args = vm["xacro-args"].as >(); - - if (!setup(config_data, keep_old, xacro_args)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Could not setup updater"); - return 1; - } - - moveit_setup_assistant::LinkPairMap link_pairs = compute(config_data, never_trials, min_collision_fraction, verbose); - - size_t skip_mask = 0; - if (!include_default) - skip_mask |= (1 << moveit_setup_assistant::DEFAULT); - if (!include_always) - skip_mask |= (1 << moveit_setup_assistant::ALWAYS); - - updateCollisionLinkPairs(config_data.srdf_->disabled_collision_pairs_, link_pairs, skip_mask); - - config_data.srdf_->writeSRDF(output_path.empty() ? config_data.srdf_path_ : output_path); - - return 0; -} diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp deleted file mode 100644 index 3d84e0a8fe..0000000000 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ /dev/null @@ -1,1901 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Dave Coleman */ - -#include -// Reading/Writing Files -#include // For writing yaml and launch files -#include -#include -#include -#include -#include // for creating folders/files -#include // for getting file path for loading images -// OMPL version -#include -#include - -namespace moveit_setup_assistant -{ -// File system -namespace fs = std::filesystem; - -// ****************************************************************************************** -// Constructor -// ****************************************************************************************** -MoveItConfigData::MoveItConfigData() : config_pkg_generated_timestamp_(0) -{ - // Create an instance of SRDF writer and URDF model for all widgets to share - srdf_ = std::make_shared(); - urdf_model_ = std::make_shared(); - - // Not in debug mode - debug_ = false; - - // Get MoveIt Setup Assistant package path - setup_assistant_path_ = ament_index_cpp::get_package_share_directory("moveit_setup_assistant"); - if (setup_assistant_path_.empty()) - { - setup_assistant_path_ = "."; - } -} - -// ****************************************************************************************** -// Destructor -// ****************************************************************************************** -MoveItConfigData::~MoveItConfigData() = default; - -// ****************************************************************************************** -// Load a robot model -// ****************************************************************************************** -void MoveItConfigData::setRobotModel(const moveit::core::RobotModelPtr& robot_model) -{ - robot_model_ = robot_model; -} - -// ****************************************************************************************** -// Provide a kinematic model. Load a new one if necessary -// ****************************************************************************************** -moveit::core::RobotModelConstPtr MoveItConfigData::getRobotModel() -{ - if (!robot_model_) - { - // Initialize with a URDF Model Interface and a SRDF Model - robot_model_ = std::make_shared(urdf_model_, srdf_->srdf_model_); - } - - return robot_model_; -} - -// ****************************************************************************************** -// Update the Kinematic Model with latest SRDF modifications -// ****************************************************************************************** -void MoveItConfigData::updateRobotModel() -{ - RCLCPP_INFO_STREAM(LOGGER, "Updating kinematic model"); - - // Tell SRDF Writer to create new SRDF Model, use original URDF model - srdf_->updateSRDFModel(*urdf_model_); - - // Create new kin model - robot_model_ = std::make_shared(urdf_model_, srdf_->srdf_model_); - - // Reset the planning scene - planning_scene_.reset(); -} - -// ****************************************************************************************** -// Provide a shared planning scene -// ****************************************************************************************** -planning_scene::PlanningScenePtr MoveItConfigData::getPlanningScene() -{ - if (!planning_scene_) - { - // make sure kinematic model exists - getRobotModel(); - - // Allocate an empty planning scene - planning_scene_ = std::make_shared(robot_model_); - } - return planning_scene_; -} - -// ****************************************************************************************** -// Load the allowed collision matrix from the SRDF's list of link pairs -// ****************************************************************************************** -void MoveItConfigData::loadAllowedCollisionMatrix() -{ - allowed_collision_matrix_.clear(); - - // load collision defaults - for (const std::string& name : srdf_->no_default_collision_links_) - allowed_collision_matrix_.setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); - // re-enable specific collision pairs - for (auto const& collision : srdf_->enabled_collision_pairs_) - allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, false); - // *finally* disable selected collision pairs - for (auto const& collision : srdf_->disabled_collision_pairs_) - allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, true); -} - -// ****************************************************************************************** -// Output MoveIt Setup Assistant hidden settings file -// ****************************************************************************************** -bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path) -{ - YAML::Emitter emitter; - emitter << YAML::BeginMap; - - // Output every available planner --------------------------------------------------- - emitter << YAML::Key << "moveit_setup_assistant_config"; - - emitter << YAML::Value << YAML::BeginMap; - - // URDF Path Location - emitter << YAML::Key << "URDF"; - emitter << YAML::Value << YAML::BeginMap; - emitter << YAML::Key << "package" << YAML::Value << urdf_pkg_name_; - emitter << YAML::Key << "relative_path" << YAML::Value << urdf_pkg_relative_path_; - emitter << YAML::Key << "xacro_args" << YAML::Value << xacro_args_; - emitter << YAML::EndMap; - - /// SRDF Path Location - emitter << YAML::Key << "SRDF"; - emitter << YAML::Value << YAML::BeginMap; - emitter << YAML::Key << "relative_path" << YAML::Value << srdf_pkg_relative_path_; - emitter << YAML::EndMap; - - /// Package generation time - emitter << YAML::Key << "CONFIG"; - emitter << YAML::Value << YAML::BeginMap; - emitter << YAML::Key << "author_name" << YAML::Value << author_name_; - emitter << YAML::Key << "author_email" << YAML::Value << author_email_; - auto cur_time = std::time(nullptr); - emitter << YAML::Key << "generated_timestamp" << YAML::Value << cur_time; // TODO: is this cross-platform? - 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(); - - /// Update the parsed setup_assistant timestamp - // NOTE: Needed for when people run the MSA generator multiple times in a row. - config_pkg_generated_timestamp_ = cur_time; - - return true; // file created successfully -} - -// ****************************************************************************************** -// Output Gazebo URDF file -// ****************************************************************************************** -bool MoveItConfigData::outputGazeboURDFFile(const std::string& file_path) -{ - std::ofstream os(file_path.c_str(), std::ios_base::trunc); - if (!os.good()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to open file for writing " << file_path); - return false; - } - - os << gazebo_urdf_string_.c_str() << std::endl; - os.close(); - - return true; -} - -// ****************************************************************************************** -// Output OMPL Planning config files -// ****************************************************************************************** -bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path) -{ - YAML::Emitter emitter; - emitter << YAML::BeginMap; - - // Output every available planner --------------------------------------------------- - emitter << YAML::Key << "planner_configs"; - - emitter << YAML::Value << YAML::BeginMap; - - std::vector planner_des = getOMPLPlanners(); - - // Add Planners with parameter values - std::vector pconfigs; - for (OMPLPlannerDescription& planner_de : planner_des) - { - std::string defaultconfig = planner_de.name_; - emitter << YAML::Key << defaultconfig; - emitter << YAML::Value << YAML::BeginMap; - emitter << YAML::Key << "type" << YAML::Value << "geometric::" + planner_de.name_; - for (OmplPlanningParameter& ompl_planner_param : planner_de.parameter_list_) - { - emitter << YAML::Key << ompl_planner_param.name; - emitter << YAML::Value << ompl_planner_param.value; - emitter << YAML::Comment(ompl_planner_param.comment); - } - emitter << YAML::EndMap; - - pconfigs.push_back(defaultconfig); - } - - // End of every avail planner - emitter << YAML::EndMap; - - // Output every group and the planners it can use ---------------------------------- - for (srdf::Model::Group& group : srdf_->groups_) - { - emitter << YAML::Key << group.name_; - emitter << YAML::Value << YAML::BeginMap; - // Output associated planners - if (!group_meta_data_[group.name_].default_planner_.empty()) - emitter << YAML::Key << "default_planner_config" << YAML::Value << group_meta_data_[group.name_].default_planner_; - emitter << YAML::Key << "planner_configs"; - emitter << YAML::Value << YAML::BeginSeq; - for (const std::string& pconfig : pconfigs) - emitter << pconfig; - emitter << YAML::EndSeq; - - // Output projection_evaluator - std::string projection_joints = decideProjectionJoints(group.name_); - if (!projection_joints.empty()) - { - emitter << YAML::Key << "projection_evaluator"; - emitter << YAML::Value << projection_joints; - // OMPL collision checking discretization - emitter << YAML::Key << "longest_valid_segment_fraction"; - emitter << YAML::Value << "0.005"; - } - - 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() << '\n'; - output_stream.close(); - - return true; // file created successfully -} - -// ****************************************************************************************** -// Output STOMP Planning config files -// ****************************************************************************************** -bool MoveItConfigData::outputSTOMPPlanningYAML(const std::string& file_path) -{ - YAML::Emitter emitter; - emitter << YAML::BeginMap; - - // Add STOMP default for every group - for (srdf::Model::Group& group : srdf_->groups_) - { - emitter << YAML::Key << "stomp/" + group.name_; - emitter << YAML::BeginMap; - emitter << YAML::Key << "group_name"; - emitter << YAML::Value << group.name_; - - emitter << YAML::Key << "optimization"; - emitter << YAML::BeginMap; - emitter << YAML::Key << "num_timesteps"; - emitter << YAML::Value << "60"; - emitter << YAML::Key << "num_iterations"; - emitter << YAML::Value << "40"; - emitter << YAML::Key << "num_iterations_after_valid"; - emitter << YAML::Value << "0"; - emitter << YAML::Key << "num_rollouts"; - emitter << YAML::Value << "30"; - emitter << YAML::Key << "max_rollouts"; - emitter << YAML::Value << "30"; - emitter << YAML::Key << "initialization_method"; - emitter << YAML::Value << "1"; - emitter << YAML::Comment("[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]"); - emitter << YAML::Key << "control_cost_weight"; - emitter << YAML::Value << "0.0"; - emitter << YAML::EndMap; - - emitter << YAML::Key << "task"; - emitter << YAML::BeginMap; - - emitter << YAML::Key << "noise_generator"; - emitter << YAML::BeginSeq; - emitter << YAML::BeginMap; - emitter << YAML::Key << "class"; - emitter << YAML::Value << "stomp_moveit/NormalDistributionSampling"; - emitter << YAML::Key << "stddev"; - emitter << YAML::Flow; - const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); - std::vector stddev(joint_models.size(), 0.05); - emitter << YAML::Value << stddev; - emitter << YAML::EndMap; - emitter << YAML::EndSeq; - - emitter << YAML::Key << "cost_functions"; - emitter << YAML::BeginSeq; - emitter << YAML::BeginMap; - emitter << YAML::Key << "class"; - emitter << YAML::Value << "stomp_moveit/CollisionCheck"; - emitter << YAML::Key << "collision_penalty"; - emitter << YAML::Value << "1.0"; - emitter << YAML::Key << "cost_weight"; - emitter << YAML::Value << "1.0"; - emitter << YAML::Key << "kernel_window_percentage"; - emitter << YAML::Value << "0.2"; - emitter << YAML::Key << "longest_valid_joint_move"; - emitter << YAML::Value << "0.05"; - emitter << YAML::EndMap; - emitter << YAML::EndSeq; - - emitter << YAML::Key << "noisy_filters"; - emitter << YAML::BeginSeq; - emitter << YAML::BeginMap; - emitter << YAML::Key << "class"; - emitter << YAML::Value << "stomp_moveit/JointLimits"; - emitter << YAML::Key << "lock_start"; - emitter << YAML::Value << "True"; - emitter << YAML::Key << "lock_goal"; - emitter << YAML::Value << "True"; - emitter << YAML::EndMap; - emitter << YAML::BeginMap; - emitter << YAML::Key << "class"; - emitter << YAML::Value << "stomp_moveit/MultiTrajectoryVisualization"; - emitter << YAML::Key << "line_width"; - emitter << YAML::Value << "0.02"; - emitter << YAML::Key << "rgb"; - emitter << YAML::Flow; - std::vector noisy_filters_rgb{ 255, 255, 0 }; - emitter << YAML::Value << noisy_filters_rgb; - emitter << YAML::Key << "marker_array_topic"; - emitter << YAML::Value << "stomp_trajectories"; - emitter << YAML::Key << "marker_namespace"; - emitter << YAML::Value << "noisy"; - emitter << YAML::EndMap; - emitter << YAML::EndSeq; - - emitter << YAML::Key << "update_filters"; - emitter << YAML::BeginSeq; - emitter << YAML::BeginMap; - emitter << YAML::Key << "class"; - emitter << YAML::Value << "stomp_moveit/PolynomialSmoother"; - emitter << YAML::Key << "poly_order"; - emitter << YAML::Value << "6"; - emitter << YAML::EndMap; - emitter << YAML::BeginMap; - emitter << YAML::Key << "class"; - emitter << YAML::Value << "stomp_moveit/TrajectoryVisualization"; - emitter << YAML::Key << "line_width"; - emitter << YAML::Value << "0.05"; - emitter << YAML::Key << "rgb"; - emitter << YAML::Flow; - std::vector update_filters_rgb{ 0, 191, 255 }; - emitter << YAML::Value << update_filters_rgb; - emitter << YAML::Key << "error_rgb"; - emitter << YAML::Flow; - std::vector update_filters_error_rgb{ 255, 0, 0 }; - emitter << YAML::Value << update_filters_error_rgb; - emitter << YAML::Key << "publish_intermediate"; - emitter << YAML::Value << "True"; - emitter << YAML::Key << "marker_topic"; - emitter << YAML::Value << "stomp_trajectory"; - emitter << YAML::Key << "marker_namespace"; - emitter << YAML::Value << "optimized"; - emitter << YAML::EndMap; - emitter << YAML::EndSeq; - - emitter << YAML::EndMap; - 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 -} - -// ****************************************************************************************** -// 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 -// ****************************************************************************************** -std::string MoveItConfigData::getJointHardwareInterface(const std::string& joint_name) -{ - for (ControllerConfig& ros_control_config : controller_configs_) - { - std::vector::iterator joint_it = - std::find(ros_control_config.joints_.begin(), ros_control_config.joints_.end(), joint_name); - if (joint_it != ros_control_config.joints_.end()) - { - if (ros_control_config.type_.substr(0, 8) == "position") - return "hardware_interface/PositionJointInterface"; - else if (ros_control_config.type_.substr(0, 8) == "velocity") - return "hardware_interface/VelocityJointInterface"; - // As of writing this, available joint command interfaces are position, velocity and effort. - else - return "hardware_interface/EffortJointInterface"; - } - } - // If the joint was not found in any controller return EffortJointInterface - return "hardware_interface/EffortJointInterface"; -} - -bool MoveItConfigData::outputFakeControllersYAML(const std::string& file_path) -{ - YAML::Emitter emitter; - emitter << YAML::BeginMap; - - emitter << YAML::Key << "controller_list"; - emitter << YAML::Value << YAML::BeginSeq; - - // Loop through groups - for (srdf::Model::Group& group : srdf_->groups_) - { - // Get list of associated joints - const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - emitter << YAML::BeginMap; - const std::vector& joint_models = joint_model_group->getActiveJointModels(); - emitter << YAML::Key << "name"; - emitter << YAML::Value << "fake_" + group.name_ + "_controller"; - emitter << YAML::Key << "type"; - emitter << YAML::Value << "$(arg fake_execution_type)"; - emitter << YAML::Key << "joints"; - emitter << YAML::Value << YAML::BeginSeq; - - // Iterate through the joints - for (const moveit::core::JointModel* joint : joint_models) - { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) - continue; - emitter << joint->getName(); - } - emitter << YAML::EndSeq; - emitter << YAML::EndMap; - } - - emitter << YAML::EndSeq; - - // Add an initial pose for each group - emitter << YAML::Key << "initial" << YAML::Comment("Define initial robot poses per group"); - - bool poses_found = false; - std::string default_group_name; - for (const srdf::Model::Group& group : srdf_->groups_) - { - if (default_group_name.empty()) - default_group_name = group.name_; - for (const srdf::Model::GroupState& group_state : srdf_->group_states_) - { - if (group.name_ == group_state.group_) - { - if (!poses_found) - { - poses_found = true; - emitter << YAML::Value << YAML::BeginSeq; - } - emitter << YAML::BeginMap; - emitter << YAML::Key << "group"; - emitter << YAML::Value << group.name_; - emitter << YAML::Key << "pose"; - emitter << YAML::Value << group_state.name_; - emitter << YAML::EndMap; - break; - } - } - } - if (poses_found) - emitter << YAML::EndSeq; - else - { - // Add commented lines to show how the feature can be used - if (default_group_name.empty()) - default_group_name = "group"; - emitter << YAML::Newline; - emitter << YAML::Comment(" - group: " + default_group_name) << YAML::Newline; - emitter << YAML::Comment(" pose: home") << YAML::Newline; - - // Add empty list for valid yaml - emitter << YAML::BeginSeq; - emitter << YAML::EndSeq; - } - - 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 -} - -std::map MoveItConfigData::getInitialJoints() const -{ - std::map joints; - for (const srdf::Model::Group& group : srdf_->groups_) - { - // use first pose of each group as initial pose - for (const srdf::Model::GroupState& group_state : srdf_->group_states_) - { - if (group.name_ != group_state.group_) - continue; - for (const auto& pair : group_state.joint_values_) - { - if (pair.second.size() != 1) - continue; // only handle simple joints here - joints[pair.first] = pair.second.front(); - } - break; - } - } - return joints; -} - -std::vector MoveItConfigData::getOMPLPlanners() const -{ - std::vector planner_des; - - OMPLPlannerDescription aps("AnytimePathShortening", "geometric"); - aps.addParameter("shortcut", "true", "Attempt to shortcut all new solution paths"); - aps.addParameter("hybridize", "true", "Compute hybrid solution trajectories"); - aps.addParameter("max_hybrid_paths", "24", "Number of hybrid paths generated per iteration"); - aps.addParameter("num_planners", "4", "The number of default planners to use for planning"); -// TODO: remove when ROS Melodic and older are no longer supported -#if OMPL_VERSION_VALUE >= 1005000 - // This parameter was added in OMPL 1.5.0 - aps.addParameter("planners", "", - "A comma-separated list of planner types (e.g., \"PRM,EST,RRTConnect\"" - "Optionally, planner parameters can be passed to change the default:" - "\"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]\""); -#endif - planner_des.push_back(aps); - - OMPLPlannerDescription sbl("SBL", "geometric"); - sbl.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); - planner_des.push_back(sbl); - - OMPLPlannerDescription est("EST", "geometric"); - est.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()"); - est.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - planner_des.push_back(est); - - OMPLPlannerDescription lbkpiece("LBKPIECE", "geometric"); - lbkpiece.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - lbkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); - lbkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); - planner_des.push_back(lbkpiece); - - OMPLPlannerDescription bkpiece("BKPIECE", "geometric"); - bkpiece.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - bkpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9"); - bkpiece.addParameter("failed_expansion_score_factor", "0.5", - "When extending motion fails, scale score by factor. " - "default: 0.5"); - bkpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); - planner_des.push_back(bkpiece); - - OMPLPlannerDescription kpiece("KPIECE", "geometric"); - kpiece.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - kpiece.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - kpiece.addParameter("border_fraction", "0.9", "Fraction of time focused on boarder default: 0.9 (0.0,1.]"); - kpiece.addParameter("failed_expansion_score_factor", "0.5", - "When extending motion fails, scale score by factor. " - "default: 0.5"); - kpiece.addParameter("min_valid_path_fraction", "0.5", "Accept partially valid moves above fraction. default: 0.5"); - planner_des.push_back(kpiece); - - OMPLPlannerDescription rrt("RRT", "geometric"); - rrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); - rrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - planner_des.push_back(rrt); - - OMPLPlannerDescription rrt_connect("RRTConnect", "geometric"); - rrt_connect.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - planner_des.push_back(rrt_connect); - - OMPLPlannerDescription rr_tstar("RRTstar", "geometric"); - rr_tstar.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - rr_tstar.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - rr_tstar.addParameter("delay_collision_checking", "1", - "Stop collision checking as soon as C-free parent found. " - "default 1"); - planner_des.push_back(rr_tstar); - - OMPLPlannerDescription trrt("TRRT", "geometric"); - trrt.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()"); - trrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - trrt.addParameter("max_states_failed", "10", "when to start increasing temp. default: 10"); - trrt.addParameter("temp_change_factor", "2.0", "how much to increase or decrease temp. default: 2.0"); - trrt.addParameter("min_temperature", "10e-10", "lower limit of temp change. default: 10e-10"); - trrt.addParameter("init_temperature", "10e-6", "initial temperature. default: 10e-6"); - trrt.addParameter("frountier_threshold", "0.0", - "dist new state to nearest neighbor to disqualify as frontier. " - "default: 0.0 set in setup()"); - trrt.addParameter("frountierNodeRatio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); - trrt.addParameter("k_constant", "0.0", "value used to normalize expression. default: 0.0 set in setup()"); - planner_des.push_back(trrt); - - OMPLPlannerDescription prm("PRM", "geometric"); - prm.addParameter("max_nearest_neighbors", "10", "use k nearest neighbors. default: 10"); - planner_des.push_back(prm); - - OMPLPlannerDescription pr_mstar("PRMstar", "geometric"); // no declares in code - planner_des.push_back(pr_mstar); - - OMPLPlannerDescription fmt("FMT", "geometric"); - fmt.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000"); - fmt.addParameter("radius_multiplier", "1.1", "multiplier used for the nearest neighbors search radius. default: 1.1"); - fmt.addParameter("nearest_k", "1", "use Knearest strategy. default: 1"); - fmt.addParameter("cache_cc", "1", "use collision checking cache. default: 1"); - fmt.addParameter("heuristics", "0", "activate cost to go heuristics. default: 0"); - fmt.addParameter("extended_fmt", "1", - "activate the extended FMT*: adding new samples if planner does not finish " - "successfully. default: 1"); - planner_des.push_back(fmt); - - OMPLPlannerDescription bfmt("BFMT", "geometric"); - bfmt.addParameter("num_samples", "1000", "number of states that the planner should sample. default: 1000"); - bfmt.addParameter("radius_multiplier", "1.0", - "multiplier used for the nearest neighbors search radius. default: " - "1.0"); - bfmt.addParameter("nearest_k", "1", "use the Knearest strategy. default: 1"); - bfmt.addParameter("balanced", "0", - "exploration strategy: balanced true expands one tree every iteration. False will " - "select the tree with lowest maximum cost to go. default: 1"); - bfmt.addParameter("optimality", "1", - "termination strategy: optimality true finishes when the best possible path is " - "found. Otherwise, the algorithm will finish when the first feasible path is " - "found. default: 1"); - bfmt.addParameter("heuristics", "1", "activates cost to go heuristics. default: 1"); - bfmt.addParameter("cache_cc", "1", "use the collision checking cache. default: 1"); - bfmt.addParameter("extended_fmt", "1", - "Activates the extended FMT*: adding new samples if planner does not finish " - "successfully. default: 1"); - planner_des.push_back(bfmt); - - OMPLPlannerDescription pdst("PDST", "geometric"); - rrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability? default: 0.05"); - planner_des.push_back(pdst); - - OMPLPlannerDescription stride("STRIDE", "geometric"); - stride.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - stride.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - stride.addParameter("use_projected_distance", "0", - "whether nearest neighbors are computed based on distances in a " - "projection of the state rather distances in the state space " - "itself. default: 0"); - stride.addParameter("degree", "16", - "desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). " - "default: 16"); - stride.addParameter("max_degree", "18", "max degree of a node in the GNAT. default: 12"); - stride.addParameter("min_degree", "12", "min degree of a node in the GNAT. default: 12"); - stride.addParameter("max_pts_per_leaf", "6", "max points per leaf in the GNAT. default: 6"); - stride.addParameter("estimated_dimension", "0.0", "estimated dimension of the free space. default: 0.0"); - stride.addParameter("min_valid_path_fraction", "0.2", "Accept partially valid moves above fraction. default: 0.2"); - planner_des.push_back(stride); - - OMPLPlannerDescription bi_trrt("BiTRRT", "geometric"); - bi_trrt.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - bi_trrt.addParameter("temp_change_factor", "0.1", "how much to increase or decrease temp. default: 0.1"); - bi_trrt.addParameter("init_temperature", "100", "initial temperature. default: 100"); - bi_trrt.addParameter("frountier_threshold", "0.0", - "dist new state to nearest neighbor to disqualify as frontier. " - "default: 0.0 set in setup()"); - bi_trrt.addParameter("frountier_node_ratio", "0.1", "1/10, or 1 nonfrontier for every 10 frontier. default: 0.1"); - bi_trrt.addParameter("cost_threshold", "1e300", - "the cost threshold. Any motion cost that is not better will not be " - "expanded. default: inf"); - planner_des.push_back(bi_trrt); - - OMPLPlannerDescription lbtrrt("LBTRRT", "geometric"); - lbtrrt.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - lbtrrt.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - lbtrrt.addParameter("epsilon", "0.4", "optimality approximation factor. default: 0.4"); - planner_des.push_back(lbtrrt); - - OMPLPlannerDescription bi_est("BiEST", "geometric"); - bi_est.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - planner_des.push_back(bi_est); - - OMPLPlannerDescription proj_est("ProjEST", "geometric"); - proj_est.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - proj_est.addParameter("goal_bias", "0.05", "When close to goal select goal, with this probability. default: 0.05"); - planner_des.push_back(proj_est); - - OMPLPlannerDescription lazy_prm("LazyPRM", "geometric"); - lazy_prm.addParameter("range", "0.0", - "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on " - "setup()"); - planner_des.push_back(lazy_prm); - - OMPLPlannerDescription lazy_pr_mstar("LazyPRMstar", "geometric"); // no declares in code - planner_des.push_back(lazy_pr_mstar); - - OMPLPlannerDescription spars("SPARS", "geometric"); - spars.addParameter("stretch_factor", "3.0", - "roadmap spanner stretch factor. multiplicative upper bound on path " - "quality. It does not make sense to make this parameter more than 3. " - "default: 3.0"); - spars.addParameter("sparse_delta_fraction", "0.25", - "delta fraction for connection distance. This value represents " - "the visibility range of sparse samples. default: 0.25"); - spars.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001"); - spars.addParameter("max_failures", "1000", "maximum consecutive failure limit. default: 1000"); - planner_des.push_back(spars); - - OMPLPlannerDescription spar_stwo("SPARStwo", "geometric"); - spar_stwo.addParameter("stretch_factor", "3.0", - "roadmap spanner stretch factor. multiplicative upper bound on path " - "quality. It does not make sense to make this parameter more than 3. " - "default: 3.0"); - spar_stwo.addParameter("sparse_delta_fraction", "0.25", - "delta fraction for connection distance. This value represents " - "the visibility range of sparse samples. default: 0.25"); - spar_stwo.addParameter("dense_delta_fraction", "0.001", "delta fraction for interface detection. default: 0.001"); - spar_stwo.addParameter("max_failures", "5000", "maximum consecutive failure limit. default: 5000"); - planner_des.push_back(spar_stwo); - - return planner_des; -} - -// ****************************************************************************************** -// Generate simple_moveit_controllers.yaml config file -// ****************************************************************************************** -bool MoveItConfigData::outputSimpleControllersYAML(const std::string& file_path) -{ - YAML::Emitter emitter; - emitter << YAML::BeginMap; - emitter << YAML::Key << "controller_list"; - emitter << YAML::Value << YAML::BeginSeq; - for (const auto& controller : controller_configs_) - { - // Only process FollowJointTrajectory types - std::string type = controller.type_; - if (boost::ends_with(type, "/JointTrajectoryController")) - type = "FollowJointTrajectory"; - if (type == "FollowJointTrajectory" || type == "GripperCommand") - { - emitter << YAML::BeginMap; - emitter << YAML::Key << "name"; - emitter << YAML::Value << controller.name_; - emitter << YAML::Key << "action_ns"; - emitter << YAML::Value << (type == "FollowJointTrajectory" ? "follow_joint_trajectory" : "gripper_action"); - emitter << YAML::Key << "type"; - emitter << YAML::Value << type; - emitter << YAML::Key << "default"; - emitter << YAML::Value << "True"; - - // Write joints - emitter << YAML::Key << "joints"; - emitter << YAML::Value << YAML::BeginSeq; - // Iterate through the joints - for (const std::string& joint : controller.joints_) - emitter << joint; - emitter << YAML::EndSeq; - - emitter << YAML::EndMap; - } - } - emitter << YAML::EndSeq; - 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 default start pose for moveit_sim_hw_interface -// ****************************************************************************************** -srdf::Model::GroupState MoveItConfigData::getDefaultStartPose() -{ - if (!srdf_->group_states_.empty()) - return srdf_->group_states_[0]; - else - return srdf::Model::GroupState{ .name_ = "todo_state_name", .group_ = "todo_group_name", .joint_values_ = {} }; -} - -// ****************************************************************************************** -// Generate ros_controllers.yaml config file -// ****************************************************************************************** -bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) -{ - // Cache the joints' names. - std::vector> planning_groups; - - // We are going to write the joints names many times. - // Loop through groups to store the joints names in group_joints vector and reuse is. - for (srdf::Model::Group& group : srdf_->groups_) - { - std::vector group_joints; - // Get list of associated joints - const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); - // Iterate through the joints and push into group_joints vector. - for (const moveit::core::JointModel* joint : joint_models) - { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) - continue; - else - group_joints.push_back(joint->getName()); - } - // Push all the group joints into planning_groups vector. - planning_groups.push_back(group_joints); - } - - YAML::Emitter emitter; - emitter << YAML::BeginMap; - - { -#if 0 // TODO: This is only for fake ROS controllers, which should go into a separate file - // Also replace moveit_sim_controllers with http://wiki.ros.org/fake_joint - emitter << YAML::Comment("Simulation settings for using moveit_sim_controllers"); - emitter << YAML::Key << "moveit_sim_hw_interface" << YAML::Value << YAML::BeginMap; - // MoveIt Simulation Controller settings for setting initial pose - { - // Use the first planning group if initial joint_model_group was not set, else write a default value - emitter << YAML::Key << "joint_model_group"; - emitter << YAML::Value << getDefaultStartPose().group_; - - // Use the first robot pose if initial joint_model_group_pose was not set, else write a default value - emitter << YAML::Key << "joint_model_group_pose"; - emitter << YAML::Value << getDefaultStartPose().name_; - - emitter << YAML::EndMap; - } - // Settings for ros_control control loop - emitter << YAML::Newline; - emitter << YAML::Comment("Settings for ros_control_boilerplate control loop"); - emitter << YAML::Key << "generic_hw_control_loop" << YAML::Value << YAML::BeginMap; - { - emitter << YAML::Key << "loop_hz"; - emitter << YAML::Value << "300"; - emitter << YAML::Key << "cycle_time_error_threshold"; - emitter << YAML::Value << "0.01"; - emitter << YAML::EndMap; - } - // Settings for ros_control hardware interface - emitter << YAML::Newline; - emitter << YAML::Comment("Settings for ros_control hardware interface"); - emitter << YAML::Key << "hardware_interface" << YAML::Value << YAML::BeginMap; - { - // Get list of all joints for the robot - const std::vector& joint_models = getRobotModel()->getJointModels(); - - emitter << YAML::Key << "joints"; - { - if (joint_models.size() != 1) - { - emitter << YAML::Value << YAML::BeginSeq; - // Iterate through the joints - for (std::vector::const_iterator joint_it = joint_models.begin(); - joint_it < joint_models.end(); ++joint_it) - { - if ((*joint_it)->isPassive() || (*joint_it)->getMimic() != nullptr || - (*joint_it)->getType() == moveit::core::JointModel::FIXED) - continue; - else - emitter << (*joint_it)->getName(); - } - emitter << YAML::EndSeq; - } - else - { - emitter << YAML::Value << YAML::BeginMap; - emitter << joint_models[0]->getName(); - emitter << YAML::EndMap; - } - } - emitter << YAML::Key << "sim_control_mode"; - emitter << YAML::Value << "1"; - emitter << YAML::Comment("0: position, 1: velocity"); - emitter << YAML::Newline; - emitter << YAML::EndMap; - } -#endif - for (const auto& controller : controller_configs_) - { - if (controller.type_ == "FollowJointTrajectory" || controller.type_ == "GripperCommand") - continue; // these are handled by outputSimpleControllersYAML() - - emitter << YAML::Key << controller.name_; - emitter << YAML::Value << YAML::BeginMap; - emitter << YAML::Key << "type"; - emitter << YAML::Value << controller.type_; - - // Write joints - emitter << YAML::Key << "joints"; - emitter << YAML::Value << YAML::BeginSeq; - // Iterate through the joints - for (const std::string& joint : controller.joints_) - emitter << joint; - emitter << YAML::EndSeq; - - // Write gains as they are required for vel and effort controllers - emitter << YAML::Key << "gains"; - emitter << YAML::Value << YAML::BeginMap; - { - // Iterate through the joints - for (const std::string& joint : controller.joints_) - { - emitter << YAML::Key << joint << YAML::Value << YAML::BeginMap; - emitter << YAML::Key << "p"; - emitter << YAML::Value << "100"; - emitter << YAML::Key << "d"; - emitter << YAML::Value << "1"; - emitter << YAML::Key << "i"; - emitter << YAML::Value << "1"; - emitter << YAML::Key << "i_clamp"; - emitter << YAML::Value << "1" << YAML::EndMap; - } - 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 -} - -// ****************************************************************************************** -// Output 3D Sensor configuration file -// ****************************************************************************************** -bool MoveItConfigData::output3DSensorPluginYAML(const std::string& file_path) -{ - YAML::Emitter emitter; - - emitter << YAML::BeginMap; - emitter << YAML::Key << "sensors"; - emitter << YAML::Value << YAML::BeginSeq; - - for (auto& sensors_plugin_config : sensors_plugin_config_parameter_list_) - { - emitter << YAML::BeginMap; - - for (auto& parameter : sensors_plugin_config) - { - emitter << YAML::Key << parameter.first; - emitter << YAML::Value << parameter.second.getValue(); - } - emitter << YAML::EndMap; - } - - emitter << YAML::EndSeq; - - 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 -} - -// ****************************************************************************************** -// Output joint limits config files -// ****************************************************************************************** -bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) -{ - YAML::Emitter emitter; - emitter << YAML::Comment("joint_limits.yaml allows the dynamics properties specified in the URDF " - "to be overwritten or augmented as needed"); - emitter << YAML::Newline; - - emitter << YAML::BeginMap; - - emitter << YAML::Comment("For beginners, we downscale velocity and acceleration limits.") << YAML::Newline; - emitter << YAML::Comment("You can always specify higher scaling factors (<= 1.0) in your motion requests."); - emitter << YAML::Comment("Increase the values below to 1.0 to always move at maximum speed."); - emitter << YAML::Key << "default_velocity_scaling_factor"; - emitter << YAML::Value << "0.1"; - - emitter << YAML::Key << "default_acceleration_scaling_factor"; - emitter << YAML::Value << "0.1"; - - emitter << YAML::Newline << YAML::Newline; - emitter << YAML::Comment("Specific joint properties can be changed with the keys " - "[max_position, min_position, max_velocity, max_acceleration]") - << YAML::Newline; - emitter << YAML::Comment("Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]"); - - emitter << YAML::Key << "joint_limits"; - emitter << YAML::Value << YAML::BeginMap; - - // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name - std::set joints; - - // Loop through groups - for (srdf::Model::Group& group : srdf_->groups_) - { - // Get list of associated joints - const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - - const std::vector& joint_models = joint_model_group->getJointModels(); - - // Iterate through the joints - for (const moveit::core::JointModel* joint_model : joint_models) - { - // Check that this joint only represents 1 variable. - if (joint_model->getVariableCount() == 1) - joints.insert(joint_model); - } - } - - // Add joints to yaml file, if no more than 1 dof - for (const moveit::core::JointModel* joint : joints) - { - emitter << YAML::Key << joint->getName(); - emitter << YAML::Value << YAML::BeginMap; - - const moveit::core::VariableBounds& b = joint->getVariableBounds()[0]; - - // Output property - emitter << YAML::Key << "has_velocity_limits"; - if (b.velocity_bounded_) - emitter << YAML::Value << "true"; - else - emitter << YAML::Value << "false"; - - // Output property - emitter << YAML::Key << "max_velocity"; - emitter << YAML::Value << std::min(fabs(b.max_velocity_), fabs(b.min_velocity_)); - - // Output property - emitter << YAML::Key << "has_acceleration_limits"; - if (b.acceleration_bounded_) - emitter << YAML::Value << "true"; - else - emitter << YAML::Value << "false"; - - // Output property - emitter << YAML::Key << "max_acceleration"; - emitter << YAML::Value << std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_)); - - 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 -} - -// ****************************************************************************************** -// Decide the best two joints to be used for the projection evaluator -// ****************************************************************************************** -std::string MoveItConfigData::decideProjectionJoints(const std::string& planning_group) -{ - std::string joint_pair = ""; - - // Retrieve pointer to the shared kinematic model - const moveit::core::RobotModelConstPtr& model = getRobotModel(); - - // Error check - if (!model->hasJointModelGroup(planning_group)) - return joint_pair; - - // Get the joint model group - const moveit::core::JointModelGroup* group = model->getJointModelGroup(planning_group); - - // get vector of joint names - const std::vector& joints = group->getJointModelNames(); - - if (joints.size() >= 2) - { - // Check that the first two joints have only 1 variable - if (group->getJointModel(joints[0])->getVariableCount() == 1 && - group->getJointModel(joints[1])->getVariableCount() == 1) - { - // Just choose the first two joints. - joint_pair = "joints(" + joints[0] + "," + joints[1] + ")"; - } - } - - return joint_pair; -} - -template -bool parse(const YAML::Node& node, const std::string& key, T& storage, const T& default_value = T()) -{ - const YAML::Node& n = node[key]; - bool valid = n.IsDefined(); - storage = valid ? n.as() : default_value; - return valid; -} - -bool MoveItConfigData::inputOMPLYAML(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) - { - // get group name - const std::string group_name = group_it->first.as(); - - // compare group name found to list of groups in group_meta_data_ - std::map::iterator group_meta_it; - group_meta_it = group_meta_data_.find(group_name); - if (group_meta_it != group_meta_data_.end()) - { - std::string planner; - parse(group_it->second, "default_planner_config", planner); - std::size_t pos = planner.find("kConfigDefault"); - if (pos != std::string::npos) - { - planner = planner.substr(0, pos); - } - group_meta_data_[group_name].default_planner_ = planner; - } - } - } - catch (YAML::ParserException& e) // Catch errors - { - RCLCPP_ERROR_STREAM(LOGGER, e.what()); - return false; - } - 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 -// ****************************************************************************************** -bool MoveItConfigData::inputPlanningContextLaunch(const std::string& file_path) -{ - TiXmlDocument launch_document(file_path); - if (!launch_document.LoadFile()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Failed parsing " << file_path); - return false; - } - - // find the kinematics section - TiXmlHandle doc(&launch_document); - TiXmlElement* kinematics_group = doc.FirstChild("launch").FirstChild("group").ToElement(); - while (kinematics_group && kinematics_group->Attribute("ns") && - kinematics_group->Attribute("ns") != std::string("$(arg robot_description)_kinematics")) - { - kinematics_group = kinematics_group->NextSiblingElement("group"); - } - if (!kinematics_group) - { - RCLCPP_ERROR(LOGGER, " not found"); - return false; - } - - // iterate over all elements - // and if 'group' matches an existing group, copy the filename - for (TiXmlElement* kinematics_parameter_file = kinematics_group->FirstChildElement("rosparam"); - kinematics_parameter_file; kinematics_parameter_file = kinematics_parameter_file->NextSiblingElement("rosparam")) - { - const char* ns = kinematics_parameter_file->Attribute("ns"); - if (ns && (group_meta_data_.find(ns) != group_meta_data_.end())) - { - group_meta_data_[ns].kinematics_parameters_file_ = kinematics_parameter_file->Attribute("file"); - } - } - - return true; -} - -// ****************************************************************************************** -// Helper function for parsing an individual ROSController from ros_controllers yaml file -// ****************************************************************************************** -bool MoveItConfigData::parseROSController(const YAML::Node& controller) -{ - // Used in parsing ROS controllers - ControllerConfig control_setting; - - if (const YAML::Node& trajectory_controllers = controller) - { - for (const YAML::Node& trajectory_controller : trajectory_controllers) - { - // Controller node - if (const YAML::Node& controller_node = trajectory_controller) - { - if (const YAML::Node& joints = controller_node["joints"]) - { - control_setting.joints_.clear(); - for (YAML::const_iterator joint_it = joints.begin(); joint_it != joints.end(); ++joint_it) - { - control_setting.joints_.push_back(joint_it->as()); - } - if (!parse(controller_node, "name", control_setting.name_)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Couldn't parse ros_controllers.yaml"); - return false; - } - if (!parse(controller_node, "type", control_setting.type_)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Couldn't parse ros_controllers.yaml"); - return false; - } - // All required fields were parsed correctly - controller_configs_.push_back(control_setting); - } - else - { - RCLCPP_ERROR_STREAM(LOGGER, "Couldn't parse ros_controllers.yaml"); - return false; - } - } - } - } - return true; -} - -// ****************************************************************************************** -// Helper function for parsing ROSControllers from ros_controllers yaml file -// ****************************************************************************************** -bool MoveItConfigData::processROSControllers(std::ifstream& input_stream) -{ - // Used in parsing ROS controllers - ControllerConfig control_setting; - YAML::Node controllers = YAML::Load(input_stream); - - // Loop through all controllers - for (YAML::const_iterator controller_it = controllers.begin(); controller_it != controllers.end(); ++controller_it) - { - // Follow Joint Trajectory action controllers - if (controller_it->first.as() == "controller_list") - { - if (!parseROSController(controller_it->second)) - return false; - } - // Other settings found in the ros_controllers file - else - { - const std::string& controller_name = controller_it->first.as(); - control_setting.joints_.clear(); - - // Push joints if found in the controller - if (const YAML::Node& joints = controller_it->second["joints"]) - { - if (joints.IsSequence()) - { - for (YAML::const_iterator joint_it = joints.begin(); joint_it != joints.end(); ++joint_it) - { - control_setting.joints_.push_back(joint_it->as()); - } - } - else - { - control_setting.joints_.push_back(joints.as()); - } - } - - // If the setting has joints then it is a controller that needs to be parsed - if (!control_setting.joints_.empty()) - { - if (const YAML::Node& urdf_node = controller_it->second["type"]) - { - control_setting.type_ = controller_it->second["type"].as(); - control_setting.name_ = controller_name; - controller_configs_.push_back(control_setting); - control_setting.joints_.clear(); - } - } - } - } - return true; -} - -// ****************************************************************************************** -// Input ros_controllers.yaml file -// ****************************************************************************************** -bool MoveItConfigData::inputROSControllersYAML(const std::string& file_path) -{ - // Load file - std::ifstream input_stream(file_path.c_str()); - if (!input_stream.good()) - { - RCLCPP_WARN_STREAM(LOGGER, "Does not exist " << file_path); - return false; - } - - // Begin parsing - try - { - processROSControllers(input_stream); - } - catch (YAML::ParserException& e) // Catch errors - { - RCLCPP_ERROR_STREAM(LOGGER, e.what()); - return false; - } - - return true; // file read successfully -} - -// ****************************************************************************************** -// Add a Follow Joint Trajectory action Controller for each Planning Group -// ****************************************************************************************** -bool MoveItConfigData::addDefaultControllers(const std::string& controller_type) -{ - if (srdf_->srdf_model_->getGroups().empty()) - return false; - // Loop through groups - for (const srdf::Model::Group& group_it : srdf_->srdf_model_->getGroups()) - { - ControllerConfig group_controller; - // Get list of associated joints - const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group_it.name_); - const std::vector& joint_models = joint_model_group->getActiveJointModels(); - - // Iterate through the joints - for (const moveit::core::JointModel* joint : joint_models) - { - if (joint->isPassive() || joint->getMimic() != nullptr || joint->getType() == moveit::core::JointModel::FIXED) - continue; - group_controller.joints_.push_back(joint->getName()); - } - if (!group_controller.joints_.empty()) - { - group_controller.name_ = group_it.name_ + "_controller"; - group_controller.type_ = controller_type; - addController(group_controller); - } - } - return true; -} - -// ****************************************************************************************** -// Set package path; try to resolve path from package name if directory does not exist -// ****************************************************************************************** -bool MoveItConfigData::setPackagePath(const std::string& pkg_path) -{ - std::string full_package_path; - - // check that the folder exists - if (!fs::is_directory(pkg_path)) - { - // does not exist, check if its a package - full_package_path = ament_index_cpp::get_package_share_directory(pkg_path); - - // check that the folder exists - if (!fs::is_directory(full_package_path)) - { - return false; - } - } - else - { - // they inputted a full path - full_package_path = pkg_path; - } - - config_pkg_path_ = full_package_path; - return true; -} - -// ****************************************************************************************** -// Extract the package/stack name from an absolute file path -// Input: path -// Output: package name and relative path -// ****************************************************************************************** -bool MoveItConfigData::extractPackageNameFromPath(const std::string& path, std::string& package_name, - std::string& relative_filepath) const -{ - fs::path sub_path = path; // holds the directory less one folder - fs::path relative_path; // holds the path after the sub_path - - bool package_found = false; - - // truncate path step by step and check if it contains a package.xml - while (!sub_path.empty()) - { - RCLCPP_DEBUG_STREAM(LOGGER, "checking in " << sub_path.make_preferred().string()); - if (fs::is_regular_file(sub_path / "package.xml")) - { - RCLCPP_DEBUG_STREAM(LOGGER, "Found package.xml in " << sub_path.make_preferred().string()); - package_found = true; - relative_filepath = relative_path.string(); - package_name = sub_path.filename(); - break; - } - relative_path = sub_path.filename() / relative_path; - sub_path.remove_filename(); - } - - // Assign data to moveit_config_data - if (!package_found) - { - // No package name found, we must be outside ROS - return false; - } - - RCLCPP_DEBUG_STREAM(LOGGER, "Package name for file \"" << path << "\" is \"" << package_name << "\""); - return true; -} - -// ****************************************************************************************** -// Resolve path to .setup_assistant file -// ****************************************************************************************** - -bool MoveItConfigData::getSetupAssistantYAMLPath(std::string& path) -{ - path = appendPaths(config_pkg_path_, ".setup_assistant"); - - // Check if the old package is a setup assistant package - return fs::is_regular_file(path); -} - -// ****************************************************************************************** -// Make the full URDF path using the loaded .setup_assistant data -// ****************************************************************************************** -bool MoveItConfigData::createFullURDFPath() -{ - boost::trim(urdf_pkg_name_); - - // Check if a package name was provided - if (urdf_pkg_name_.empty() || urdf_pkg_name_ == "\"\"") - { - urdf_path_ = urdf_pkg_relative_path_; - urdf_pkg_name_.clear(); - } - else - { - // Check that ROS can find the package - std::string robot_desc_pkg_path = ament_index_cpp::get_package_share_directory(urdf_pkg_name_); - - if (robot_desc_pkg_path.empty()) - { - urdf_path_.clear(); - return false; - } - - // Append the relative URDF url path - urdf_path_ = appendPaths(robot_desc_pkg_path, urdf_pkg_relative_path_); - } - - // Check that this file exits ------------------------------------------------- - return fs::is_regular_file(urdf_path_); -} - -// ****************************************************************************************** -// Make the full SRDF path using the loaded .setup_assistant data -// ****************************************************************************************** -bool MoveItConfigData::createFullSRDFPath(const std::string& package_path) -{ - srdf_path_ = appendPaths(package_path, srdf_pkg_relative_path_); - - return fs::is_regular_file(srdf_path_); -} - -// ****************************************************************************************** -// Input .setup_assistant file - contains data used for the MoveIt Setup Assistant -// ****************************************************************************************** -bool MoveItConfigData::inputSetupAssistantYAML(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 - { - const YAML::Node& doc = YAML::Load(input_stream); - - // Get title node - if (const YAML::Node& title_node = doc["moveit_setup_assistant_config"]) - { - // URDF Properties - if (const YAML::Node& urdf_node = title_node["URDF"]) - { - if (!parse(urdf_node, "package", urdf_pkg_name_)) - return false; // if we do not find this value we cannot continue - - if (!parse(urdf_node, "relative_path", urdf_pkg_relative_path_)) - return false; // if we do not find this value we cannot continue - - parse(urdf_node, "xacro_args", xacro_args_); - } - // SRDF Properties - if (const YAML::Node& srdf_node = title_node["SRDF"]) - { - if (!parse(srdf_node, "relative_path", srdf_pkg_relative_path_)) - return false; // if we do not find this value we cannot continue - } - // Package generation time - if (const YAML::Node& config_node = title_node["CONFIG"]) - { - parse(config_node, "author_name", author_name_); - parse(config_node, "author_email", author_email_); - parse(config_node, "generated_timestamp", config_pkg_generated_timestamp_); - } - return true; - } - } - catch (YAML::ParserException& e) // Catch errors - { - RCLCPP_ERROR_STREAM(LOGGER, e.what()); - } - - return false; // if it gets to this point an error has occurred -} - -// ****************************************************************************************** -// Input sensors_3d yaml file -// ****************************************************************************************** -void MoveItConfigData::input3DSensorsYAML(const std::string& file_path) -{ - sensors_plugin_config_parameter_list_ = load3DSensorsYAML(file_path); -} - -// ****************************************************************************************** -// Load sensors_3d.yaml file -// ****************************************************************************************** -std::vector> MoveItConfigData::load3DSensorsYAML(const std::string& file_path) -{ - std::vector> config; - - // Is there a sensors config in the package? - if (file_path.empty()) - return config; - - // 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 config; - } - - // Begin parsing - try - { - const YAML::Node& doc = YAML::Load(input_stream); - // Get sensors node - const YAML::Node& sensors_node = doc["sensors"]; - - // Make sure that the sensors are written as a sequence - if (sensors_node && sensors_node.IsSequence()) - { - // Loop over the sensors available in the file - for (const YAML::Node& sensor : sensors_node) - { - std::map sensor_map; - bool empty_node = true; - for (YAML::const_iterator sensor_it = sensor.begin(); sensor_it != sensor.end(); ++sensor_it) - { - empty_node = false; - GenericParameter sensor_param; - sensor_param.setName(sensor_it->first.as()); - sensor_param.setValue(sensor_it->second.as()); - - // Set the key as the parameter name to make accessing it easier - sensor_map[sensor_it->first.as()] = sensor_param; - } - // Don't push empty nodes - if (!empty_node) - config.push_back(sensor_map); - } - } - } - catch (YAML::ParserException& e) // Catch errors - { - RCLCPP_ERROR_STREAM(LOGGER, "Error parsing sensors yaml: " << e.what()); - } - - return config; -} - -// ****************************************************************************************** -// Helper Function for joining a file path and a file name, or two file paths, etc, in a cross-platform way -// ****************************************************************************************** -std::string MoveItConfigData::appendPaths(const std::string& path1, const std::string& path2) -{ - fs::path result = path1; - result /= path2; - return result.make_preferred().string(); -} - -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 -// ****************************************************************************************** -ControllerConfig* MoveItConfigData::findControllerByName(const std::string& controller_name) -{ - // Find the controller we are editing based on its name - for (ControllerConfig& controller : controller_configs_) - { - if (controller.name_ == controller_name) // string match - return &controller; // convert to pointer from iterator - } - - return nullptr; // not found -} - -// ****************************************************************************************** -// Deletes a controller by name -// ****************************************************************************************** -bool MoveItConfigData::deleteController(const std::string& controller_name) -{ - for (std::vector::iterator controller_it = controller_configs_.begin(); - controller_it != controller_configs_.end(); ++controller_it) - { - if (controller_it->name_ == controller_name) // string match - { - controller_configs_.erase(controller_it); - // we are done searching - return true; - } - } - return false; -} - -// ****************************************************************************************** -// Adds a controller to controller_configs_ vector -// ****************************************************************************************** -bool MoveItConfigData::addController(const ControllerConfig& new_controller) -{ - // Find if there is an existing controller with the same name - ControllerConfig* controller = findControllerByName(new_controller.name_); - - if (controller && controller->type_ == new_controller.type_) - return false; - - controller_configs_.push_back(new_controller); - return true; -} - -// ****************************************************************************************** -// Used to add a sensor plugin configuration parameter to the sensor plugin configuration parameter list -// ****************************************************************************************** -void MoveItConfigData::addGenericParameterToSensorPluginConfig(const std::string& name, const std::string& value, - const std::string& /*comment*/) -{ - // Use index 0 since we only write one plugin - GenericParameter new_parameter; - new_parameter.setName(name); - new_parameter.setValue(value); - sensors_plugin_config_parameter_list_[0][name] = new_parameter; -} - -// ****************************************************************************************** -// Used to clear sensor plugin configuration parameter list -// ****************************************************************************************** -void MoveItConfigData::clearSensorPluginConfig() -{ - sensors_plugin_config_parameter_list_.clear(); -} - -} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp deleted file mode 100644 index 23d84033c5..0000000000 --- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp +++ /dev/null @@ -1,1353 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Dave Coleman */ - -// Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "configuration_files_widget.h" -#include "header_widget.h" - -// Boost -#include // for string find and replace in templates -// Read write files -#include // For writing yaml and launch files -#include -#include - -namespace moveit_setup_assistant -{ -// Boost file system -namespace fs = std::filesystem; - -static const std::string SETUP_ASSISTANT_FILE = ".setup_assistant"; -static const std::string CONFIG_PATH = "config"; - -// ****************************************************************************************** -// Outer User Interface for MoveIt Configuration Assistant -// ****************************************************************************************** -ConfigurationFilesWidget::ConfigurationFilesWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data), has_generated_pkg_(false) -{ - // Basic widget container - QVBoxLayout* layout = new QVBoxLayout(); - - // Top Header Area ------------------------------------------------ - - HeaderWidget* header = - new HeaderWidget("Generate Configuration Files", - "Create or update the configuration files package needed to run your robot with MoveIt. Uncheck " - "files to disable them from being generated - this is useful if you have made custom changes to " - "them. Files in orange have been automatically detected as changed.", - this); - layout->addWidget(header); - - // Path Widget ---------------------------------------------------- - - // Stack Path Dialog - stack_path_ = new LoadPathWidget("Configuration Package Save Path", - "Specify the desired directory for the MoveIt configuration package to be " - "generated. Overwriting an existing configuration package directory is acceptable. " - "Example: /u/robot/ros/panda_moveit_config", - this, true); // is directory - layout->addWidget(stack_path_); - - // Pass the package path from start screen to configuration files screen - stack_path_->setPath(config_data_->config_pkg_path_); - - // Generated Files List ------------------------------------------- - QLabel* generated_list = new QLabel("Check files you want to be generated:", this); - layout->addWidget(generated_list); - - QSplitter* splitter = new QSplitter(Qt::Horizontal, this); - splitter->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - - // List Box - action_list_ = new QListWidget(this); - action_list_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - action_list_->setSelectionMode(QAbstractItemView::ExtendedSelection); - connect(action_list_, SIGNAL(currentRowChanged(int)), this, SLOT(changeActionDesc(int))); - // Allow checking / unchecking of multiple items - action_list_->setContextMenuPolicy(Qt::ActionsContextMenu); - QAction* action = new QAction("Check all selected files", this); - connect(action, &QAction::triggered, [this]() { setCheckSelected(true); }); - action_list_->addAction(action); - action = new QAction("Uncheck all selected files", this); - connect(action, &QAction::triggered, [this]() { setCheckSelected(false); }); - action_list_->addAction(action); - - // Description - action_label_ = new QLabel(this); - action_label_->setFrameShape(QFrame::StyledPanel); - action_label_->setFrameShadow(QFrame::Raised); - action_label_->setLineWidth(1); - action_label_->setMidLineWidth(0); - action_label_->setWordWrap(true); - action_label_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - action_label_->setMinimumWidth(100); - action_label_->setAlignment(Qt::AlignTop); - action_label_->setOpenExternalLinks(true); // open with web browser - - // Add to splitter - splitter->addWidget(action_list_); - splitter->addWidget(action_label_); - - // Add Layout - layout->addWidget(splitter); - - // Progress bar and generate buttons --------------------------------------------------- - QHBoxLayout* hlayout1 = new QHBoxLayout(); - - // Progress Bar - progress_bar_ = new QProgressBar(this); - progress_bar_->setMaximum(100); - progress_bar_->setMinimum(0); - hlayout1->addWidget(progress_bar_); - // hlayout1->setContentsMargins( 20, 30, 20, 30 ); - - // Generate Package Button - btn_save_ = new QPushButton("&Generate Package", this); - // btn_save_->setMinimumWidth(180); - btn_save_->setMinimumHeight(40); - connect(btn_save_, SIGNAL(clicked()), this, SLOT(savePackage())); - hlayout1->addWidget(btn_save_); - - // Add Layout - layout->addLayout(hlayout1); - - // Bottom row -------------------------------------------------- - - QHBoxLayout* hlayout3 = new QHBoxLayout(); - - // Success label - success_label_ = new QLabel(this); - QFont success_label_font(QFont().defaultFamily(), 12, QFont::Bold); - success_label_->setFont(success_label_font); - success_label_->hide(); // only show once the files have been generated - success_label_->setText("Configuration package generated successfully!"); - hlayout3->addWidget(success_label_); - hlayout3->setAlignment(success_label_, Qt::AlignRight); - - // Exit button - QPushButton* btn_exit = new QPushButton("E&xit Setup Assistant", this); - btn_exit->setMinimumWidth(180); - connect(btn_exit, SIGNAL(clicked()), this, SLOT(exitSetupAssistant())); - hlayout3->addWidget(btn_exit); - hlayout3->setAlignment(btn_exit, Qt::AlignRight); - - layout->addLayout(hlayout3); - - // Finish Layout -------------------------------------------------- - this->setLayout(layout); -} - -void ConfigurationFilesWidget::setCheckSelected(bool checked) -{ - for (const QModelIndex& row : action_list_->selectionModel()->selectedRows()) - action_list_->model()->setData(row, checked ? Qt::Checked : Qt::Unchecked, Qt::CheckStateRole); -} - -// ****************************************************************************************** -// Populate the 'Files to be generated' list -// ****************************************************************************************** -bool ConfigurationFilesWidget::loadGenFiles() -{ - GenerateFile file; // re-used - std::string template_path; // re-used - const std::string robot_name = config_data_->srdf_->robot_name_; - - gen_files_.clear(); // reset vector - - // Get template package location ---------------------------------------------------------------------- - fs::path template_package_path = config_data_->setup_assistant_path_; - template_package_path /= "templates"; - template_package_path /= "moveit_config_pkg_template"; - config_data_->template_package_path_ = template_package_path.make_preferred().string(); - - if (!fs::is_directory(config_data_->template_package_path_)) - { - QMessageBox::critical( - this, "Error Generating", - QString("Unable to find package template directory: ").append(config_data_->template_package_path_.c_str())); - return false; - } - - // ------------------------------------------------------------------------------------------------------------------- - // ROS PACKAGE FILES AND FOLDERS ---------------------------------------------------------------------------- - // ------------------------------------------------------------------------------------------------------------------- - - // package.xml -------------------------------------------------------------------------------------- - // Note: we call the file package.xml.template so that it isn't automatically indexed by rosprofile - // in the scenario where we want to disabled the setup_assistant by renaming its root package.xml - file.file_name_ = "package.xml"; - file.rel_path_ = file.file_name_; - template_path = config_data_->appendPaths(config_data_->template_package_path_, "package.xml.template"); - file.description_ = "Defines a ROS package"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = MoveItConfigData::AUTHOR_INFO; - gen_files_.push_back(file); - - // CMakeLists.txt -------------------------------------------------------------------------------------- - file.file_name_ = "CMakeLists.txt"; - file.rel_path_ = file.file_name_; - template_path = config_data_->appendPaths(config_data_->template_package_path_, file.file_name_); - file.description_ = "CMake build system configuration file"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // ------------------------------------------------------------------------------------------------------------------- - // CONFIG FILES ------------------------------------------------------------------------------------------------------- - // ------------------------------------------------------------------------------------------------------------------- - - // config/ -------------------------------------------------------------------------------------- - file.file_name_ = "config/"; - file.rel_path_ = file.file_name_; - file.description_ = "Folder containing all MoveIt configuration files for your robot. This folder is required and " - "cannot be disabled."; - file.gen_func_ = [this](std::string output_path) { return createFolder(output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // robot.srdf ---------------------------------------------------------------------------------------------- - file.file_name_ = config_data_->srdf_pkg_relative_path_.empty() ? config_data_->urdf_model_->getName() + ".srdf" : - config_data_->srdf_pkg_relative_path_; - file.rel_path_ = config_data_->srdf_pkg_relative_path_.empty() ? - config_data_->appendPaths(CONFIG_PATH, file.file_name_) : - config_data_->srdf_pkg_relative_path_; - file.description_ = "SRDF (Semantic Robot Description Format) is a " - "representation of semantic information about robots. This format is intended to represent " - "information about the robot that is not in the URDF file, but it is useful for a variety of " - "applications. The intention is to include information that has a semantic aspect to it."; - file.gen_func_ = [&writer = *config_data_->srdf_](std::string output_path) { return writer.writeSRDF(output_path); }; - file.write_on_changes = MoveItConfigData::SRDF; - gen_files_.push_back(file); - // special step required so the generated .setup_assistant yaml has this value - config_data_->srdf_pkg_relative_path_ = file.rel_path_; - - // gazebo_.urdf --------------------------------------------------------------------------------------- - if (config_data_->save_gazebo_urdf_) - { - file.file_name_ = "gazebo_" + config_data_->urdf_model_->getName() + ".urdf"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - file.description_ = - "This URDF file comprises your original robot description " - "augmented with tags required for use with Gazebo, i.e. defining inertia and transmission properties. " - "Checkout the URDF Gazebo documentation " - "for more infos."; - file.gen_func_ = std::bind(&MoveItConfigData::outputGazeboURDFFile, config_data_, std::placeholders::_1); - file.write_on_changes = MoveItConfigData::SIMULATION; - gen_files_.push_back(file); - } - - // ompl_planning.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "ompl_planning.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - file.description_ = "Configures the OMPL (Open Motion Planning Library) " - "planning plugin. For every planning group defined in the SRDF, a number of planning " - "configurations are specified (under planner_configs). Additionally, default settings for the " - "state space to plan in for a particular group can be specified, such as the collision checking " - "resolution. Each planning configuration specified for a group must be defined under the " - "planner_configs tag. While defining a planner configuration, the only mandatory parameter is " - "'type', which is the name of the motion planner to be used. Any other planner-specific " - "parameters can be defined but are optional."; - file.gen_func_ = [this](std::string output_path) { return config_data_->outputOMPLPlanningYAML(output_path); }; - file.write_on_changes = MoveItConfigData::GROUPS; - gen_files_.push_back(file); - - // chomp_planning.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "chomp_planning.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); - file.description_ = "Specifies which chomp planning plugin parameters to be used for the CHOMP planner"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - gen_files_.push_back(file); - - // stomp_planning.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "stomp_planning.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - file.description_ = "Specifies which stomp planning plugin parameters to be used for the STOMP planner"; - file.gen_func_ = [this](std::string output_path) { return config_data_->outputSTOMPPlanningYAML(output_path); }; - file.write_on_changes = MoveItConfigData::GROUPS; // need to double check if this is actually correct! - gen_files_.push_back(file); - - // 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_ = [this](std::string output_path) { return config_data_->outputKinematicsYAML(output_path); }; - file.write_on_changes = MoveItConfigData::GROUPS | MoveItConfigData::GROUP_KINEMATICS; - gen_files_.push_back(file); - - // joint_limits.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "joint_limits.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - file.description_ = "Contains additional information about joints that appear in your planning groups that is not " - "contained in the URDF, as well as allowing you to set maximum and minimum limits for velocity " - "and acceleration than those contained in your URDF. This information is used by our trajectory " - "filtering system to assign reasonable velocities and timing for the trajectory before it is " - "passed to the robots controllers."; - file.gen_func_ = [this](std::string output_path) { return config_data_->outputJointLimitsYAML(output_path); }; - file.write_on_changes = 0; // Can they be changed? - gen_files_.push_back(file); - - // cartesian_limits.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "cartesian_limits.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); - file.description_ = "Cartesian velocity for planning in the workspace." - "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian " - "planning requests scaled by the velocity scaling factor of an individual planning request."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; // Can they be changed? - gen_files_.push_back(file); - - // fake_controllers.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "fake_controllers.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - file.description_ = "Creates dummy configurations for controllers that correspond to defined groups. This is mostly " - "useful for testing."; - file.gen_func_ = - file.gen_func_ = [this](std::string output_path) { return config_data_->outputFakeControllersYAML(output_path); }; - file.write_on_changes = MoveItConfigData::GROUPS; - gen_files_.push_back(file); - - // simple_moveit_controllers.yaml ------------------------------------------------------------------------------- - file.file_name_ = "simple_moveit_controllers.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - file.description_ = "Creates controller configuration for SimpleMoveItControllerManager"; - file.gen_func_ = [this](std::string output_path) { return config_data_->outputSimpleControllersYAML(output_path); }; - file.write_on_changes = MoveItConfigData::GROUPS; - gen_files_.push_back(file); - - // gazebo_controllers.yaml ------------------------------------------------------------------ - file.file_name_ = "gazebo_controllers.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); - file.description_ = "Configuration of Gazebo controllers"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - gen_files_.push_back(file); - - // ros_controllers.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "ros_controllers.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - file.description_ = "Creates controller configurations for ros_control."; - file.gen_func_ = [this](std::string output_path) { return config_data_->outputROSControllersYAML(output_path); }; - file.write_on_changes = MoveItConfigData::GROUPS; - gen_files_.push_back(file); - - // sensors_3d.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "sensors_3d.yaml"; - file.rel_path_ = config_data_->appendPaths(CONFIG_PATH, file.file_name_); - file.description_ = "Creates configurations 3d sensors."; - file.gen_func_ = [this](std::string output_path) { return config_data_->output3DSensorPluginYAML(output_path); }; - file.write_on_changes = MoveItConfigData::SENSORS_CONFIG; - gen_files_.push_back(file); - - // ------------------------------------------------------------------------------------------------------------------- - // LAUNCH FILES ------------------------------------------------------------------------------------------------------ - // ------------------------------------------------------------------------------------------------------------------- - std::string launch_path = "launch"; - const std::string template_launch_path = config_data_->appendPaths(config_data_->template_package_path_, launch_path); - - // launch/ -------------------------------------------------------------------------------------- - file.file_name_ = "launch/"; - file.rel_path_ = file.file_name_; - file.description_ = "Folder containing all MoveIt launch files for your robot. " - "This folder is required and cannot be disabled."; - file.gen_func_ = [this](std::string output_path) { return createFolder(output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // move_group.launch -------------------------------------------------------------------------------------- - file.file_name_ = "move_group.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Launches the move_group node that provides the MoveGroup action and other parameters MoveGroup action"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // planning_context.launch -------------------------------------------------------------------------------------- - file.file_name_ = "planning_context.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Loads settings for the ROS parameter server, required for running MoveIt. This includes the " - "SRDF, joints_limits.yaml file, ompl_planning.yaml file, optionally the URDF, etc"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // moveit_rviz.launch -------------------------------------------------------------------------------------- - file.file_name_ = "moveit_rviz.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Visualize in Rviz the robot's planning groups running with interactive markers that allow goal " - "states to be set."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // ompl_planning_pipeline.launch - // -------------------------------------------------------------------------------------- - file.file_name_ = "ompl_planning_pipeline.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Intended to be included in other launch files that require the OMPL planning plugin. Defines " - "the proper plugin name on the parameter server and a default selection of planning request " - "adapters."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // pilz_industrial_motion_planner_planning_pipeline.launch - // -------------------------------------------------------------------------------------- - file.file_name_ = "pilz_industrial_motion_planner_planning_pipeline.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Intended to be included in other launch files that require the Pilz industrial motion planner " - "planning plugin. Defines the proper plugin name on the parameter server and a default selection " - "of planning request adapters."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // chomp_planning_pipeline.launch - // -------------------------------------------------------------------------------------- - file.file_name_ = "chomp_planning_pipeline.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Intended to be included in other launch files that require the CHOMP planning plugin. Defines " - "the proper plugin name on the parameter server and a default selection of planning request " - "adapters."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // stomp_planning_pipeline.launch - // -------------------------------------------------------------------------------------- - file.file_name_ = "stomp_planning_pipeline.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Intended to be included in other launch files that require the STOMP planning plugin. Defines " - "the proper plugin name on the parameter server and a default selection of planning request " - "adapters."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // ompl-chomp_planning_pipeline.launch.xml - // -------------------------------------------------------------------------------------- - file.file_name_ = "ompl-chomp_planning_pipeline.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = - "Intended to be included in other launch files that require the OMPL-CHOMP planning plugins. Defines " - "the proper plugin name on the parameter server and a default selection of planning request " - "adapters."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // planning_pipeline.launch -------------------------------------------------------------------------------------- - file.file_name_ = "planning_pipeline.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Helper launch file that can choose between different planning pipelines to be loaded."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // warehouse_settings.launch -------------------------------------------------------------------------------------- - file.file_name_ = "warehouse_settings.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Helper launch file that specifies default settings for MongoDB."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // warehouse.launch -------------------------------------------------------------------------------------- - file.file_name_ = "warehouse.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Launch file for starting MongoDB."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // default_warehouse_db.launch -------------------------------------------------------------------------------------- - file.file_name_ = "default_warehouse_db.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Launch file for starting the warehouse with a default MongoDB."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // run_benchmark_ompl.launch -------------------------------------------------------------------------------------- - file.file_name_ = "run_benchmark_ompl.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Launch file for benchmarking OMPL planners"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // sensor_manager.launch -------------------------------------------------------------------------------------- - file.file_name_ = "sensor_manager.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Helper launch file that can choose between different sensor managers to be loaded."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // robot_moveit_sensor_manager.launch ------------------------------------------------------------------ - file.file_name_ = robot_name + "_moveit_sensor_manager.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, "moveit_sensor_manager.launch.xml"); - file.description_ = "Placeholder for settings specific to the MoveIt sensor manager implemented for you robot."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // trajectory_execution.launch ------------------------------------------------------------------ - file.file_name_ = "trajectory_execution.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Loads settings for the ROS parameter server required for executing trajectories using the " - "trajectory_execution_manager::TrajectoryExecutionManager."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - file.file_name_ = "fake_moveit_controller_manager.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Loads the fake controller plugin."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - file.file_name_ = "simple_moveit_controller_manager.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Loads the default controller plugin."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - file.file_name_ = "ros_control_moveit_controller_manager.launch.xml"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Loads the ros_control controller plugin."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // demo.launch ------------------------------------------------------------------ - file.file_name_ = "demo.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Run a demo of MoveIt."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // demo_gazebo.launch ------------------------------------------------------------------ - file.file_name_ = "demo_gazebo.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Run a demo of MoveIt with Gazebo and Rviz"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // gazebo.launch ------------------------------------------------------------------ - file.file_name_ = "gazebo.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, "gazebo.launch"); - file.description_ = "Gazebo launch file which also launches ros_controllers and sends robot urdf to param server, " - "then using gazebo_ros pkg the robot is spawned to Gazebo"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = MoveItConfigData::SIMULATION; - gen_files_.push_back(file); - - // joystick_control.launch ------------------------------------------------------------------ - file.file_name_ = "joystick_control.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, file.file_name_); - file.description_ = "Control the Rviz Motion Planning Plugin with a joystick"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // setup_assistant.launch ------------------------------------------------------------------ - file.file_name_ = "setup_assistant.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths( - template_launch_path, "edit_configuration_package.launch"); // named this so that this launch file is not mixed - // up with the SA's real launch file - file.description_ = "Launch file for easily re-starting the MoveIt Setup Assistant to edit this robot's generated " - "configuration package."; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // ros_controllers.launch ------------------------------------------------------------------ - file.file_name_ = "ros_controllers.launch"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, "ros_controllers.launch"); - file.description_ = "ros_controllers launch file"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = MoveItConfigData::GROUPS; - gen_files_.push_back(file); - - // moveit.rviz ------------------------------------------------------------------ - file.file_name_ = "moveit.rviz"; - file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_); - template_path = config_data_->appendPaths(template_launch_path, "moveit.rviz"); - file.description_ = "Configuration file for Rviz with the Motion Planning Plugin already setup. Used by passing " - "roslaunch moveit_rviz.launch config:=true"; - file.gen_func_ = [this, template_path](std::string output_path) { return copyTemplate(template_path, output_path); }; - file.write_on_changes = 0; - gen_files_.push_back(file); - - // ------------------------------------------------------------------------------------------------------------------- - // OTHER FILES ------------------------------------------------------------------------------------------------------- - // ------------------------------------------------------------------------------------------------------------------- - - // .setup_assistant ------------------------------------------------------------------ - file.file_name_ = SETUP_ASSISTANT_FILE; - file.rel_path_ = file.file_name_; - file.description_ = "MoveIt Setup Assistant's hidden settings file. You should not need to edit this file."; - file.gen_func_ = [this](const std::string& file_path) { return outputSetupAssistantFile(file_path); }; - file.write_on_changes = -1; // write on any changes - gen_files_.push_back(file); - - return true; -} - -// ****************************************************************************************** -// Verify with user if certain screens have not been completed -// ****************************************************************************************** -bool ConfigurationFilesWidget::checkDependencies() -{ - QStringList dependencies; - bool required_actions = false; - - // Check that at least 1 planning group exists - if (config_data_->srdf_->groups_.empty()) - { - dependencies << "No robot model planning groups have been created"; - } - - // Check that at least 1 link pair is disabled from collision checking - if (config_data_->srdf_->disabled_collision_pairs_.empty()) - { - dependencies << "No self-collisions have been disabled"; - } - - // Check that there is at least 1 end effector added - if (config_data_->srdf_->end_effectors_.empty()) - { - dependencies << "No end effectors have been added"; - } - - // Check that there is at least 1 virtual joint added - if (config_data_->srdf_->virtual_joints_.empty()) - { - dependencies << "No virtual joints have been added"; - } - - // Check that there is a author name - if (config_data_->author_name_.find_first_not_of(' ') == std::string::npos) - { - // There is no name or it consists of whitespaces only - dependencies << "No author name added"; - required_actions = true; - } - - // Check that email information is filled - QRegExp mail_regex("\\b[A-Z0-9._%+-]+@[A-Z0-9.-]+\\.[A-Z]{2,4}\\b"); - mail_regex.setCaseSensitivity(Qt::CaseInsensitive); - mail_regex.setPatternSyntax(QRegExp::RegExp); - QString test_email = QString::fromStdString(config_data_->author_email_); - if (!mail_regex.exactMatch(test_email)) - { - dependencies << "No valid email address added"; - required_actions = true; - } - - // Display all accumumlated errors: - if (!dependencies.empty()) - { - // Create a dependency message - QString dep_message; - if (!required_actions) - { - dep_message = "Some setup steps have not been completed. None of the steps are required, but here is a reminder " - "of what was not filled in, just in case something was forgotten:
      "; - } - else - { - dep_message = "Some setup steps have not been completed. Please fix the required steps (printed in bold), " - "otherwise the setup cannot be completed:
        "; - } - - for (int i = 0; i < dependencies.size(); ++i) - { - dep_message.append("
      • ").append(dependencies.at(i)).append("
        • "); - } - - if (!required_actions) - { - dep_message.append("

      Press Ok to continue generating files."); - if (QMessageBox::question(this, "Incomplete MoveIt Setup Assistant Steps", dep_message, - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) - { - return false; // abort - } - } - else - { - QMessageBox::warning(this, "Incomplete MoveIt Setup Assistant Steps", dep_message); - return false; - } - } - - return true; -} - -// ****************************************************************************************** -// A function for showing progress and user feedback about what happened -// ****************************************************************************************** -void ConfigurationFilesWidget::updateProgress() -{ - action_num_++; - - // Calc percentage - progress_bar_->setValue(double(action_num_) / gen_files_.size() * 100); - - // allow the progress bar to be shown - QApplication::processEvents(); -} - -// ****************************************************************************************** -// Display the selected action in the desc box -// ****************************************************************************************** -void ConfigurationFilesWidget::changeActionDesc(int id) -{ - // Only allow event if list is not empty - if (id >= 0) - { - // Show the selected text - action_label_->setText(action_desc_.at(id)); - } -} - -// ****************************************************************************************** -// Disable or enable item in gen_files_ array -// ****************************************************************************************** -void ConfigurationFilesWidget::changeCheckedState(QListWidgetItem* item) -{ - std::size_t index = item->data(Qt::UserRole).toUInt(); - bool generate = (item->checkState() == Qt::Checked); - - if (!generate && (gen_files_[index].write_on_changes & config_data_->changes)) - { - QMessageBox::warning(this, "Package Generation", - "You should generate this file to ensure your changes will take " - "effect."); - } - - // Enable/disable file - gen_files_[index].generate_ = generate; -} - -// ****************************************************************************************** -// Called when setup assistant navigation switches to this screen -// ****************************************************************************************** -void ConfigurationFilesWidget::focusGiven() -{ - loadGenFiles(); - - // Which files have been modified outside the Setup Assistant? - bool files_already_modified = checkGenFiles(); - - // disable reaction to checkbox changes - disconnect(action_list_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(changeCheckedState(QListWidgetItem*))); - - // Show files in GUI - bool have_conflicting_changes = showGenFiles(); - - // react to manual changes only (not programmatic ones) - connect(action_list_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(changeCheckedState(QListWidgetItem*))); - - // Allow list box to populate - QApplication::processEvents(); - - if (files_already_modified) - { - // Some were found to be modified - QString msg("Some files have been modified outside of the Setup Assistant (according to timestamp). " - "The Setup Assistant will not overwrite these changes by default because often changing configuration " - "files manually is necessary, " - "but we recommend you check the list and enable the checkbox next to files you would like to " - "overwrite. "); - if (have_conflicting_changes) - msg += "
      Attention: Some files (marked red) are changed " - "both, externally and in Setup Assistant."; - QMessageBox::information(this, "Files Modified", msg); - } -} - -// ****************************************************************************************** -// Check the list of files to be generated for modification -// Returns true if files were detected as modified -// ****************************************************************************************** -bool ConfigurationFilesWidget::checkGenFiles() -{ - // Check if we are 'editing' a prev generated config pkg - if (config_data_->config_pkg_path_.empty()) - return false; // this is a new package - - // Check if we have the previous modification timestamp to compare against - if (config_data_->config_pkg_generated_timestamp_ == 0) - return false; // this package has not been generated with a timestamp, backwards compatible. - - static const std::time_t TIME_MOD_TOLERANCE = 10; - - // Check all old file's modification time - bool found_modified = false; - for (GenerateFile& gen_file : gen_files_) - { - GenerateFile* file = &gen_file; - - fs::path file_path = config_data_->appendPaths(config_data_->config_pkg_path_, file->rel_path_); - - // Don't disable folders from being generated - if (fs::is_directory(file_path)) - continue; - - if (fs::is_regular_file(file_path)) - { - std::time_t mod_time = fs::last_write_time(file_path); - - // ROS_DEBUG_STREAM("File " << file->file_name_ << " file modified " << mod_time << " pkg modified " << - // config_data_->config_pkg_generated_timestamp_); - - if (mod_time > config_data_->config_pkg_generated_timestamp_ + TIME_MOD_TOLERANCE || - mod_time < config_data_->config_pkg_generated_timestamp_ - TIME_MOD_TOLERANCE) - { - ROS_INFO_STREAM("Manual editing detected: not over-writing by default file " << file->file_name_); - - if (file->write_on_changes & config_data_->changes) - ROS_WARN_STREAM("Editing in Setup Assistant conflicts with external editing of file " << file->file_name_); - - file->generate_ = false; // do not overwrite by default - file->modified_ = true; - found_modified = true; - } - else - { - file->modified_ = false; - } - } - } - - // Warn user if files have been modified outside Setup Assistant - return found_modified; -} - -// ****************************************************************************************** -// Show the list of files to be generated -// ****************************************************************************************** -bool ConfigurationFilesWidget::showGenFiles() -{ - bool have_conflicting_changes = false; - action_list_->clear(); - - // Display this list in the GUI - for (std::size_t i = 0; i < gen_files_.size(); ++i) - { - GenerateFile* file = &gen_files_[i]; - - // Create a formatted row - QListWidgetItem* item = new QListWidgetItem(QString(file->rel_path_.c_str()), action_list_, 0); - - fs::path file_path = config_data_->appendPaths(config_data_->config_pkg_path_, file->rel_path_); - - // Checkbox - item->setCheckState(file->generate_ ? Qt::Checked : Qt::Unchecked); - // externally modified? - if (file->modified_) - { - if (file->write_on_changes & config_data_->changes) - { - have_conflicting_changes = true; - item->setForeground(QBrush(QColor(255, 0, 0))); - } - else - item->setForeground(QBrush(QColor(255, 135, 0))); - } - - // Don't allow folders to be disabled - if (fs::is_directory(file_path)) - { - item->setFlags(Qt::ItemIsSelectable | Qt::ItemIsEnabled); - } - - // Link the gen_files_ index to this item - item->setData(Qt::UserRole, QVariant(static_cast(i))); - - // Add actions to list - action_list_->addItem(item); - action_desc_.append(QString(file->description_.c_str())); - } - - // Select the first item in the list so that a description is visible - action_list_->setCurrentRow(0); - return have_conflicting_changes; -} - -// ****************************************************************************************** -// Save configuration click event -// ****************************************************************************************** -void ConfigurationFilesWidget::savePackage() -{ - // Feedback - success_label_->hide(); - - // Reset the progress bar counter and GUI stuff - action_num_ = 0; - progress_bar_->setValue(0); - - if (!generatePackage()) - { - ROS_ERROR_STREAM("Failed to generate entire configuration package"); - return; - } - - // Alert user it completed successfully -------------------------------------------------- - progress_bar_->setValue(100); - success_label_->show(); - has_generated_pkg_ = true; -} - -// ****************************************************************************************** -// Save package using default path -// ****************************************************************************************** -bool ConfigurationFilesWidget::generatePackage() -{ - // Get path name - std::string new_package_path = stack_path_->getPath(); - - // Check that a valid stack package name has been given - if (new_package_path.empty()) - { - QMessageBox::warning(this, "Error Generating", - "No package path provided. Please choose a directory location to " - "generate the MoveIt configuration files."); - return false; - } - - // Check setup assist deps - if (!checkDependencies()) - return false; // canceled - - // Check that all groups have components - if (!noGroupsEmpty()) - return false; // not ready - - // Trim whitespace from user input - boost::trim(new_package_path); - - // Get the package name --------------------------------------------------------------------------------- - new_package_name_ = getPackageName(new_package_path); - - const std::string setup_assistant_file = config_data_->appendPaths(new_package_path, SETUP_ASSISTANT_FILE); - - // Make sure old package is correct package type and verify over write - if (fs::is_directory(new_package_path) && !fs::is_empty(new_package_path)) - { - // Check if the old package is a setup assistant package. If it is not, quit - if (!fs::is_regular_file(setup_assistant_file)) - { - QMessageBox::warning( - this, "Incorrect Folder/Package", - QString("The chosen package location already exists but was not previously created using this MoveIt Setup " - "Assistant. " - "If this is a mistake, add the missing file: ") - .append(setup_assistant_file.c_str())); - return false; - } - - // Confirm overwrite - if (QMessageBox::question(this, "Confirm Package Update", - QString("Are you sure you want to overwrite this existing package with updated " - "configurations?
      ") - .append(new_package_path.c_str()) - .append(""), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) - { - return false; // abort - } - } - else // this is a new package (but maybe the folder already exists) - { - // Create new directory ------------------------------------------------------------------ - try - { - fs::create_directory(new_package_path) && !fs::is_directory(new_package_path); - } - catch (...) - { - QMessageBox::critical(this, "Error Generating Files", - QString("Unable to create directory ").append(new_package_path.c_str())); - return false; - } - } - - // Begin to create files and folders ---------------------------------------------------------------------- - std::string absolute_path; - - for (GenerateFile& gen_file : gen_files_) - { - GenerateFile* file = &gen_file; - - // Check if we should skip this file - if (!file->generate_) - { - continue; - } - - // Create the absolute path - absolute_path = config_data_->appendPaths(new_package_path, file->rel_path_); - ROS_DEBUG_STREAM("Creating file " << absolute_path); - - // Clear template strings in case export is run multiple times with changes in between - template_strings_.clear(); - - // Run the generate function - if (!file->gen_func_(absolute_path)) - { - // Error occurred - QMessageBox::critical(this, "Error Generating File", - QString("Failed to generate folder or file: '") - .append(file->rel_path_.c_str()) - .append("' at location:\n") - .append(absolute_path.c_str())); - return false; - } - updateProgress(); // Increment and update GUI - } - - return true; -} - -// ****************************************************************************************** -// Quit the program because we are done -// ****************************************************************************************** -void ConfigurationFilesWidget::exitSetupAssistant() -{ - if (has_generated_pkg_ || QMessageBox::question(this, "Exit Setup Assistant", - QString("Are you sure you want to exit the MoveIt Setup Assistant?"), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Ok) - { - QApplication::quit(); - } -} - -// ****************************************************************************************** -// Get the last folder name in a directory path -// ****************************************************************************************** -const std::string ConfigurationFilesWidget::getPackageName(std::string package_path) -{ - // Remove end slash if there is one - if (!package_path.compare(package_path.size() - 1, 1, "/")) - { - package_path = package_path.substr(0, package_path.size() - 1); - } - - // Get the last directory name - std::string package_name; - fs::path fs_package_path = package_path; - - package_name = fs_package_path.filename().string(); - - // check for empty - if (package_name.empty()) - package_name = "unknown"; - - return package_name; -} - -// ****************************************************************************************** -// Check that no group is empty (without links/joints/etc) -// ****************************************************************************************** -bool ConfigurationFilesWidget::noGroupsEmpty() -{ - // Loop through all groups - for (const auto& group : config_data_->srdf_->groups_) - { - // Whenever 1 of the 4 component types are found, stop checking this group - if (!group.joints_.empty()) - continue; - if (!group.links_.empty()) - continue; - if (!group.chains_.empty()) - continue; - if (!group.subgroups_.empty()) - continue; - - // This group has no contents, bad - QMessageBox::warning( - this, "Empty Group", - QString("The planning group '") - .append(group.name_.c_str()) - .append("' is empty and has no subcomponents associated with it (joints/links/chains/subgroups). You must " - "edit or remove this planning group before this configuration package can be saved.")); - return false; - } - - return true; // good -} - -// ****************************************************************************************** -// Load the strings that will be replaced in all templates -// ****************************************************************************************** -void ConfigurationFilesWidget::loadTemplateStrings() -{ - // Pair 1 - addTemplateString("[GENERATED_PACKAGE_NAME]", new_package_name_); - - // Pair 2 - std::string urdf_location = config_data_->urdf_pkg_name_.empty() ? config_data_->urdf_path_ : - "$(find " + config_data_->urdf_pkg_name_ + ")/" + - config_data_->urdf_pkg_relative_path_; - addTemplateString("[URDF_LOCATION]", urdf_location); - - // Pair 3 - if (config_data_->urdf_from_xacro_) - addTemplateString("[URDF_LOAD_ATTRIBUTE]", - "command=\"xacro " + config_data_->xacro_args_ + " '" + urdf_location + "'\""); - else - addTemplateString("[URDF_LOAD_ATTRIBUTE]", "textfile=\"" + urdf_location + "\""); - - // Pair 4 - if (config_data_->save_gazebo_urdf_) - { - std::string file_name = "gazebo_" + config_data_->urdf_model_->getName() + ".urdf"; - std::string rel_path = config_data_->appendPaths(CONFIG_PATH, file_name); - addTemplateString("[GAZEBO_URDF_LOAD_ATTRIBUTE]", "textfile=\"$(find " + new_package_name_ + ")/" + rel_path + "\""); - } - else // reuse [URDF_LOAD_ATTRIBUTE] template - addTemplateString("[GAZEBO_URDF_LOAD_ATTRIBUTE]", template_strings_.back().second); - - // Pair 5 - addTemplateString("[ROBOT_NAME]", config_data_->srdf_->robot_name_); - - // Pair 6 - addTemplateString("[ROBOT_ROOT_LINK]", config_data_->getRobotModel()->getRootLinkName()); - - // Pair 7 - addTemplateString("[PLANNING_FRAME]", config_data_->getRobotModel()->getModelFrame()); - - // Pair 8 - std::stringstream vjb; - for (std::size_t i = 0; i < config_data_->srdf_->virtual_joints_.size(); ++i) - { - const srdf::Model::VirtualJoint& vj = config_data_->srdf_->virtual_joints_[i]; - vjb << " " << '\n'; - } - addTemplateString("[VIRTUAL_JOINT_BROADCASTER]", vjb.str()); - - // Pair 9 - Add dependencies to package.xml if the robot.urdf file is relative to a ROS package - if (config_data_->urdf_pkg_name_.empty()) - { - addTemplateString("[OTHER_DEPENDENCIES", ""); // not relative to a ROS package - } - else - { - std::stringstream deps; - deps << " " << config_data_->urdf_pkg_name_ << "\n"; - addTemplateString("[OTHER_DEPENDENCIES]", deps.str()); // not relative to a ROS package - } - - // Pair 10 - List of ROS Controllers to load in ros_controllers.launch file - if (config_data_->getControllers().empty()) - { - addTemplateString("[ROS_CONTROLLERS]", ""); - } - else - { - std::stringstream controllers; - for (ControllerConfig& controller : config_data_->getControllers()) - { - // Check if the controller belongs to controller_list namespace - if (controller.type_ != "FollowJointTrajectory") - controllers << controller.name_ << " "; - } - addTemplateString("[ROS_CONTROLLERS]", controllers.str()); - } - - // Pair 11 - 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; - } - addTemplateString("[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]", kinematics_parameters_files_block); - - addTemplateString("[AUTHOR_NAME]", config_data_->author_name_); - addTemplateString("[AUTHOR_EMAIL]", config_data_->author_email_); - - { - std::stringstream joints; - for (const auto& pair : config_data_->getInitialJoints()) - joints << " -J " << pair.first << " " << pair.second; - addTemplateString("[GAZEBO_INITIAL_JOINTS]", joints.str()); - } -} - -// ****************************************************************************************** -// Insert a string pair into the template_strings_ datastructure -// ****************************************************************************************** -bool ConfigurationFilesWidget::addTemplateString(const std::string& key, const std::string& value) -{ - template_strings_.push_back(std::pair(key, value)); - - return true; -} - -// ****************************************************************************************** -// Copy a template from location to location and replace package name -// ****************************************************************************************** -bool ConfigurationFilesWidget::copyTemplate(const std::string& template_path, const std::string& output_path) -{ - // Check if template strings have been loaded yet - if (template_strings_.empty()) - { - loadTemplateStrings(); - } - - // Error check file - if (!fs::is_regular_file(template_path)) - { - ROS_ERROR_STREAM("Unable to find template file " << template_path); - return false; - } - - // Load file - std::ifstream template_stream(template_path.c_str()); - if (!template_stream.good()) // File not found - { - ROS_ERROR_STREAM("Unable to load file " << template_path); - return false; - } - - // Load the file to a string using an efficient memory allocation technique - std::string template_string; - template_stream.seekg(0, std::ios::end); - template_string.reserve(template_stream.tellg()); - template_stream.seekg(0, std::ios::beg); - template_string.assign((std::istreambuf_iterator(template_stream)), std::istreambuf_iterator()); - template_stream.close(); - - // Replace keywords in string ------------------------------------------------------------ - for (std::pair& it : template_strings_) - { - boost::replace_all(template_string, it.first, it.second); - } - - // Save string to new location ----------------------------------------------------------- - std::ofstream output_stream(output_path.c_str(), std::ios_base::trunc); - if (!output_stream.good()) - { - ROS_ERROR_STREAM("Unable to open file for writing " << output_path); - return false; - } - - output_stream << template_string.c_str(); - output_stream.close(); - - return true; // file created successfully -} - -// ****************************************************************************************** -// Create a folder -// ****************************************************************************************** -bool ConfigurationFilesWidget::createFolder(const std::string& output_path) -{ - if (!fs::is_directory(output_path)) - { - if (!fs::create_directory(output_path)) - { - QMessageBox::critical(this, "Error Generating Files", - QString("Unable to create directory ").append(output_path.c_str())); - return false; - } - } - return true; -} - -} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp deleted file mode 100644 index b4ec5c39f4..0000000000 --- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp +++ /dev/null @@ -1,531 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Dave Coleman */ - -// SA -#include "setup_screen_widget.h" // a base class for screens in the setup assistant -#include "setup_assistant_widget.h" -#include "header_widget.h" - -// Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // for loading all avail kinematic planners -// Rviz -#include -#include -#include -#include -#include - -namespace moveit_setup_assistant -{ -// ****************************************************************************************** -// Outer User Interface for MoveIt Configuration Assistant -// ****************************************************************************************** -SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, const boost::program_options::variables_map& args) - : QWidget(parent) -{ - rviz_manager_ = nullptr; - rviz_render_panel_ = nullptr; - - // Create object to hold all MoveIt configuration data - config_data_ = std::make_shared(); - - // Set debug mode flag if necessary - if (args.count("debug")) - config_data_->debug_ = true; - - // Setting the window icon - std::string moveit_ros_visualization_package_path = ros::package::getPath("moveit_ros_visualization"); - moveit_ros_visualization_package_path += "/icons/classes/MotionPlanning.png"; - this->setWindowIcon(QIcon(moveit_ros_visualization_package_path.c_str())); - - // Basic widget container ----------------------------------------- - QHBoxLayout* layout = new QHBoxLayout(); - layout->setAlignment(Qt::AlignTop); - - // Create main content stack for various screens - main_content_ = new QStackedWidget(); - current_index_ = 0; - - // Screens -------------------------------------------------------- - - // Start Screen - start_screen_widget_ = new StartScreenWidget(this, config_data_); - start_screen_widget_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); - connect(start_screen_widget_, SIGNAL(readyToProgress()), this, SLOT(progressPastStartScreen())); - connect(start_screen_widget_, SIGNAL(loadRviz()), this, SLOT(loadRviz())); - main_content_->addWidget(start_screen_widget_); - - // Pass command arg values to start screen and show appropriate part of screen - if (args.count("urdf_path")) - { - start_screen_widget_->urdf_file_->setPath(args["urdf_path"].as()); - start_screen_widget_->select_mode_->btn_new_->click(); - } - if (args.count("config_pkg")) - { - start_screen_widget_->stack_path_->setPath(args["config_pkg"].as()); - start_screen_widget_->select_mode_->btn_exist_->click(); - } - else - { - start_screen_widget_->stack_path_->setPath(QString(getenv("PWD"))); - } - - // Add Navigation Buttons (but do not load widgets yet except start screen) - nav_name_list_ << "Start"; - nav_name_list_ << "Self-Collisions"; - nav_name_list_ << "Virtual Joints"; - nav_name_list_ << "Planning Groups"; - nav_name_list_ << "Robot Poses"; - nav_name_list_ << "End Effectors"; - nav_name_list_ << "Passive Joints"; - nav_name_list_ << "Controllers"; - nav_name_list_ << "Simulation"; - nav_name_list_ << "3D Perception"; - nav_name_list_ << "Author Information"; - nav_name_list_ << "Configuration Files"; - - // Navigation Left Pane -------------------------------------------------- - navs_view_ = new NavigationWidget(this); - navs_view_->setNavs(nav_name_list_); - navs_view_->setDisabled(true); - navs_view_->setSelected(0); // start screen - - // Rviz View Right Pane --------------------------------------------------- - rviz_container_ = new QWidget(this); - rviz_container_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - rviz_container_->hide(); // do not show until after the start screen - - // Split screen ----------------------------------------------------- - splitter_ = new QSplitter(Qt::Horizontal, this); - splitter_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - splitter_->addWidget(navs_view_); - splitter_->addWidget(main_content_); - splitter_->addWidget(rviz_container_); - splitter_->setHandleWidth(6); - // splitter_->setCollapsible( 0, false ); // don't let navigation collapse - layout->addWidget(splitter_); - - // Add event for switching between screens ------------------------- - connect(navs_view_, SIGNAL(clicked(const QModelIndex&)), this, SLOT(navigationClicked(const QModelIndex&))); - - // Final Layout Setup --------------------------------------------- - this->setLayout(layout); - - // Title - this->setWindowTitle("MoveIt Setup Assistant"); // title of window - - // Show screen before message - QApplication::processEvents(); -} - -// ****************************************************************************************** -// Decontructor -// ****************************************************************************************** -SetupAssistantWidget::~SetupAssistantWidget() -{ - if (rviz_manager_ != nullptr) - rviz_manager_->removeAllDisplays(); - if (rviz_render_panel_ != nullptr) - delete rviz_render_panel_; - if (rviz_manager_ != nullptr) - delete rviz_manager_; -} - -void SetupAssistantWidget::virtualJointReferenceFrameChanged() -{ - if (rviz_manager_ && robot_state_display_) - { - rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame())); - robot_state_display_->reset(); - robot_state_display_->setVisible(true); - } -} - -// ****************************************************************************************** -// Change screens of Setup Assistant -// ****************************************************************************************** -void SetupAssistantWidget::navigationClicked(const QModelIndex& index) -{ - // Convert QModelIndex to int - moveToScreen(index.row()); -} - -// ****************************************************************************************** -// Change screens -// ****************************************************************************************** -void SetupAssistantWidget::moveToScreen(const int index) -{ - std::scoped_lock slock(change_screen_lock_); - - if (current_index_ != index) - { - // Send the focus lost command to the screen widget - SetupScreenWidget* ssw = qobject_cast(main_content_->widget(current_index_)); - if (!ssw->focusLost()) - { - navs_view_->setSelected(current_index_); - return; // switching not accepted - } - - current_index_ = index; - - // Unhighlight anything on robot - unhighlightAll(); - - // Change screens - main_content_->setCurrentIndex(index); - - // Send the focus given command to the screen widget - ssw = qobject_cast(main_content_->widget(index)); - ssw->focusGiven(); - - // Change navigation selected option - navs_view_->setSelected(index); // Select first item in list - } -} - -// ****************************************************************************************** -// Loads other windows, enables navigation -// ****************************************************************************************** -void SetupAssistantWidget::progressPastStartScreen() -{ - // Load all widgets ------------------------------------------------ - - // Self-Collisions - default_collisions_widget_ = new DefaultCollisionsWidget(this, config_data_); - main_content_->addWidget(default_collisions_widget_); - connect(default_collisions_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(default_collisions_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(default_collisions_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // Virtual Joints - virtual_joints_widget_ = new VirtualJointsWidget(this, config_data_); - main_content_->addWidget(virtual_joints_widget_); - connect(virtual_joints_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(virtual_joints_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(virtual_joints_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(virtual_joints_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - connect(virtual_joints_widget_, SIGNAL(referenceFrameChanged()), this, SLOT(virtualJointReferenceFrameChanged())); - - // Planning Groups - planning_groups_widget = new PlanningGroupsWidget(this, config_data_); - main_content_->addWidget(planning_groups_widget); - connect(planning_groups_widget, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(planning_groups_widget, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(planning_groups_widget, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(planning_groups_widget, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // Robot Poses - robot_poses_widget_ = new RobotPosesWidget(this, config_data_); - main_content_->addWidget(robot_poses_widget_); - connect(robot_poses_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(robot_poses_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(robot_poses_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(robot_poses_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // End Effectors - end_effectors_widget_ = new EndEffectorsWidget(this, config_data_); - main_content_->addWidget(end_effectors_widget_); - connect(end_effectors_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(end_effectors_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(end_effectors_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(end_effectors_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // Virtual Joints - passive_joints_widget_ = new PassiveJointsWidget(this, config_data_); - main_content_->addWidget(passive_joints_widget_); - connect(passive_joints_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(passive_joints_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(passive_joints_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(passive_joints_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // Controllers - controllers_widget_ = new ControllersWidget(this, config_data_); - main_content_->addWidget(controllers_widget_); - connect(controllers_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(controllers_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(controllers_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(controllers_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // Simulation Screen - simulation_widget_ = new SimulationWidget(this, config_data_); - main_content_->addWidget(simulation_widget_); - connect(simulation_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(simulation_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(simulation_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(simulation_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // Perception - perception_widget_ = new PerceptionWidget(this, config_data_); - main_content_->addWidget(perception_widget_); - connect(perception_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(perception_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(perception_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(perception_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // Author Information - author_information_widget_ = new AuthorInformationWidget(this, config_data_); - main_content_->addWidget(author_information_widget_); - connect(author_information_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool))); - connect(author_information_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this, - SLOT(highlightLink(const std::string&, const QColor&))); - connect(author_information_widget_, SIGNAL(highlightGroup(const std::string&)), this, - SLOT(highlightGroup(const std::string&))); - connect(author_information_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll())); - - // Configuration Files - configuration_files_widget_ = new ConfigurationFilesWidget(this, config_data_); - main_content_->addWidget(configuration_files_widget_); - - // Enable all nav buttons ------------------------------------------- - for (int i = 0; i < nav_name_list_.count(); ++i) - { - navs_view_->setEnabled(i, true); - } - - // Enable navigation - navs_view_->setDisabled(false); - - // Replace logo with Rviz screen - rviz_container_->show(); - - // Move to next screen in debug mode - if (config_data_->debug_) - { - moveToScreen(3); - } -} - -// ****************************************************************************************** -// Ping ROS on internval -// ****************************************************************************************** -void SetupAssistantWidget::updateTimer() -{ - ros::spinOnce(); // keep ROS node alive -} - -// ****************************************************************************************** -// Load Rviz once we have a robot description ready -// ****************************************************************************************** -void SetupAssistantWidget::loadRviz() -{ - // Create rviz frame - rviz_render_panel_ = new rviz::RenderPanel(); - rviz_render_panel_->setMinimumWidth(200); - rviz_render_panel_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - - rviz_manager_ = new rviz::VisualizationManager(rviz_render_panel_); - rviz_render_panel_->initialize(rviz_manager_->getSceneManager(), rviz_manager_); - rviz_manager_->initialize(); - rviz_manager_->startUpdate(); - - // Set the fixed and target frame - rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame())); - - // Create the MoveIt Rviz Plugin and attach to display - robot_state_display_ = new moveit_rviz_plugin::RobotStateDisplay(); - robot_state_display_->setName("Robot State"); - - rviz_manager_->addDisplay(robot_state_display_, true); - - // Set the topic on which the moveit_msgs::msg::PlanningScene messages are received - robot_state_display_->subProp("Robot State Topic")->setValue(QString::fromStdString(MOVEIT_ROBOT_STATE)); - - // Set robot description - robot_state_display_->subProp("Robot Description")->setValue(QString::fromStdString(ROBOT_DESCRIPTION)); - robot_state_display_->setVisible(true); - - // Zoom into robot - rviz::ViewController* view = rviz_manager_->getViewManager()->getCurrent(); - view->subProp("Distance")->setValue(4.0f); - - // Add Rviz to Planning Groups Widget - QVBoxLayout* rviz_layout = new QVBoxLayout(); - rviz_layout->addWidget(rviz_render_panel_); - rviz_container_->setLayout(rviz_layout); - - // visual / collision buttons - auto btn_layout = new QHBoxLayout(); - rviz_layout->addLayout(btn_layout); - - QCheckBox* btn; - btn_layout->addWidget(btn = new QCheckBox("visual"), 0); - btn->setChecked(true); - connect(btn, &QCheckBox::toggled, - [this](bool checked) { robot_state_display_->subProp("Visual Enabled")->setValue(checked); }); - - btn_layout->addWidget(btn = new QCheckBox("collision"), 1); - btn->setChecked(false); - connect(btn, &QCheckBox::toggled, - [this](bool checked) { robot_state_display_->subProp("Collision Enabled")->setValue(checked); }); - - rviz_container_->show(); -} - -// ****************************************************************************************** -// Highlight a robot link -// ****************************************************************************************** -void SetupAssistantWidget::highlightLink(const std::string& link_name, const QColor& color) -{ - const moveit::core::LinkModel* lm = config_data_->getRobotModel()->getLinkModel(link_name); - if (!lm->getShapes().empty()) // skip links with no geometry - robot_state_display_->setLinkColor(link_name, color); -} - -// ****************************************************************************************** -// Highlight a robot group -// ****************************************************************************************** -void SetupAssistantWidget::highlightGroup(const std::string& group_name) -{ - // Highlight the selected planning group by looping through the links - if (!config_data_->getRobotModel()->hasJointModelGroup(group_name)) - return; - - const moveit::core::JointModelGroup* joint_model_group = - config_data_->getRobotModel()->getJointModelGroup(group_name); - if (joint_model_group) - { - // Iterate through the links - for (const moveit::core::LinkModel* lm : joint_model_group->getLinkModels()) - highlightLink(lm->getName(), QColor(255, 0, 0)); - } -} - -// ****************************************************************************************** -// Unhighlight all robot links -// ****************************************************************************************** -void SetupAssistantWidget::unhighlightAll() -{ - // Get the names of the all links robot - const std::vector& links = config_data_->getRobotModel()->getLinkModelNamesWithCollisionGeometry(); - - // Quit if no links found - if (links.empty()) - { - return; - } - - // check if rviz is ready - if (!rviz_manager_ || !robot_state_display_) - { - return; - } - - // Iterate through the links - for (const std::string& link : links) - { - if (link.empty()) - continue; - - robot_state_display_->unsetLinkColor(link); - } -} - -// ****************************************************************************************** -// Qt close event function for reminding user to save -// ****************************************************************************************** -void SetupAssistantWidget::closeEvent(QCloseEvent* event) -{ - // Only prompt to close if not in debug mode - if (!config_data_->debug_) - { - if (QMessageBox::question(this, "Exit Setup Assistant", - QString("Are you sure you want to exit the MoveIt Setup Assistant?"), - QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel) - { - event->ignore(); - return; - } - } - - // Shutdown app - event->accept(); -} - -// ****************************************************************************************** -// Qt Error Handling - TODO -// ****************************************************************************************** -bool SetupAssistantWidget::notify(QObject* /*receiver*/, QEvent* /*event*/) -{ - QMessageBox::critical(this, "Error", "An error occurred and was caught by Qt notify event handler.", QMessageBox::Ok); - - return false; -} - -// ****************************************************************************************** -// Change the widget modal state based on subwidgets state -// ****************************************************************************************** -void SetupAssistantWidget::setModalMode(bool isModal) -{ - navs_view_->setDisabled(isModal); - - for (int i = 0; i < nav_name_list_.count(); ++i) - { - navs_view_->setEnabled(i, !isModal); - } -} - -} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp deleted file mode 100644 index 51d42f31b5..0000000000 --- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp +++ /dev/null @@ -1,802 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Dave Coleman */ - -// Qt -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// ROS -#include // for getting file path for loadng images -// SA -#include "header_widget.h" // title and instructions -#include "start_screen_widget.h" -// C -#include // for reading in urdf -#include -#include -// MoveIt -#include - -// chdir -#ifdef _WIN32 -#include -#else -#include -#endif - -namespace moveit_setup_assistant -{ -// Boost file system -namespace fs = std::filesystem; - -// ****************************************************************************************** -// Start screen user interface for MoveIt Configuration Assistant -// ****************************************************************************************** -StartScreenWidget::StartScreenWidget(QWidget* parent, const MoveItConfigDataPtr& config_data) - : SetupScreenWidget(parent), config_data_(config_data) -{ - // Basic widget container - QVBoxLayout* layout = new QVBoxLayout(this); - - // Horizontal layout splitter - QHBoxLayout* hlayout = new QHBoxLayout(); - // Left side of screen - QVBoxLayout* left_layout = new QVBoxLayout(); - // Right side of screen - QVBoxLayout* right_layout = new QVBoxLayout(); - - // Right Image Area ---------------------------------------------- - right_image_ = new QImage(); - right_image_label_ = new QLabel(this); - std::string image_path = "./resources/MoveIt_Setup_Assistant2.png"; - if (chdir(config_data_->setup_assistant_path_.c_str()) != 0) - { - ROS_ERROR("FAILED TO CHANGE PACKAGE TO moveit_setup_assistant"); - } - - if (right_image_->load(image_path.c_str())) - { - right_image_label_->setPixmap(QPixmap::fromImage(*right_image_)); - right_image_label_->setMinimumHeight(384); // size of right_image_label_ - } - else - { - ROS_ERROR_STREAM("FAILED TO LOAD " << image_path); - } - - right_layout->addWidget(right_image_label_); - right_layout->setAlignment(right_image_label_, Qt::AlignRight | Qt::AlignTop); - - // Top Label Area --------------------------------------------------- - HeaderWidget* header = new HeaderWidget("MoveIt Setup Assistant", - "These tools will assist you in creating a Semantic Robot " - "Description Format (SRDF) file, various yaml configuration and many " - "roslaunch files for utilizing all aspects of MoveIt functionality.", - this); - layout->addWidget(header); - - // Select Mode Area ------------------------------------------------- - select_mode_ = new SelectModeWidget(this); - connect(select_mode_->btn_new_, SIGNAL(clicked()), this, SLOT(showNewOptions())); - connect(select_mode_->btn_exist_, SIGNAL(clicked()), this, SLOT(showExistingOptions())); - left_layout->addWidget(select_mode_); - - // Path Box Area ---------------------------------------------------- - - // Stack Path Dialog - stack_path_ = - new LoadPathArgsWidget("Load MoveIt Configuration Package", - "Specify the package name or path of an existing MoveIt configuration package to be " - "edited for your robot. Example package name: panda_moveit_config", - "optional xacro arguments:", this, true); // directory - // user needs to select option before this is shown - stack_path_->hide(); - stack_path_->setArgs(""); - connect(stack_path_, SIGNAL(pathChanged(QString)), this, SLOT(onPackagePathChanged(QString))); - left_layout->addWidget(stack_path_); - - // URDF File Dialog - urdf_file_ = new LoadPathArgsWidget("Load a URDF or COLLADA Robot Model", - "Specify the location of an existing Universal Robot Description Format or " - "COLLADA file for your robot", - "optional xacro arguments:", this, false, true); // no directory, load only - // user needs to select option before this is shown - urdf_file_->hide(); - urdf_file_->setArgs(""); - connect(urdf_file_, SIGNAL(pathChanged(QString)), this, SLOT(onUrdfPathChanged(QString))); - left_layout->addWidget(urdf_file_); - - // Load settings box --------------------------------------------- - QHBoxLayout* load_files_layout = new QHBoxLayout(); - - progress_bar_ = new QProgressBar(this); - progress_bar_->setMaximum(100); - progress_bar_->setMinimum(0); - progress_bar_->hide(); - load_files_layout->addWidget(progress_bar_); - - btn_load_ = new QPushButton("&Load Files", this); - btn_load_->setMinimumWidth(180); - btn_load_->setMinimumHeight(40); - btn_load_->hide(); - load_files_layout->addWidget(btn_load_); - load_files_layout->setAlignment(btn_load_, Qt::AlignRight); - connect(btn_load_, SIGNAL(clicked()), this, SLOT(loadFilesClick())); - - // Next step instructions - next_label_ = new QLabel(this); - QFont next_label_font(QFont().defaultFamily(), 11, QFont::Bold); - next_label_->setFont(next_label_font); - next_label_->setText("Success! Use the left navigation pane to continue."); - next_label_->hide(); // only show once the files have been loaded. - - // Final Layout Setup --------------------------------------------- - // Alignment - layout->setAlignment(Qt::AlignTop); - hlayout->setAlignment(Qt::AlignTop); - left_layout->setAlignment(Qt::AlignTop); - right_layout->setAlignment(Qt::AlignTop); - - // Stretch - left_layout->setSpacing(10); - - // Attach Layouts - hlayout->addLayout(left_layout); - hlayout->addLayout(right_layout); - layout->addLayout(hlayout); - - // Vertical Spacer - layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding)); - - // Attach bottom layout - layout->addWidget(next_label_); - layout->setAlignment(next_label_, Qt::AlignRight); - layout->addLayout(load_files_layout); - - this->setLayout(layout); - - // Debug mode: auto load the configuration file by clicking button after a timeout - if (config_data_->debug_) - { - // select_mode_->btn_exist_->click(); - - QTimer* update_timer = new QTimer(this); - update_timer->setSingleShot(true); // only run once - connect(update_timer, SIGNAL(timeout()), btn_load_, SLOT(click())); - update_timer->start(100); - } -} - -// ****************************************************************************************** -// Destructor -// ****************************************************************************************** -StartScreenWidget::~StartScreenWidget() -{ - delete right_image_; // does not have a parent passed to it -} - -// ****************************************************************************************** -// Show options for creating a new configuration package -// ****************************************************************************************** -void StartScreenWidget::showNewOptions() -{ - // Do GUI stuff - select_mode_->btn_exist_->setChecked(false); - select_mode_->btn_new_->setChecked(true); - select_mode_->widget_instructions_->hide(); - urdf_file_->show(); - stack_path_->hide(); - btn_load_->show(); - - // Remember choice - create_new_package_ = true; -} - -// ****************************************************************************************** -// Show options for editing an existing configuration package -// ****************************************************************************************** -void StartScreenWidget::showExistingOptions() -{ - // Do GUI stuff - select_mode_->btn_exist_->setChecked(true); - select_mode_->btn_new_->setChecked(false); - select_mode_->widget_instructions_->hide(); - urdf_file_->hide(); - stack_path_->show(); - btn_load_->show(); - - // Remember choice - create_new_package_ = false; -} - -// ****************************************************************************************** -// Load files to parameter server - CLICK -// ****************************************************************************************** -void StartScreenWidget::loadFilesClick() -{ - // Disable start screen GUI components from being changed - urdf_file_->setDisabled(true); - // srdf_file_->setDisabled(true); - stack_path_->setDisabled(true); - select_mode_->setDisabled(true); - btn_load_->setDisabled(true); - progress_bar_->show(); - - bool result; - - // Decide if this is a new config package, or loading an old one - if (create_new_package_) - { - result = loadNewFiles(); - // Load 3d_sensors config file - load3DSensorsFile(); - } - else - { - result = loadExistingFiles(); - } - - // Check if there was a failure loading files - if (!result) - { - // Renable components - urdf_file_->setDisabled(false); - // srdf_file_->setDisabled(false); - stack_path_->setDisabled(false); - select_mode_->setDisabled(false); - btn_load_->setDisabled(false); - progress_bar_->hide(); - } - else - { - // Hide the logo image so that other screens can resize the rviz thing properly - right_image_label_->hide(); - } -} - -void StartScreenWidget::onPackagePathChanged(const QString& /*path*/) -{ - if (!loadPackageSettings(false)) - return; - // set xacro args from loaded settings - stack_path_->setArgs(QString::fromStdString(config_data_->xacro_args_)); -} - -void StartScreenWidget::onUrdfPathChanged(const QString& path) -{ - urdf_file_->setArgsEnabled(rdf_loader::RDFLoader::isXacroFile(path.toStdString())); -} - -bool StartScreenWidget::loadPackageSettings(bool show_warnings) -{ - // Get the package path - std::string package_path_input = stack_path_->getPath(); - // Check that input is provided - if (package_path_input.empty()) - { - if (show_warnings) - QMessageBox::warning(this, "Error Loading Files", "Please specify a configuration package path to load."); - return false; - } - - // check that the folder exists - if (!config_data_->setPackagePath(package_path_input)) - { - if (show_warnings) - QMessageBox::critical(this, "Error Loading Files", "The specified path is not a directory or is not accessible"); - return false; - } - - std::string setup_assistant_path; - - // Check if the old package is a setup assistant package. If it is not, quit - if (!config_data_->getSetupAssistantYAMLPath(setup_assistant_path)) - { - if (show_warnings) - QMessageBox::warning(this, "Incorrect Directory/Package", - QString( - "The chosen package location exists but was not created using MoveIt Setup Assistant. " - "If this is a mistake, provide the missing file: ") - .append(setup_assistant_path.c_str())); - return false; - } - - // Get setup assistant data - if (!config_data_->inputSetupAssistantYAML(setup_assistant_path)) - { - if (show_warnings) - QMessageBox::warning(this, "Setup Assistant File Error", - QString("Unable to correctly parse the setup assistant configuration file: ") - .append(setup_assistant_path.c_str())); - return false; - } - return true; -} - -// ****************************************************************************************** -// Load existing package files -// ****************************************************************************************** -bool StartScreenWidget::loadExistingFiles() -{ - // Progress Indicator - progress_bar_->setValue(10); - QApplication::processEvents(); - - if (!loadPackageSettings(true)) - return false; - - // Progress Indicator - progress_bar_->setValue(30); - QApplication::processEvents(); - - // Get the URDF path using the loaded .setup_assistant data and check it - if (!createFullURDFPath()) - return false; // error occurred - - // use xacro args from GUI - config_data_->xacro_args_ = stack_path_->getArgs().toStdString(); - - // Load the URDF - if (!loadURDFFile(config_data_->urdf_path_, config_data_->xacro_args_)) - return false; // error occurred - - // Get the SRDF path using the loaded .setup_assistant data and check it - if (!createFullSRDFPath(config_data_->config_pkg_path_)) - return false; // error occurred - - // Progress Indicator - progress_bar_->setValue(50); - QApplication::processEvents(); - - // Load the SRDF - if (!loadSRDFFile(config_data_->srdf_path_, config_data_->xacro_args_)) - return false; // error occurred - - // Progress Indicator - progress_bar_->setValue(60); - QApplication::processEvents(); - - // 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()); - } - - // Load 3d_sensors config file - load3DSensorsFile(); - - // Load ros_controllers.yaml file if available----------------------------------------------- - fs::path ros_controllers_yaml_path = config_data_->config_pkg_path_; - ros_controllers_yaml_path /= "config/ros_controllers.yaml"; - config_data_->inputROSControllersYAML(ros_controllers_yaml_path.make_preferred().string()); - - fs::path ompl_yaml_path = config_data_->config_pkg_path_; - ompl_yaml_path /= "config/ompl_planning.yaml"; - config_data_->inputOMPLYAML(ompl_yaml_path.make_preferred().string()); - - // DONE LOADING -------------------------------------------------------------------------- - - // Call a function that enables navigation - Q_EMIT readyToProgress(); - - // Progress Indicator - progress_bar_->setValue(70); - QApplication::processEvents(); - - // Load Rviz - Q_EMIT loadRviz(); - - // Progress Indicator - progress_bar_->setValue(100); - QApplication::processEvents(); - - next_label_->show(); // only show once the files have been loaded - - ROS_INFO("Loading Setup Assistant Complete"); - return true; // success! -} - -// ****************************************************************************************** -// Load chosen files for creating new package -// ****************************************************************************************** -bool StartScreenWidget::loadNewFiles() -{ - // Get URDF file path - config_data_->urdf_path_ = urdf_file_->getPath(); - - // Check that box is filled out - if (config_data_->urdf_path_.empty()) - { - QMessageBox::warning(this, "Error Loading Files", "No robot model file specified"); - return false; - } - - // Check that this file exits - if (!fs::is_regular_file(config_data_->urdf_path_)) - { - QMessageBox::warning(this, "Error Loading Files", - QString("Unable to locate the URDF file: ").append(config_data_->urdf_path_.c_str())); - return false; - } - - // Attempt to get the ROS package from the path - if (!extractPackageNameFromPath()) - { - return false; // An error occurred - } - - // Progress Indicator - progress_bar_->setValue(20); - QApplication::processEvents(); - - // use xacro args from GUI - config_data_->xacro_args_ = urdf_file_->getArgs().toStdString(); - - // Load the URDF to the parameter server and check that it is correct format - if (!loadURDFFile(config_data_->urdf_path_, config_data_->xacro_args_)) - return false; // error occurred - - // Progress Indicator - progress_bar_->setValue(50); - QApplication::processEvents(); - - // Create blank SRDF file - const std::string blank_srdf = ""; - - // Load a blank SRDF file to the parameter server - if (!setSRDFFile(blank_srdf)) - { - QMessageBox::warning(this, "Error Loading Files", "Failure loading blank SRDF file."); - return false; - } - - // Progress Indicator - progress_bar_->setValue(60); - QApplication::processEvents(); - - // DONE LOADING -------------------------------------------------------------------------- - - // Call a function that enables navigation - Q_EMIT readyToProgress(); - - // Progress Indicator - progress_bar_->setValue(70); - QApplication::processEvents(); - - // Load Rviz - Q_EMIT loadRviz(); - - // Progress Indicator - progress_bar_->setValue(100); - QApplication::processEvents(); - - next_label_->show(); // only show once the files have been loaded - - ROS_INFO("Loading Setup Assistant Complete"); - return true; // success! -} - -// ****************************************************************************************** -// Load URDF File to Parameter Server -// ****************************************************************************************** -bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args) -{ - if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, { xacro_args })) - { - QMessageBox::warning(this, "Error Loading Files", - QString("URDF/COLLADA file not found: ").append(urdf_file_path.c_str())); - return false; - } - - if (config_data_->urdf_string_.empty() && rdf_loader::RDFLoader::isXacroFile(urdf_file_path)) - { - QMessageBox::warning(this, "Error Loading Files", "Running xacro failed.\nPlease check console for errors."); - return false; - } - - // Verify that file is in correct format / not an XACRO by loading into robot model - if (!config_data_->urdf_model_->initString(config_data_->urdf_string_)) - { - QMessageBox::warning(this, "Error Loading Files", "URDF/COLLADA file is not a valid robot model."); - return false; - } - config_data_->urdf_from_xacro_ = rdf_loader::RDFLoader::isXacroFile(urdf_file_path); - - ROS_INFO_STREAM("Loaded " << config_data_->urdf_model_->getName() << " robot model."); - - // Load the robot model to the parameter server - ros::NodeHandle nh; - int steps = 0; - while (!nh.ok() && steps < 4) - { - ROS_WARN("Waiting for node handle"); - ros::Duration(1).sleep(); - steps++; - ros::spinOnce(); - } - - ROS_INFO("Setting Param Server with Robot Description"); - // ROS_WARN("Ignore the following error message 'Failed to contact master'. This is a known issue."); - nh.setParam("/robot_description", config_data_->urdf_string_); - - return true; -} - -// ****************************************************************************************** -// Load SRDF File to Parameter Server -// ****************************************************************************************** -bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args) -{ - std::string srdf_string; - if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, { xacro_args })) - { - QMessageBox::warning(this, "Error Loading Files", QString("SRDF file not found: ").append(srdf_file_path.c_str())); - return false; - } - - // Put on param server - return setSRDFFile(srdf_string); -} - -// ****************************************************************************************** -// Put SRDF File on Parameter Server -// ****************************************************************************************** -bool StartScreenWidget::setSRDFFile(const std::string& srdf_string) -{ - // Verify that file is in correct format / not an XACRO by loading into robot model - if (!config_data_->srdf_->initString(*config_data_->urdf_model_, srdf_string)) - { - QMessageBox::warning(this, "Error Loading Files", "SRDF file not a valid semantic robot description model."); - return false; - } - ROS_INFO_STREAM("Robot semantic model successfully loaded."); - - // Load to param server - ros::NodeHandle nh; - int steps = 0; - while (!nh.ok() && steps < 4) - { - ROS_WARN("Waiting for node handle"); - ros::Duration(1).sleep(); - steps++; - ros::spinOnce(); - } - - ROS_INFO("Setting Param Server with Robot Semantic Description"); - nh.setParam("/robot_description_semantic", srdf_string); - - return true; -} - -// ****************************************************************************************** -// Extract the package/stack name and relative path to urdf from an absolute path name -// Input: cofig_data_->urdf_path_ -// Output: cofig_data_->urdf_pkg_name_ -// cofig_data_->urdf_pkg_relative_path_ -// ****************************************************************************************** -bool StartScreenWidget::extractPackageNameFromPath() -{ - std::string relative_path; // holds the path after the sub_path - std::string package_name; // result - - bool package_found = config_data_->extractPackageNameFromPath(config_data_->urdf_path_, package_name, relative_path); - - // Assign data to moveit_config_data - if (!package_found) - { - // No package name found, we must be outside ROS - config_data_->urdf_pkg_name_ = ""; - config_data_->urdf_pkg_relative_path_ = config_data_->urdf_path_; // just the absolute path - } - else - { - // Check that ROS can find the package - const std::string robot_desc_pkg_path = ros::package::getPath(package_name); - - if (robot_desc_pkg_path.empty()) - { - QMessageBox::warning(this, "Package Not Found In ROS Workspace", - QString("ROS was unable to find the package name '") - .append(package_name.c_str()) - .append("' within the ROS workspace. This may cause issues later.")); - } - - // Success - config_data_->urdf_pkg_name_ = package_name; - config_data_->urdf_pkg_relative_path_ = relative_path; - } - - ROS_DEBUG_STREAM("URDF Package Name: " << config_data_->urdf_pkg_name_); - ROS_DEBUG_STREAM("URDF Package Path: " << config_data_->urdf_pkg_relative_path_); - - return true; // success -} - -// ****************************************************************************************** -// Make the full URDF path using the loaded .setup_assistant data -// ****************************************************************************************** -bool StartScreenWidget::createFullURDFPath() -{ - if (!config_data_->createFullURDFPath()) - { - if (config_data_->urdf_path_.empty()) // no path could be resolved - { - QMessageBox::warning(this, "Error Loading Files", - QString("ROS was unable to find the package name '") - .append(config_data_->urdf_pkg_name_.c_str()) - .append("'. Verify this package is inside your ROS " - "workspace and is a proper ROS package.")); - } - else - { - QMessageBox::warning(this, "Error Loading Files", - QString("Unable to locate the URDF file in package. Expected File: \n") - .append(config_data_->urdf_path_.c_str())); - } - return false; - } - - // Check if a package name was provided - if (config_data_->urdf_pkg_name_.empty()) - { - ROS_WARN("The URDF path is absolute to the filesystem and not relative to a ROS package/stack"); - } - - return true; // success -} - -// ****************************************************************************************** -// Make the full SRDF path using the loaded .setup_assistant data -// ****************************************************************************************** -bool StartScreenWidget::createFullSRDFPath(const std::string& package_path) -{ - if (!config_data_->createFullSRDFPath(package_path)) - { - QMessageBox::warning(this, "Error Loading Files", - QString("Unable to locate the SRDF file: ").append(config_data_->srdf_path_.c_str())); - return false; - } - - return true; // success -} - -// ****************************************************************************************** -// Loads sensors_3d yaml file -// ****************************************************************************************** -bool StartScreenWidget::load3DSensorsFile() -{ - // Loads parameters values from sensors_3d yaml file if available - fs::path sensors_3d_yaml_path = config_data_->config_pkg_path_; - sensors_3d_yaml_path /= "config/sensors_3d.yaml"; - - if (fs::is_regular_file(sensors_3d_yaml_path)) - config_data_->input3DSensorsYAML(sensors_3d_yaml_path.make_preferred().string()); - return true; -} - -// ****************************************************************************************** -// ****************************************************************************************** -// Class for selecting which mode -// ****************************************************************************************** -// ****************************************************************************************** - -// ****************************************************************************************** -// Create the widget -// ****************************************************************************************** -SelectModeWidget::SelectModeWidget(QWidget* parent) : QFrame(parent) -{ - // Set frame graphics - setFrameShape(QFrame::StyledPanel); - setFrameShadow(QFrame::Raised); - setLineWidth(1); - setMidLineWidth(0); - - // Basic widget container - QVBoxLayout* layout = new QVBoxLayout(this); - - // Horizontal layout splitter - QHBoxLayout* hlayout = new QHBoxLayout(); - - // Widget Title - QLabel* widget_title = new QLabel(this); - widget_title->setText("Create new or edit existing?"); - QFont widget_title_font(QFont().defaultFamily(), 12, QFont::Bold); - widget_title->setFont(widget_title_font); - layout->addWidget(widget_title); - layout->setAlignment(widget_title, Qt::AlignTop); - - // Widget Instructions - widget_instructions_ = new QLabel(this); - widget_instructions_->setAlignment(Qt::AlignLeft | Qt::AlignTop); - widget_instructions_->setWordWrap(true); - widget_instructions_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - widget_instructions_->setText( - "All settings for MoveIt are stored in the MoveIt configuration package. Here you have " - "the option to create a new configuration package or load an existing one. Note: " - "changes to a MoveIt configuration package outside this Setup Assistant are likely to be " - "overwritten by this tool."); - - layout->addWidget(widget_instructions_); - layout->setAlignment(widget_instructions_, Qt::AlignTop); - - // New Button - btn_new_ = new QPushButton(this); - btn_new_->setText("Create &New MoveIt\nConfiguration Package"); - hlayout->addWidget(btn_new_); - - // Exist Button - btn_exist_ = new QPushButton(this); - btn_exist_->setText("&Edit Existing MoveIt\nConfiguration Package"); - btn_exist_->setCheckable(true); - hlayout->addWidget(btn_exist_); - - // Add horizontal layer to vertical layer - layout->addLayout(hlayout); - setLayout(layout); - btn_new_->setCheckable(true); -} - -} // namespace moveit_setup_assistant diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/CMakeLists.txt b/moveit_setup_assistant/templates/moveit_config_pkg_template/CMakeLists.txt deleted file mode 100644 index e0c28ee223..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -cmake_minimum_required(VERSION 3.1.3) -project([GENERATED_PACKAGE_NAME]) - -find_package(catkin REQUIRED) - -catkin_package() - -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "setup_assistant.launch" EXCLUDE) -install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml deleted file mode 100644 index eb9c91225b..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml +++ /dev/null @@ -1,18 +0,0 @@ -planning_time_limit: 10.0 -max_iterations: 200 -max_iterations_after_collision_free: 5 -smoothness_cost_weight: 0.1 -obstacle_cost_weight: 1.0 -learning_rate: 0.01 -smoothness_cost_velocity: 0.0 -smoothness_cost_acceleration: 1.0 -smoothness_cost_jerk: 0.0 -ridge_factor: 0.0 -use_pseudo_inverse: false -pseudo_inverse_ridge_factor: 1e-4 -joint_update_limit: 0.1 -collision_clearance: 0.2 -collision_threshold: 0.07 -use_stochastic_descent: true -enable_failure_recovery: false -max_recovery_attempts: 5 diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml deleted file mode 100644 index 3b47012050..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/default_warehouse_db.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/default_warehouse_db.launch deleted file mode 100644 index ee1b6d34fe..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/default_warehouse_db.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch deleted file mode 100644 index 837ffb93f7..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - -[VIRTUAL_JOINT_BROADCASTER] - - - - - - [move_group/fake_controller_joint_states] - - - - [move_group/fake_controller_joint_states] - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch deleted file mode 100644 index 7c4ad14872..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/edit_configuration_package.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch deleted file mode 100644 index 3e27d9b9e9..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/move_group.launch +++ /dev/null @@ -1,101 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz deleted file mode 100644 index e0e7d2da6a..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit.rviz +++ /dev/null @@ -1,131 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 84 - Name: Displays - Property Tree Widget: - Expanded: - - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 226 - - Class: rviz/Help - Name: Help - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Name: MotionPlanning - Planned Path: - Links: ~ - Loop Animation: true - Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trajectory Topic: display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 - Joint Violation Color: 255; 0; 255 - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 1 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: ~ - Robot Alpha: 0.5 - Show Scene Robot: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: [PLANNING_FRAME] - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.0 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: -0.1 - Y: 0.25 - Z: 0.30 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.5 - Target Frame: [PLANNING_FRAME] - Yaw: -0.6232355833053589 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 848 - Help: - collapsed: false - Hide Left Dock: false - Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Views: - collapsed: false - Width: 1291 - X: 454 - Y: 25 diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch deleted file mode 100644 index a4605c0374..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_sensor_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698d7b..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml deleted file mode 100644 index afbdf88d25..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl_planning_pipeline.launch.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml deleted file mode 100644 index c7c4cf5005..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch deleted file mode 100644 index 635179b3b8..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - -[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK] - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml deleted file mode 100644 index 4b4d0d663a..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_pipeline.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_control_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_control_moveit_controller_manager.launch.xml deleted file mode 100644 index 9ebc91c108..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_control_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_controllers.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_controllers.launch deleted file mode 100644 index 77e6addd7e..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/sensor_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/sensor_manager.launch.xml deleted file mode 100644 index b0683aac02..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/sensor_manager.launch.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml deleted file mode 100644 index f05ca303f4..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml deleted file mode 100644 index c1de09f874..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml deleted file mode 100644 index 20c3dfc459..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajectory_execution.launch.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml deleted file mode 100644 index 0df0ff6f2b..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/trajopt_planning_pipeline.launch.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/warehouse.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/warehouse.launch deleted file mode 100644 index 0712e670f2..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/warehouse.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/warehouse_settings.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/warehouse_settings.launch.xml deleted file mode 100644 index e473b083bc..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/warehouse_settings.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template deleted file mode 100644 index 5b1004df52..0000000000 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template +++ /dev/null @@ -1,39 +0,0 @@ - - - [GENERATED_PACKAGE_NAME] - 0.3.0 - - An automatically generated package with all the configuration and launch files for using the [ROBOT_NAME] with the MoveIt Motion Planning Framework - - [AUTHOR_NAME] - [AUTHOR_NAME] - - BSD - - http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 - - catkin - - moveit_ros_move_group - moveit_kinematics - moveit_planners_ompl - moveit_ros_visualization - moveit_setup_assistant - moveit_simple_controller_manager - joint_state_publisher - joint_state_publisher_gui - robot_state_publisher - rviz - tf2_ros - xacro - - - - - -[OTHER_DEPENDENCIES] - - diff --git a/moveit_setup_assistant/unported_templates/README.md b/moveit_setup_assistant/unported_templates/README.md new file mode 100644 index 0000000000..8bb545d5d8 --- /dev/null +++ b/moveit_setup_assistant/unported_templates/README.md @@ -0,0 +1,5 @@ + * `gdb_settings.gdb` + * `joystick_control.launch` - Control the Rviz Motion Planning Plugin with a joystick + * `ompl-chomp_planning_pipeline.launch.xml` - + * `run_benchmark_ompl.launch` - Launch file for benchmarking OMPL planners + * `run_benchmark_trajopt.launch` diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gdb_settings.gdb b/moveit_setup_assistant/unported_templates/gdb_settings.gdb similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/launch/gdb_settings.gdb rename to moveit_setup_assistant/unported_templates/gdb_settings.gdb diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/joystick_control.launch b/moveit_setup_assistant/unported_templates/joystick_control.launch similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/launch/joystick_control.launch rename to moveit_setup_assistant/unported_templates/joystick_control.launch diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml rename to moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch b/moveit_setup_assistant/unported_templates/run_benchmark_ompl.launch similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch rename to moveit_setup_assistant/unported_templates/run_benchmark_ompl.launch diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch b/moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch similarity index 100% rename from moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_trajopt.launch rename to moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch