Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MoveIt Setup Assistant - Merge the Feature branch #1254

Merged
merged 30 commits into from
Jun 27, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
30 commits
Select commit Hold shift + click to select a range
1c948ca
Move Files Around (split into multiple packages, change to hpp extens…
DLu May 19, 2022
65d92e9
[MSA] Initial Refactor
DLu May 19, 2022
55d08ec
[MSA] Navigation Widget Fixes (#1054)
DLu Feb 2, 2022
5e59a29
[MSA] SRDF Setup (#1057)
DLu Mar 2, 2022
c53935b
[MSA] Upgrade templates to ROS 2 (#1101)
DLu Mar 10, 2022
5498b4f
[MSA] Merge Upstream into `feature/msa` (#1119)
DLu Mar 14, 2022
fbbd990
[MSA] Simplify loading of new SRDF (#1102)
DLu Mar 15, 2022
93d4088
[MSA] Fix arg parsing in main (#1110)
DLu Mar 15, 2022
a4134fd
[MSA] Add Setup Step for Generating Launch Files (#1129)
DLu Mar 24, 2022
36d92eb
[MSA] Three small edits (#1203)
DLu May 2, 2022
4982967
[MSA] Fix for long TLDs (#1214)
DLu May 9, 2022
2421efc
[MSA] Existing Package Loading Tweaks (#1212)
DLu May 9, 2022
d1f71f6
[MSA] Generate More New Launch Files (#1213)
DLu May 10, 2022
ba1ddc7
[MSA] Merge main into feature/msa (Part II) (#1240)
DLu May 12, 2022
89113bc
[MSA] Generate joint_limits.yaml and cartesian_limits.yaml (#1245)
DLu May 13, 2022
a2b5204
[MSA] Fix loading from previous config (#1246)
DLu May 13, 2022
472e66a
[MSA] Launch Updates (#1247)
DLu May 13, 2022
4159ea0
[MSA] Merge main into feature/msa (Part III) (#1249)
DLu May 16, 2022
091fd13
[MSA] Initial Controllers and Simulation Steps Port (#1252)
DLu May 16, 2022
c567498
[MSA] Migration Cleanup (#1253)
DLu May 16, 2022
488a8c3
PR Feedback
DLu May 19, 2022
56a5a8d
[MSA] Workaround to launch files without controllers (#1275)
May 27, 2022
4008b2c
clang-format fixes
Jun 14, 2022
ab84da9
[MSA] ros2_control Integration (#1299)
DLu Jun 14, 2022
fcfe561
[MSA] Clean up extra parentheses (#1366)
DLu Jun 16, 2022
640af1e
Merge remote-tracking branch 'upstream/main' into feature/msa
DLu Jun 16, 2022
bd49e18
Fix compile issues
Jun 16, 2022
2d4aadc
[MSA] Fix SRDF Initialization Bug / Copy Paste Error (#1381)
DLu Jun 21, 2022
6ab2036
[MSA] One XML Parser (#1382)
DLu Jun 21, 2022
2ee96e6
Merge remote-tracking branch 'origin/main' into feature/msa
Jun 27, 2022
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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"])
)
Expand Down Expand Up @@ -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
Expand All @@ -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(
Expand Down
144 changes: 0 additions & 144 deletions moveit_setup_assistant/CMakeLists.txt

This file was deleted.

135 changes: 134 additions & 1 deletion moveit_setup_assistant/README.md
Original file line number Diff line number Diff line change
@@ -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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We can remove this once #1387 is merged


## 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 <path_to_urdf/xacro> --xacro-args <optional_xacro_args> --srdf <path_to_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<URDFConfig>("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,

<name>[GENERATED_PACKAGE_NAME]</name>

becomes

<name>r2d2_moveit_config</name>
Binary file not shown.
Binary file not shown.
Binary file added moveit_setup_assistant/graphics/Wizard.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading