From 76454ea7573d08845f56ddbb16f81da7265f4a46 Mon Sep 17 00:00:00 2001 From: Johnson Sun Date: Thu, 5 Sep 2024 15:58:30 +0800 Subject: [PATCH] feat(aloha_ws): Clone `moveit_visual_tools` Branch: ros2 Reference: https://github.com/moveit/moveit_visual_tools/tree/a15df241092620f4d29286472427bff9ce9a032e --- .../src/moveit_visual_tools/.ament_flake8.ini | 6 + .../src/moveit_visual_tools/.clang-format | 75 + aloha_ws/src/moveit_visual_tools/.clang-tidy | 48 + .../.github/workflows/build_and_test.yaml | 74 + .../.github/workflows/format.yaml | 21 + .../.pre-commit-config.yaml | 48 + .../src/moveit_visual_tools/CHANGELOG.rst | 406 ++++ .../src/moveit_visual_tools/CMakeLists.txt | 103 + .../src/moveit_visual_tools/CONTRIBUTING.md | 3 + aloha_ws/src/moveit_visual_tools/LICENSE | 25 + aloha_ws/src/moveit_visual_tools/MIGRATION.md | 7 + aloha_ws/src/moveit_visual_tools/README.md | 139 ++ .../imarker_end_effector.h | 178 ++ .../moveit_visual_tools/imarker_robot_state.h | 178 ++ .../moveit_visual_tools/moveit_visual_tools.h | 753 ++++++++ .../src/moveit_visual_tools/launch/demo.rviz | 251 +++ .../launch/demo_rviz.launch.py | 97 + aloha_ws/src/moveit_visual_tools/mainpage.dox | 14 + .../moveit_visual_tools.repos | 9 + aloha_ws/src/moveit_visual_tools/package.xml | 39 + .../moveit_visual_tools/resources/demo.png | Bin 0 -> 43411 bytes .../resources/demo_mesh.stl | Bin 0 -> 52684 bytes .../moveit_visual_tools/resources/rrbot.srdf | 10 + .../moveit_visual_tools/resources/rrbot.urdf | 125 ++ .../resources/screenshot.png | Bin 0 -> 65395 bytes .../src/imarker_end_effector.cpp | 329 ++++ .../src/imarker_robot_state.cpp | 391 ++++ .../src/moveit_visual_tools.cpp | 1702 +++++++++++++++++ .../src/moveit_visual_tools_demo.cpp | 330 ++++ 29 files changed, 5361 insertions(+) create mode 100644 aloha_ws/src/moveit_visual_tools/.ament_flake8.ini create mode 100644 aloha_ws/src/moveit_visual_tools/.clang-format create mode 100644 aloha_ws/src/moveit_visual_tools/.clang-tidy create mode 100644 aloha_ws/src/moveit_visual_tools/.github/workflows/build_and_test.yaml create mode 100644 aloha_ws/src/moveit_visual_tools/.github/workflows/format.yaml create mode 100644 aloha_ws/src/moveit_visual_tools/.pre-commit-config.yaml create mode 100644 aloha_ws/src/moveit_visual_tools/CHANGELOG.rst create mode 100644 aloha_ws/src/moveit_visual_tools/CMakeLists.txt create mode 100644 aloha_ws/src/moveit_visual_tools/CONTRIBUTING.md create mode 100644 aloha_ws/src/moveit_visual_tools/LICENSE create mode 100644 aloha_ws/src/moveit_visual_tools/MIGRATION.md create mode 100644 aloha_ws/src/moveit_visual_tools/README.md create mode 100644 aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/imarker_end_effector.h create mode 100644 aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/imarker_robot_state.h create mode 100644 aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/moveit_visual_tools.h create mode 100644 aloha_ws/src/moveit_visual_tools/launch/demo.rviz create mode 100644 aloha_ws/src/moveit_visual_tools/launch/demo_rviz.launch.py create mode 100644 aloha_ws/src/moveit_visual_tools/mainpage.dox create mode 100644 aloha_ws/src/moveit_visual_tools/moveit_visual_tools.repos create mode 100644 aloha_ws/src/moveit_visual_tools/package.xml create mode 100644 aloha_ws/src/moveit_visual_tools/resources/demo.png create mode 100644 aloha_ws/src/moveit_visual_tools/resources/demo_mesh.stl create mode 100644 aloha_ws/src/moveit_visual_tools/resources/rrbot.srdf create mode 100644 aloha_ws/src/moveit_visual_tools/resources/rrbot.urdf create mode 100644 aloha_ws/src/moveit_visual_tools/resources/screenshot.png create mode 100644 aloha_ws/src/moveit_visual_tools/src/imarker_end_effector.cpp create mode 100644 aloha_ws/src/moveit_visual_tools/src/imarker_robot_state.cpp create mode 100644 aloha_ws/src/moveit_visual_tools/src/moveit_visual_tools.cpp create mode 100644 aloha_ws/src/moveit_visual_tools/src/moveit_visual_tools_demo.cpp diff --git a/aloha_ws/src/moveit_visual_tools/.ament_flake8.ini b/aloha_ws/src/moveit_visual_tools/.ament_flake8.ini new file mode 100644 index 00000000..2eafac0d --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/.ament_flake8.ini @@ -0,0 +1,6 @@ +[flake8] +extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,Q000 +import-order-style = google +max-line-length = 99 +show-source = true +statistics = true diff --git a/aloha_ws/src/moveit_visual_tools/.clang-format b/aloha_ws/src/moveit_visual_tools/.clang-format new file mode 100644 index 00000000..9a6ec509 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/.clang-format @@ -0,0 +1,75 @@ +--- +BasedOnStyle: Google +ColumnLimit: 120 +MaxEmptyLinesToKeep: 1 +SortIncludes: false + +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +NamespaceIndentation: None +ContinuationIndentWidth: 4 +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false + +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true + +AllowAllParametersOfDeclarationOnNextLine: false +ExperimentalAutoDetectBinPacking: false +ObjCSpaceBeforeProtocolList: true +Cpp11BracedListStyle: false + +AllowShortBlocksOnASingleLine: true +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortCaseLabelsOnASingleLine: false + +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true + +BinPackParameters: true +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true + +PenaltyExcessCharacter: 50 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 1000 +PenaltyBreakFirstLessLess: 10 +PenaltyBreakString: 100 +PenaltyReturnTypeOnItsOwnLine: 50 + +SpacesBeforeTrailingComments: 2 +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterCStyleCast: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: + AfterCaseLabel: true + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false +... diff --git a/aloha_ws/src/moveit_visual_tools/.clang-tidy b/aloha_ws/src/moveit_visual_tools/.clang-tidy new file mode 100644 index 00000000..238bcfc8 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/.clang-tidy @@ -0,0 +1,48 @@ +--- +Checks: '-*, + performance-*, + llvm-namespace-comment, + modernize-redundant-void-arg, + modernize-use-nullptr, + modernize-use-default, + modernize-use-override, + readability-named-parameter, + readability-redundant-smartptr-get, + readability-redundant-string-cstr, + readability-simplify-boolean-expr, + readability-container-size-empty, + ' +HeaderFilterRegex: '' +AnalyzeTemporaryDtors: false +CheckOptions: + - key: llvm-namespace-comment.ShortNamespaceLines + value: '10' + - key: llvm-namespace-comment.SpacesBeforeComments + value: '2' + - key: readability-braces-around-statements.ShortStatementLines + value: '2' + # type names + - key: readability-identifier-naming.ClassCase + value: CamelCase + - key: readability-identifier-naming.EnumCase + value: CamelCase + - key: readability-identifier-naming.UnionCase + value: CamelCase + # method names + - key: readability-identifier-naming.MethodCase + value: camelBack + # variable names + - key: readability-identifier-naming.VariableCase + value: lower_case + - key: readability-identifier-naming.ClassMemberSuffix + value: '_' + # const static or global variables are UPPER_CASE + - key: readability-identifier-naming.EnumConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.StaticConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.ClassConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.GlobalVariableCase + value: UPPER_CASE +... diff --git a/aloha_ws/src/moveit_visual_tools/.github/workflows/build_and_test.yaml b/aloha_ws/src/moveit_visual_tools/.github/workflows/build_and_test.yaml new file mode 100644 index 00000000..ebb9f32e --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/.github/workflows/build_and_test.yaml @@ -0,0 +1,74 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: Build and Test + +on: + workflow_dispatch: + pull_request: + push: + branches: + - ros2 + +jobs: + industrial_ci: + strategy: + fail-fast: false + matrix: + env: + - IMAGE: humble-source + - IMAGE: rolling-source + CLANG_TIDY: true + + env: + DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }} + UPSTREAM_WORKSPACE: moveit_visual_tools.repos + AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src + AFTER_SETUP_UPSTREAM_WORKSPACE_EMBED: set +u && source ~/ws_moveit/install/setup.bash && set - u + CCACHE_DIR: ${{ github.workspace }}/.ccache + BASEDIR: ${{ github.workspace }}/.work + CACHE_PREFIX: ${{ matrix.env.IMAGE }} + CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} + + name: ${{ matrix.env.IMAGE }}${{ matrix.env.CLANG_TIDY && ' + clang-tidy' || '' }} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + - name: cache upstream_ws + uses: actions/cache@v4 + with: + save-always: true + path: ${{ env.BASEDIR }}/upstream_ws + key: upstream_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('moveit_visual_tools.repos') }}-${{ github.run_id }} + restore-keys: | + upstream_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('moveit_visual_tools.repos') }} + # The target directory cache doesn't include the source directory because + # that comes from the checkout. See "prepare target_ws for cache" task below + - name: cache target_ws + uses: actions/cache@v4 + with: + save-always: true + path: ${{ env.BASEDIR }}/target_ws + key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} + restore-keys: | + target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} + - name: cache ccache + uses: actions/cache@v4 + with: + save-always: true + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} + restore-keys: | + ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} + ccache-${{ env.CACHE_PREFIX }} + - name: industrial_ci + uses: ros-industrial/industrial_ci@master + env: ${{ matrix.env }} + - name: prepare target_ws for cache + run: | + du -sh ${{ env.BASEDIR }}/target_ws + sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete + sudo rm -rf ${{ env.BASEDIR }}/target_ws/src + du -sh ${{ env.BASEDIR }}/target_ws diff --git a/aloha_ws/src/moveit_visual_tools/.github/workflows/format.yaml b/aloha_ws/src/moveit_visual_tools/.github/workflows/format.yaml new file mode 100644 index 00000000..df7cda38 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/.github/workflows/format.yaml @@ -0,0 +1,21 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Formatting (pre-commit) + +on: + workflow_dispatch: + pull_request: + push: + branches: + - ros2 + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Install clang-format-14 + run: sudo apt-get install clang-format-14 + - uses: pre-commit/action@v3.0.1 diff --git a/aloha_ws/src/moveit_visual_tools/.pre-commit-config.yaml b/aloha_ws/src/moveit_visual_tools/.pre-commit-config.yaml new file mode 100644 index 00000000..d3e15475 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/.pre-commit-config.yaml @@ -0,0 +1,48 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.6.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-json + - id: check-merge-conflict + - id: check-symlinks + - id: check-toml + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: mixed-line-ending + - id: pretty-format-json + - id: trailing-whitespace + + - repo: https://github.com/psf/black + rev: 24.8.0 + hooks: + - id: black + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-14 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] diff --git a/aloha_ws/src/moveit_visual_tools/CHANGELOG.rst b/aloha_ws/src/moveit_visual_tools/CHANGELOG.rst new file mode 100644 index 00000000..ad2cb254 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/CHANGELOG.rst @@ -0,0 +1,406 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_visual_tools +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +4.1.0 (2022-05-09) +------------------ +* Update black version (`#117 `_) +* Jammy fixes and clang-format-12 (`#116 `_) +* Fixed deprecated call to PlanningSceneMonitor's constructor (`#108 `_) +* Remove Foxy CI builds (`#111 `_) +* Generate license using ament_copyright (`#105 `_) +* Contributors: Jafar Abdi, Stephanie Eng, Vatan Aksoy Tezer + +4.0.0 (2021-09-27) +------------------ +* Port to ros2 (`#92 `_) +* Fix typo in package.xml (`#87 `_) +* Contributors: Davide Faconti, Felix von Drigalski, Vatan Aksoy Tezer + +3.6.0 (2020-10-09) +------------------ +* [feature] Unified collision environment used (`#54 `_) +* [feature] publish cuboids with size (`#59 `_) +* [feature] hideRobot(): use new .hide member of DisplayRobotState msg (`#56 `_) +* [feature] Use first trajectory point as start state for visualization (`#49 `_) +* [feature] exposing the publishers to the user (`#48 `_) +* [feature] Highlight selected links in publishRobotState() + improved collision visualization (`#45 `_) +* [fix] Update to Noetic and fix various warnings (`#79 `_) +* [fix] Fix Eigen alignment (`#63 `_) +* [fix] missing end effector markers because clear (`#51 `_) +* [fix] Remove #attempts from setFromIK (`#43 `_) +* [documentation] Update README.md (`#69 `_) +* [documentation] update doc of hideRobot() (`#64 `_) +* [maint] add soname version (`#74 `_) +* [maint] Replace tf_conversions with tf2's toMsg / fromMsg (`#66 `_) +* [maint] fix clang-tidy issue (`#68 `_) +* [maint] Cleanup (`#65 `_) + * Replace robot_model and robot_state namespaces by moveit::core + * Fix clang-tidy issues + * Remove deprecated function +* [maint] Bump required cmake version (`#62 `_) +* [maint] moveit.rosinstall: use master branch for all deps (`#57 `_) +* [maint] drop melodic + kinetic support for master branch (`#58 `_) +* [maint] Fix Travis config + issues (`#47 `_) +* [maint] Change 'MoveIt!' to MoveIt +* Contributors: Bjar Ne, Dave Coleman, Henning Kayser, Jafar Abdi, Jens P, Mark Moll, Michael Görner, Mike Lautman, Robert Haschke, Tyler Weaver + +3.5.2 (2018-12-10) +------------------ +* Use LOGNAME for named logging (`#42 `_) +* Eigen::Affine3d -> Eigen::Isometry3d (`#39 `_) +* Contributors: Dave Coleman + +3.5.1 (2018-11-14) +------------------ +* Adding trigger call to animations when batch_publishing_enabled\_ is enabled (`#36 `_) +* Contributors: Mike Lautman + +3.5.0 (2018-09-05) +------------------ +* Add Markdown ROS Buildfarm badges for Melodic +* Fixing melodic branch with tf2 updates (`#34 `_) +* Additional visualization (`#31 `_) + * adding a visualization for publishing a box with width, height, and depth + * adding additional publishCollisionCuboid method overloads + * changing x,y,z to width, depth, height +* Fixup CMakeLists and package.xml (`#30 `_) +* Triggering UPDATE_GEOMETRY is sufficient, dont spinOnce + - triggering UPDATE_SCENE leads to sending a full planning scene including all meshes. For all use-cases in this lib UPDATE_GEOMETRY is sufficient and sends only diffs + - calling ros::spinOnce() in random places messes up the event queue (e.g. joint state updates being processed in gui thread instead of the async spinner thread) +* Fix API compatibility by providing a default empty list of end-effector joints +* Allow setting joint values for end-effector marker + currently end-effector markers are based on the default robot state (ie all zero), + this allows passing a vector of doubles for all active joint in the end-effector group + Passing a new set of joint values will invalidate the cache but for our use-case this is + neglectible and could be optimized later +* Moved boost::shared_ptr to std for tf2 +* Converted to use tf2 moveit interfaces +* Fix broken CI for Melodic +* Fix Continuous Integration +* Contributors: Dave Coleman, Ian McMahon, Mike Lautman, Simon Schmeisser + +3.4.0 (2017-12-27) +------------------ +* Apply current MoveIt clang-format +* Various improvements needed while finishing planning thesis +* Fix greater than/less than issue in clearance check +* Ability to specify clearance for random state +* Small threading fixes +* imarker: Fix setToRandomState() + imarker: Switch to std::makeshared +* Improve console output +* Contributors: Dave Coleman, Mike Lautman + +3.3.0 (2017-06-20) +------------------ +* Change error message to warning +* Make planning scene monitor publicly exposed +* Remove label from imarkers +* Ability to move a collision object without removing it first +* IMarkerRobotState: update imarkers location when setting robot state +* IMarkerRobotState: Added setRobotState() +* IMarkerRobotState: Renamed function publishRobotState() +* MoveItVisualTools: renamed variable to psm\_ +* Expose verbose collision checking +* Contributors: Dave Coleman + +3.2.1 (2016-11-02) +------------------ +* New publishTrajectoryPath() functions +* New publishTrajectoryLine() functions +* getRobotState() return by reference +* Trajectory path has smaller vertices +* IMarkerRobotState: added isStateValid() +* Contributors: Dave Coleman + +3.2.0 (2016-10-20) +------------------ +* Added publishState() to imarker_robot_state +* New publishTrajectoryLine() function that automatically chooses end effectors to visualize +* New collision table function that takes z input +* Fixed callbacks for multiple EEFs +* Allow for two end effectors +* Ability to use two end effectors for interactive markers +* Make ik solving at any end effector link, not just end of kinematic chain +* Better debugging for collision +* Only save when mouse up +* Fix API for changes in rviz_visual_tools +* Allow collision walls to have variable z location +* Make applyVirtualJointTransform() static +* Make checkForVirtualJoint() static +* IMarkerRobotState remove offset capability +* IMarkerRobotState remove imarker box control +* Switched travis to MoveIt CI +* Added new IMarker Robot control method +* Cleaned up code base: catkin lint, roslint +* Fixed bug in planning scene triggering +* Optimize planning scene updates to only update GEOMETRY +* Fix xacro +* Upgrade to Eigen3 per ROS Kinetic requirements +* New publishRobotState() function +* Fix Eigen bugs +* Removed deprecated code +* Converted to C++11 +* Optional blocking publisher calls +* Added getter for getRobotRootState() +* Contributors: Dave Coleman + +3.1.0 (2016-04-28) +------------------ +* Re-factored and fixed visual tools demo! +* Fixes for catkin lint +* Fixes for roslint +* Removed deprecated function call +* Remove deprecated test +* New root_robot_state utilization +* Ablity to move a RobotState's root frame permenatly around in the scene +* Better publishCollisionWall() function +* Deprecated old publishTrajectoryLine() functions - removed clear_all_markers argument +* New publishTrajectoryPath() variant +* Rename namespace of RobotState +* Made INFO into DEBUG output +* New publishTrajectoryLine function +* Switched publishTrajectoryLine to use cylinders instead of lines +* New showJointLimits() function for console debugging a robot state +* Fix publishTrajectoryPath() bug +* Default blocking time for trajectory if not parameterized +* Publish workspace parameters was incorrectly creating a collision object +* Contributors: Dave Coleman + +3.0.5 (2016-02-09) +------------------ +* Updated README +* Better comment +* Contributors: Dave Coleman + +3.0.4 (2016-01-12) +------------------ +* Removed stray debug output +* Improved debugging output for the hideRobot() feature and virtual_joints +* Contributors: Dave Coleman + +3.0.3 (2016-01-10) +------------------ +* Renamed test to demo +* New publishTrajectoryLine() function +* Fix travis +* Deprecated loadEEMarker() that uses string +* Formatted code +* Switched from MOVEIT deprecated to RVIZ_VISUAL_TOOLS deprecated +* Fixed shared_robot_state to initialize correctly every time +* Switched to using name\_ variables +* Add error checks to publishTrajectoryLine +* Added ability for publishTrajectoryLine to clear all previous markers +* Contributors: Dave Coleman + +3.0.2 (2015-12-27) +------------------ +* Updated README +* Temp fix missing variable +* Contributors: Dave Coleman + +3.0.1 (2015-12-05) +------------------ +* catkin lint cleanup +* Fix travis +* Contributors: Dave Coleman + +3.0.0 (2015-12-02) +------------------ +* Release 3.0 +* Added travis support +* fix the how to link a demo img +* Updated link to Doxygen API description +* Formatting and better debug output +* Fix hide robot bug +* Remove incompatible humanoid function +* Default color when publishing collision meshes +* Added error check for bad value +* API change for removal of shape_tools +* New publish trajectory line function +* Remove slash from topic name +* Removed mute functionality +* Improved loading efficiency +* publishContactPoints accepts a color +* Change topics to default when opening Rviz +* New publishCollisionMesh() function +* Changed publishCollisionMesh() API +* Renamed publishCollisionRectangle to publishCollisionCuboid() +* Updated rviz_visual_tools API +* New publishMesh from ROS msg function +* publishRobotState() for a RobotStateMsg now allows color +* publishTrajectoryPath() for a ROS msg now requires a RobotState +* New method for attaching collision objects that does not require a publisher +* Specify scene name and cleanup logging +* Fixed error checking for hideRobot() function +* loadTrajectoryPub() allows custom topic +* New publishTrajectoryPoints() function +* New publishContactPoints function +* New publishTrajectoryPath() function +* New getRobotModel() function +* New ability to visualize IK solutions with arbitrary virtual joint +* API Broken: ability to have different end effectors for different arms, auto EE marker loading +* Publish collision meshes +* Added check for virtual joint +* Fixed which arrow gets published +* Publish fixed link arrows to show footstep locations +* Ability to specify robot_state_topic without loading the publisher +* Contributors: Dave Coleman, Daiki Maekawa, simonschmeisser + +2.2.0 (2015-01-07) +------------------ +* Code cleanup +* Improved naming +* Joint model bug fix +* Improved speed of sending collision objects to Rviz + Added Manual planning scene update mode + Ability to apply colors to all collision objects (YAY) + API: removed removeAllCollisionObjectsPS function + Removed loadPlanningPub() function + Removed publishRemoveAllCollisionObjects() function +* Added backwards compatibile loadCollisionSceneFromFile() +* New publishCollisionRectangle function + API: Changed loadCollisionSceneFromFile() to accept a pose instead of x,y +* Fix for renamed function +* New publishWorkspaceParameters() function +* Added ability to publish robot states with color +* Fixed install method +* Merge pull request `#5 `_ from robomakery/feature/fix-collision-objects-test +* Fixes for missing declarations in collision_objects_test.cpp +* Refactored how collision ojects are published + Created new collision objects test and roslaunch file + Optimized header file + Removed loadCollisionPub() function + Fixed publishCollisionFloor + Added publishCollisionRectangle +* Contributors: Dave Coleman, Dylan Vaughn + +2.1.0 (2014-10-31) +------------------ +* Fix for upstream change of RvizVisualTools +* Set animation speed of grasps +* Fix publishing end effector +* New publishCollisionObjectMsg() function +* New getSharedRobotState() accessor function +* Consolidated publish marker functions +* Fixed loadEEMarker() to be called more than once +* Contributors: Dave Coleman + +2.0.0 (2014-10-27) +------------------ +* Updated README +* API Upgrade Notes +* Renamed to have 'MoveIt' prefix in class and file name, moved base functionality to rviz_visual_tools +* Added new publishSphere function and publish_sphere test script +* Created better test script +* Better static_id handling for publishText +* Added mainpage for API docs +* Enabled colors +* Improved integer random num generation +* New publishSpheres functions +* Contributors: Dave Coleman + +1.3.0 (2014-09-17) +------------------ +* Added new getRandColor() function +* Added TRANSLUCENT color +* Added two new publishSphere() functions +* New convertPointToPose function +* Reduced sleep timer for starting all publishers from 0.5 seconds to 0.2 seconds +* Removed stacktrace tool because already exists in moveit_core +* New publishText function that allows custom scale and id number be passed in +* Removed deprecated getEEParentLink() function +* Added new scale sizes +* Added new processCollisionObvMsg() +* Added new setPlanningSceneMonitor() +* Deprecated removeAllColisionObejcts() +* Created new removeAllCollisionObjectsPS() +* Added new publishCollisionFloor() +* Added new loadCollisionSceneFromFile() +* New color purple +* Added new setBaseFrame() function +* Contributors: Dave Coleman + +1.2.1 (2014-08-11) +------------------ +* Renamed base_link to base_frame +* Added new getBaseFrame() function +* Deprecated getBaseLink() function +* Contributors: Dave Coleman + +1.2.0 (2014-08-08) +------------------ +* Added XXLarge size +* Added global_scale feature +* Added hideRobot() functionality +* Added removeAllCollisionObjects from planning scene monitor +* Added publishCollisionSceneFromFile function +* Formatting +* Contributors: Dave Coleman + +1.1.0 (2014-07-31) +------------------ +* Bug fixes +* Fixed convertPoint32ToPose +* Added scale to publishText +* New publishPolygon, publishMarker, convertPose, convertPointToPose, and convertPoint32 functions +* New deleteAllMarkers, publishPath, publishSpheres, and convertPoseToPoint functions +* Added getCollisionWall +* Made lines darker +* Added reset marker feature +* Namespaces for publishSphere +* New publishTrajectory function +* Merging features from OMPL viewer +* Refactored functions, new robot_model intialization +* Added more rand functions and made them static +* Added graph_msgs generated messages dependence so it waits for it to be compiled +* Updated README +* Contributors: Dave Coleman, Sammy Pfeiffer + +1.0.1 (2014-05-30) +------------------ +* Updated README +* Indigo support +* Fix for strict cppcheck and g++ warnings/errors +* Compatibilty fix for Eigen package in ROS Indigo +* Fix uninitialized +* Fix functions with no return statement and other cppcheck errors +* Contributors: Bence Magyar, Dave Coleman, Jordi Pages + +1.0.0 (2014-05-05) +------------------ +* Enabled dual arm manipulation +* Removed notions of a global planning group, ee group name, or ee parent link. +* Changed functionality of loadEEMarker +* Added new print function +* Made getPlanningSceneMonitor() private function +* Renamed loadPathPub() +* Added tool for visualizing unmangled stack trace +* Created function for publishing non-animated grasps +* Created new publishGraph function. Renamed publishCollisionTree to publishCollisionGraph +* Created functions for loading publishers with a delay +* Removed old method of removing all collision objects +* Created better testing functionality +* Changed return type from void to bool for many functions +* Changed way trajectory is timed +* Created new publishIKSolutions() function for grasp poses, etc +* Added new MoveIt robot state functionality +* Added visualize grasp functionality +* Removed unnecessary run dependencies +* Updated README + +0.2.0 (2014-04-11) +------------------ +* Improved header comments are re-ordered functions into groups +* Started to create new trajectory point publisher +* Added getBaseLink function +* Added dependency on graph_msgs +* Added new collision cylinder functionality +* Created example code in README +* Renamed visualization to visual keyword +* Updated README + +0.1.0 (2014-04-04) +------------------ +* Split moveit_visual_tools from its original usage within block_grasp_generator package diff --git a/aloha_ws/src/moveit_visual_tools/CMakeLists.txt b/aloha_ws/src/moveit_visual_tools/CMakeLists.txt new file mode 100644 index 00000000..2e08aa65 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/CMakeLists.txt @@ -0,0 +1,103 @@ +cmake_minimum_required(VERSION 3.5) +project(moveit_visual_tools) + +find_package(moveit_common REQUIRED) +moveit_package() + +# Load all dependencies required for this package +find_package(Boost REQUIRED system) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(graph_msgs REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rviz_visual_tools REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + Boost + geometry_msgs + graph_msgs + moveit_core + moveit_ros_planning + rclcpp + rviz_visual_tools + std_msgs + tf2_eigen + tf2_ros + trajectory_msgs + visualization_msgs +) + +# Visualization Tools Library +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}.cpp + src/imarker_robot_state.cpp + src/imarker_end_effector.cpp +) + +target_include_directories(${PROJECT_NAME} + PUBLIC $ + PUBLIC $ +) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Demo executable +add_executable(${PROJECT_NAME}_demo + src/${PROJECT_NAME}_demo.cpp +) +target_link_libraries(${PROJECT_NAME}_demo + ${PROJECT_NAME} +) +ament_target_dependencies(${PROJECT_NAME}_demo ${THIS_PACKAGE_INCLUDE_DEPENDS}) + + +# Exports +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +############# +## Install ## +############# + +# Install libraries +install( + TARGETS ${PROJECT_NAME} + EXPORT + export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +# Install executables +install( + TARGETS ${PROJECT_NAME}_demo + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +# Install header files +install(DIRECTORY include/ DESTINATION include) + +# Install shared resources +install(DIRECTORY launch resources DESTINATION share/${PROJECT_NAME}) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_cppcheck_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_flake8_CONFIG_FILE ".ament_flake8.ini") + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/aloha_ws/src/moveit_visual_tools/CONTRIBUTING.md b/aloha_ws/src/moveit_visual_tools/CONTRIBUTING.md new file mode 100644 index 00000000..309be1e1 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the 3-Clause BSD License, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/aloha_ws/src/moveit_visual_tools/LICENSE b/aloha_ws/src/moveit_visual_tools/LICENSE new file mode 100644 index 00000000..574ef079 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/LICENSE @@ -0,0 +1,25 @@ +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 the copyright holder 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 HOLDER 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. diff --git a/aloha_ws/src/moveit_visual_tools/MIGRATION.md b/aloha_ws/src/moveit_visual_tools/MIGRATION.md new file mode 100644 index 00000000..ff4ff317 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/MIGRATION.md @@ -0,0 +1,7 @@ +# Migration Notes + +API changes in MoveIt Visual Tools releases + +## ROS Melodic + +- Affine3d no longer used, replaced by more computationally efficient Isometry3d. Simple find-replace should suffice diff --git a/aloha_ws/src/moveit_visual_tools/README.md b/aloha_ws/src/moveit_visual_tools/README.md new file mode 100644 index 00000000..555954d7 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/README.md @@ -0,0 +1,139 @@ +# MoveIt Visual Tools + +Helper functions for displaying and debugging MoveIt data in Rviz via published markers, trajectories, and MoveIt collision objects. It is sometimes hard to understand everything that is going on internally with MoveIt, but using these quick convenience functions allows one to easily visualize their code. This package is built in top of [rviz_visual_tools](https://github.com/PickNikRobotics/rviz_visual_tools) and all those features are included via class inheritance. + +This package helps you visualize: + + - Basic Rviz geometric shapes + - MoveIt collision objects + - MoveIt and ROS trajectories + - Robot states + - End effectors + - Interactive markers to move robot arms using IK from remote applications + + + +This open source project was developed at [PickNik Robotics](https://picknik.ai/). Need professional ROS development and consulting? Contact us at projects@picknik.ai for a free consultation. + +## Status: + +### ROS: +- [![Build and Test](https://github.com/ros-planning/moveit_visual_tools/actions/workflows/build_and_test.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit_visual_tools/actions/workflows/build_and_test.yaml?query=branch%3Amaster) [![Formatting](https://github.com/ros-planning/moveit_visual_tools/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit_visual_tools/actions/workflows/format.yaml?query=branch%3Amaster) Github Actions +- [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_visual_tools__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__moveit_visual_tools__ubuntu_focal__source/) ROS Buildfarm - AMD64 Focal Source Build - Ubuntu 20.04 LTS +- [![Build Status](http://build.ros.org/buildStatus/icon?job=Ndev__moveit_visual_tools__ubuntu_focal_amd64)](http://build.ros.org/job/Ndev__moveit_visual_tools__ubuntu_focal_amd64/) ROS Buildfarm - AMD64 Focal Devel Build - Ubuntu 20.04 LTS + +### ROS2: +- [![Build and Test](https://github.com/ros-planning/moveit_visual_tools/actions/workflows/build_and_test.yaml/badge.svg?branch=ros2)](https://github.com/ros-planning/moveit_visual_tools/actions/workflows/build_and_test.yaml?query=branch%3Aros2) GHA: Build and Test +- [![Formatting](https://github.com/ros-planning/moveit_visual_tools/actions/workflows/format.yaml/badge.svg?branch=ros2)](https://github.com/ros-planning/moveit_visual_tools/actions/workflows/format.yaml?query=branch%3Aros2) GHA: Formatting + +![](resources/screenshot.png) + +![](resources/demo.png) + +## ROS2 Install + +### Install From Source + + +Install moveit2 following the instructions [here](https://moveit.ros.org/install-moveit2/source/). After sourcing the moveit workspace clone this repository into a colcon workspace, import and install dependencies and build: + + source ~/ws_moveit/install/setup.bash + cd $COLCON_WS/src + git clone -b ros2 https://github.com/ros-planning/moveit_visual_tools + vcs import < moveit_visual_tools/moveit_visual_tools.repos + rosdep install -r --from-paths . --ignore-src --rosdistro foxy -y + cd $COLCON_WS + colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release + +## Quick Start Demo + +To run some demos displaying robot states and collision objects: + + ros2 launch moveit_visual_tools demo_rviz.launch.py + +## Code API + +See [VisualTools Class Reference](http://docs.ros.org/kinetic/api/moveit_visual_tools/html/classmoveit__visual__tools_1_1MoveItVisualTools.html) + +## Usage + +We'll assume you will be using these helper functions within a class. + +### Initialize + +Add to your includes: +``` +#include +``` + +Add to your class's member variables: +``` +// For visualizing things in rviz +moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; +``` + +In your class' constructor add: +``` +visual_tools_ = std::make_shared(node_, "world", "/moveit_visual_tools"); +``` + +### Collision Object Functions + +Helpers for adding and removing objects from the MoveIt planning scene. CO stands for Collision Object and ACO stands for Active Collision Object. + + - cleanupCO + - cleanupACO + - attachCO + - publishCollisionBlock + - publishCollisionCylinder + - publishCollisionTree + - publishCollisionTable + - publishCollisionWall + +And more... + +### Animate Trajectories + +Higher level robot and trajectory functions + + - publishTrajectoryPath + - publishTrajectoryPoint + - publishRobotState + - publishAnimatedGrasps + - publishIKSolutions + +## Show parts of a robot + +These functions are a little more complicated + + - publishEEMarkers + +## Parent Class + +This class is built on top of [rviz_visual_tools](https://github.com/PickNikRobotics/rviz_visual_tools) so all features and documentation for that package apply here as well. + +## Developers Notes + +Useful notes for anyone wanting to dig in deeper: + + - All poses are published with respect to the world frame e.g. /world, /odom, or maybe /base + - All publish() ROS topics should be followed by a ``rclcpp::spin_some(node);`` but no sleep + - Do not want to load any features/publishers until they are actually needed since this library contains so many components + +## Linting (pre-commit) + +pre-commit is a tool that is used in moveit2_tutorials to check and apply style guidelines automatically. To install pre-commit into your system: + + pip3 install pre-commit + +Then under moveit2_tutorials directory install the git hooks like this: + + cd $COLCON_WS/src/moveit2_tutorials && pre-commit install + +With this pre-commit will automatically run and check a list of styling including clang-format, end of files and trailing whitespaces whenever you run `git commit`. To run pre-commit any time other than `git commit`: + + cd $COLCON_WS/src/moveit2_tutorials && pre-commit run -a + +## Contribute + +Please send PRs for new helper functions, fixes, etc! diff --git a/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/imarker_end_effector.h b/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/imarker_end_effector.h new file mode 100644 index 00000000..39b7e114 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/imarker_end_effector.h @@ -0,0 +1,178 @@ +// Copyright 2016 University of Colorado, Boulder +// +// 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 the copyright holder 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 HOLDER 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 + Desc: Class to encapsule a visualized robot state that can be controlled using an interactive marker +*/ + +#ifndef MOVEIT_VISUAL_TOOLS_IMARKER_END_EFFECTOR_H +#define MOVEIT_VISUAL_TOOLS_IMARKER_END_EFFECTOR_H + +// ROS +#include +#include +#include +#include +#include + +// MoveIt +#include + +// this package +#include +#include + +// C++ +#include +#include + +#include + +namespace moveit_visual_tools +{ +using visualization_msgs::msg::InteractiveMarkerControl; +using visualization_msgs::msg::InteractiveMarkerFeedback; + +typedef std::function + IMarkerCallback; + +class IMarkerRobotState; + +class IMarkerEndEffector +{ +public: + /** + * \brief Constructor + */ + IMarkerEndEffector(IMarkerRobotState* imarker_parent, const std::string& imarker_name, ArmData arm_data, + rviz_visual_tools::Colors color); + + ~IMarkerEndEffector() + { + } + + /** \brief Get the current end effector pose */ + void getPose(Eigen::Isometry3d& pose); + + bool setPoseFromRobotState(); + + void iMarkerCallback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + + void solveIK(Eigen::Isometry3d& pose); + + void initializeInteractiveMarkers(); + + void updateIMarkerPose(const Eigen::Isometry3d& pose); + + void sendUpdatedIMarkerPose(); + + void make6DofMarker(const geometry_msgs::msg::Pose& pose); + + visualization_msgs::msg::InteractiveMarkerControl& makeBoxControl(visualization_msgs::msg::InteractiveMarker& msg); + + void setCollisionCheckingVerbose(bool collision_checking_verbose) + { + collision_checking_verbose_ = collision_checking_verbose; + } + + void setOnlyCheckSelfCollision(bool only_check_self_collision) + { + only_check_self_collision_ = only_check_self_collision; + } + + void setUseCollisionChecking(bool use_collision_checking) + { + use_collision_checking_ = use_collision_checking; + } + + void setIMarkerCallback(const IMarkerCallback& callback) + { + imarker_callback_ = std::move(callback); + } + + const moveit::core::LinkModel* getEELink() + { + return arm_data_.ee_link_; + } + +private: + // -------------------------------------------------------- + + // The short name of this class + std::string name_; + + // Pointer to parent + IMarkerRobotState* imarker_parent_; + + // State + moveit::core::RobotStatePtr imarker_state_; + Eigen::Isometry3d imarker_pose_; + + // Core MoveIt components + planning_scene_monitor::PlanningSceneMonitorPtr psm_; + + // Visual tools + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + + // Settings + ArmData arm_data_; + rviz_visual_tools::Colors color_ = rviz_visual_tools::PURPLE; + + // File saving + rclcpp::Time time_since_last_save_; + + // Interactive markers + // interactive_markers::MenuHandler menu_handler_; + visualization_msgs::msg::InteractiveMarker int_marker_; + bool imarker_ready_to_process_ = true; + std::mutex imarker_mutex_; + + InteractiveMarkerServerPtr imarker_server_; + + // Verbose settings + bool collision_checking_verbose_ = false; + bool only_check_self_collision_ = false; + bool use_collision_checking_ = false; + + // Hook to parent class + IMarkerCallback imarker_callback_; + + // Clock + rclcpp::Clock clock_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; // end class + +// Create std pointers for this class +typedef std::shared_ptr IMarkerEndEffectorPtr; +typedef std::shared_ptr IMarkerEndEffectorConstPtr; +} // namespace moveit_visual_tools + +#endif // MOVEIT_VISUAL_TOOLS_IMARKER_END_EFFECTOR_H diff --git a/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/imarker_robot_state.h b/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/imarker_robot_state.h new file mode 100644 index 00000000..01c5c2b4 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/imarker_robot_state.h @@ -0,0 +1,178 @@ +// Copyright 2016 University of Colorado, Boulder +// +// 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 the copyright holder 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 HOLDER 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 + Desc: Class to encapsule a visualized robot state that can be controlled using an interactive marker +*/ + +#ifndef MOVEIT_VISUAL_TOOLS_IMARKER_ROBOT_STATE_H +#define MOVEIT_VISUAL_TOOLS_IMARKER_ROBOT_STATE_H + +// ROS +#include +#include +#include + +// MoveIt +#include +#include + +// C++ +#include +#include + +namespace moveit_visual_tools +{ +using visualization_msgs::msg::InteractiveMarkerControl; +using visualization_msgs::msg::InteractiveMarkerFeedback; + +typedef std::function + IMarkerCallback; + +typedef std::shared_ptr InteractiveMarkerServerPtr; + +class IMarkerEndEffector; +typedef std::shared_ptr IMarkerEndEffectorPtr; +typedef std::shared_ptr IMarkerEndEffectorConstPtr; + +/** \brief I wish this struct wasn't necessary, but the SRDF does not seem to support choosing one particular link + * as an end effector tip - instead it does so automatically. + */ +struct ArmData +{ + ArmData(moveit::core::JointModelGroup* jmg, moveit::core::LinkModel* ee_link) : jmg_(jmg), ee_link_(ee_link){}; + + moveit::core::JointModelGroup* jmg_; + moveit::core::LinkModel* ee_link_; +}; + +class IMarkerRobotState +{ + friend class IMarkerEndEffector; + +public: + /** + * \brief Constructor + */ + IMarkerRobotState(rclcpp::Node::SharedPtr node, planning_scene_monitor::PlanningSceneMonitorPtr psm, + const std::string& imarker_name, std::vector arm_datas, rviz_visual_tools::Colors color, + const std::string& package_path); + + ~IMarkerRobotState() + { + output_file_.close(); + } + + bool loadFromFile(const std::string& file_name); + + bool saveToFile(); + + /** \brief Set where in the parent class the feedback should be sent */ + void setIMarkerCallback(const IMarkerCallback& callback); + + /** \brief Get a pointer to the current robot state */ + moveit::core::RobotStateConstPtr getRobotState() + { + return imarker_state_; + } + moveit::core::RobotStatePtr getRobotStateNonConst() + { + return imarker_state_; + } + + /** \brief Set the robot state */ + void setRobotState(const moveit::core::RobotStatePtr& state); + + /** \brief Set the robot state to current in planning scene monitor */ + void setToCurrentState(); + + /** + * \brief Set the robot to a random position + * \param clearance - optional value to ensure random state is not too close to obstacles. 0 is disable + * \return true on success + */ + bool setToRandomState(double clearance = 0); + + /** \brief Return true if the currently solved IK solution is valid */ + bool isStateValid(bool verbose = false); + + /** \brief Show current state in Rviz */ + void publishRobotState(); + + moveit_visual_tools::MoveItVisualToolsPtr getVisualTools(); + + bool getFilePath(std::string& file_path, const std::string& file_name, const std::string& subdirectory) const; + + IMarkerEndEffectorPtr getEEF(const std::string& name) + { + return name_to_eef_[name]; + } + + bool setFromPoses(const EigenSTL::vector_Isometry3d& poses, const moveit::core::JointModelGroup* group); + +protected: + // -------------------------------------------------------- + + // The short name of this class + std::string name_; + + // State + moveit::core::RobotStatePtr imarker_state_; + + // End effectors + std::vector arm_datas_; + std::vector end_effectors_; + std::map name_to_eef_; + + // Core MoveIt components + planning_scene_monitor::PlanningSceneMonitorPtr psm_; + + // Visual tools + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + + // Settings + std::size_t refresh_rate_ = 30; + rviz_visual_tools::Colors color_ = rviz_visual_tools::PURPLE; + + // Interactive markers + InteractiveMarkerServerPtr imarker_server_; + + // File saving + std::string file_path_; + std::ofstream output_file_; + std::string package_path_; // File location of this package + +}; // end class + +// Create std pointers for this class +typedef std::shared_ptr IMarkerRobotStatePtr; +typedef std::shared_ptr IMarkerRobotStateConstPtr; +} // namespace moveit_visual_tools + +#endif // MOVEIT_VISUAL_TOOLS_IMARKER_ROBOT_STATE_H diff --git a/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/moveit_visual_tools.h b/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/moveit_visual_tools.h new file mode 100644 index 00000000..03a5eb42 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/include/moveit_visual_tools/moveit_visual_tools.h @@ -0,0 +1,753 @@ +// Copyright 2015 University of Colorado, Boulder +// +// 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 the copyright holder 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 HOLDER 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 + * \desc Helper functions for displaying and debugging MoveIt data in Rviz via published markers + * and MoveIt collision objects. Very useful for debugging complex software + */ + +#ifndef MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H +#define MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H + +// Rviz Visualization Tool +#include + +// MoveIt +#include + +// MoveIt Messages +#include +#include +#include +#include +#include + +// ROS Messages +#include +#include + +// C++ +#include +#include +#include +#include + +namespace moveit_visual_tools +{ +// Default constants +static const std::string ROBOT_DESCRIPTION = "robot_description"; // this is the default used in ROS +static const std::string DISPLAY_PLANNED_PATH_TOPIC = + "/move_group/display_planned_path"; // this is the default when adding the Rviz plugin +static const std::string DISPLAY_ROBOT_STATE_TOPIC = + "display_robot_state"; // this is the default when adding the Rviz plugin +static const std::string PLANNING_SCENE_TOPIC = "planning_scene"; // this is the default when adding the Rviz plugin + +class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools +{ +public: + /** + * \brief Constructor + * + * All Markers will be rendered in the planning frame of the model ROBOT_DESCRIPTION + * and are published to rviz_visual_tools::RVIZ_MARKER_TOPIC + */ + MoveItVisualTools(const rclcpp::Node::SharedPtr& node); + + /** + * \brief Constructor + * \param base_frame - common base for all visualization markers, usually "/world" or "/odom" + * \param marker_topic - rostopic to publish markers to - your Rviz display should match + * \param planning_scene_monitor - optionally pass in a pre-loaded planning scene monitor to + * avoid having to re-load the URDF, kinematic solvers, etc + */ + MoveItVisualTools(const rclcpp::Node::SharedPtr& node, const std::string& base_frame, const std::string& marker_topic, + planning_scene_monitor::PlanningSceneMonitorPtr psm); + + /** + * \brief Constructor + * \param base_frame - common base for all visualization markers, usually "/world" or "/odom" + * \param marker_topic - rostopic to publish markers to - your Rviz display should match + * \param robot_model - load robot model pointer so that we don't have do re-parse it here + */ + MoveItVisualTools(const rclcpp::Node::SharedPtr& node, const std::string& base_frame, + const std::string& marker_topic = rviz_visual_tools::RVIZ_MARKER_TOPIC, + moveit::core::RobotModelConstPtr robot_model = moveit::core::RobotModelConstPtr()); + + /** + * \brief Set the ROS topic for publishing a robot state + * \param topic + */ + void setRobotStateTopic(const std::string& robot_state_topic) + { + robot_state_topic_ = robot_state_topic; + } + + /** + * \brief Set the planning scene topic + * \param topic + */ + void setPlanningSceneTopic(const std::string& planning_scene_topic) + { + planning_scene_topic_ = planning_scene_topic; + } + + /** + * \brief Load a planning scene monitor if one was not passed into the constructor + * \return true if successful in loading + */ + bool loadPlanningSceneMonitor(); + + /** + * \brief Skip a ROS message call by sending directly to planning scene monitor + * \param collision object message + * \param color to display the collision object with + * \return true on success + */ + bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject& msg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Skip a ROS message call by sending directly to planning scene monitor + * \param attached collision object message + * \return true on success + */ + bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& msg); + + /** + * \brief Move an already published collision object to a new locaiton in space + * \param pose - location of center of object + * \param name - semantic name of MoveIt collision object + * \return true on success + */ + bool moveCollisionObject(const Eigen::Isometry3d& pose, const std::string& name, + const rviz_visual_tools::Colors& color); + bool moveCollisionObject(const geometry_msgs::msg::Pose& pose, const std::string& name, + const rviz_visual_tools::Colors& color); + + /** + * \brief When manual_trigger_update_ is true, use this to tell the planning scene to send + * an update out. Do not use otherwise + */ + bool triggerPlanningSceneUpdate(); + + /** + * \brief Load robot state only as needed + * \return true if successful in loading + */ + bool loadSharedRobotState(); + + /** + * \brief Allow robot state to be altered. + * \return shared pointer to robot state + */ + moveit::core::RobotStatePtr& getSharedRobotState(); + + /** + * \brief Allow robot state to be altered. + * \return shared pointer to robot state + */ + moveit::core::RobotStatePtr& getRootRobotState() + { + return root_robot_state_; + } + + /** + * \brief Get a pointer to the robot model + * \return const RobotModel + */ + moveit::core::RobotModelConstPtr getRobotModel(); + + /** + * \brief Call this once at begining to load the end effector markers and then whenever a joint changes + * \param ee_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_gripper" + * \param ee_joint_pos - the values of all active joints in this planning group + * \return true if it is successful + */ + bool loadEEMarker(const moveit::core::JointModelGroup* ee_jmg, const std::vector& ee_joint_pos = {}); + + /** + * \brief Load publishers as needed + */ + void loadTrajectoryPub(const std::string& display_planned_path_topic = DISPLAY_PLANNED_PATH_TOPIC, + bool blocking = true); + void loadRobotStatePub(const std::string& robot_state_topic = DISPLAY_ROBOT_STATE_TOPIC, bool blocking = true); + + /** + * \brief Allow a pre-configured planning scene monitor to be set for publishing collision objects, etc + * \param a pointer to a load planning scene + */ + void setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm) + { + psm_ = std::move(psm); + } + + /** + * \brief Prevent the planning scene from always auto-pushing, but rather do it manually + * \param bool true to enable manual mode + */ + void setManualSceneUpdating(bool enable_manual = true) + { + manual_trigger_update_ = enable_manual; + } + + /** + * \brief Publish an end effector to rviz + * \param pose - the location to publish the marker with respect to the base frame + * \param ee_jmg - joint model group of the end effector, e.g. "left_hand" + * \param ee_joint_pos - position of all active joints in the end effector + * \param color to display the collision object with + * \return true on success + */ + bool publishEEMarkers(const Eigen::Isometry3d& pose, const moveit::core::JointModelGroup* ee_jmg, + const std::vector& ee_joint_pos, + const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, + const std::string& ns = "end_effector") + { + return publishEEMarkers(convertPose(pose), ee_jmg, ee_joint_pos, color, ns); + } + bool publishEEMarkers(const Eigen::Isometry3d& pose, const moveit::core::JointModelGroup* ee_jmg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, + const std::string& ns = "end_effector") + { + return publishEEMarkers(convertPose(pose), ee_jmg, {}, color, ns); + } + bool publishEEMarkers(const geometry_msgs::msg::Pose& pose, const moveit::core::JointModelGroup* ee_jmg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, + const std::string& ns = "end_effector") + { + return publishEEMarkers(pose, ee_jmg, {}, color, ns); + } + bool publishEEMarkers(const geometry_msgs::msg::Pose& pose, const moveit::core::JointModelGroup* ee_jmg, + const std::vector& ee_joint_pos, + const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, + const std::string& ns = "end_effector"); + + /** + * \brief Show grasps generated from moveit_simple_grasps or other MoveIt Grasp message sources + * \param possible_grasps - a set of grasp positions to visualize + * \param ee_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm" + * \param animate_speed - how fast the gripper approach is animated in seconds, optional + */ + bool publishGrasps(const std::vector& possible_grasps, + const moveit::core::JointModelGroup* ee_jmg, double animate_speed = 0.1 /* seconds */); + + /** + * \brief Display an animated vector of grasps including its approach movement in Rviz + * Note this function calls publish() automatically in order to achieve animations + * \param possible_grasps - a set of grasp positions to visualize + * \param ee_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm" + * \param animate_speed - how fast the gripper approach is animated in seconds, optional + */ + bool publishAnimatedGrasps(const std::vector& possible_grasps, + const moveit::core::JointModelGroup* ee_jmg, double animate_speed = 0.01 /* seconds */); + + /** + * \brief Animate a single grasp in its movement direction + * Note this function calls publish() automatically in order to achieve animations + * \param grasp + * \param ee_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm" + * \param animate_speed - how fast the gripper approach is animated in seconds + * \return true on sucess + */ + bool publishAnimatedGrasp(const moveit_msgs::msg::Grasp& grasp, const moveit::core::JointModelGroup* ee_jmg, + double animate_speed /* seconds */); + + /** + * \brief Display an vector of inverse kinematic solutions for the IK service in Rviz + * Note: this is published to the 'Planned Path' section of the 'MotionPlanning' display in Rviz + * \param ik_solutions - a set of corresponding arm positions to achieve each grasp + * \param arm_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm" + * \param display_time - amount of time to sleep between sending trajectories, optional + */ + bool publishIKSolutions(const std::vector& ik_solutions, + const moveit::core::JointModelGroup* arm_jmg, double display_time = 0.4); + + /** + * \brief Remove all collision objects that this class has added to the MoveIt planning scene + * Communicates directly to a planning scene monitor e.g. if this is the move_group node + * \param the scene to directly clear the collision objects from + * \return true on sucess + */ + bool removeAllCollisionObjects(); + + /** + * \brief Remove a collision object from the planning scene + * \param Name of object + * \return true on sucess + */ + bool cleanupCO(const std::string& name); + + /** + * \brief Remove an active collision object from the planning scene + * \param Name of object + * \return true on sucess + */ + bool cleanupACO(const std::string& name); + + /** + * \brief Attach a collision object from the planning scene + * \param Name of object + * \param + * \return true on sucess + */ + bool attachCO(const std::string& name, const std::string& ee_parent_link); + + /** + * \brief Make the floor a collision object + * \param z location of floor + * \param name of floor + * \param color to display the collision object with + * \return true on success + */ + bool publishCollisionFloor(double z = 0.0, const std::string& plane_name = "Floor", + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Create a MoveIt Collision block at the given pose + * \param pose - location of center of block + * \param name - semantic name of MoveIt collision object + * \param size - height=width=depth=size + * \param color to display the collision object with + * \return true on sucess + **/ + bool publishCollisionBlock(const geometry_msgs::msg::Pose& block_pose, const std::string& block_name = "block", + double block_size = 0.1, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Create a MoveIt collision rectangular cuboid at the given pose + * \param point1 - top left of rectangle + * \param point2 - bottom right of rectangle + * \param name - semantic name of MoveIt collision object + * \param color to display the collision object with + * \return true on sucess + **/ + bool publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std::string& name, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + bool publishCollisionCuboid(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2, + const std::string& name, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Create a MoveIt collision rectangular cuboid at the given pose + * \param pose - position of the centroid of the cube + * \param width - width of the object in its local frame + * \param depth - depth of the object in its local frame + * \param height - height of the object in its local frame + * \param name - semantic name of MoveIt collision object + * \param color to display the collision object with + * \return true on sucess + **/ + bool publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height, + const std::string& name, const rviz_visual_tools::Colors& color); + + bool publishCollisionCuboid(const geometry_msgs::msg::Pose& pose, double width, double depth, double height, + const std::string& name, const rviz_visual_tools::Colors& color); + + /** + * \brief Create a MoveIt collision rectangular cuboid at the given pose + * \param pose - position of the centroid of the cube + * \param size - the size (x,y,z) of the object in its local frame + * \param name - semantic name of MoveIt collision object + * \param color to display the collision object with + * \return true on sucess + **/ + bool publishCollisionCuboid(const Eigen::Isometry3d& pose, const Eigen::Vector3d& size, const std::string& name, + const rviz_visual_tools::Colors& color) + { + return publishCollisionCuboid(pose, size.x(), size.y(), size.z(), name, color); + } + bool publishCollisionCuboid(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Vector3& size, + const std::string& name, const rviz_visual_tools::Colors& color) + { + return publishCollisionCuboid(pose, size.x, size.y, size.z, name, color); + } + + /** + * \brief Create a MoveIt Collision cylinder between two points + * \param point a - x,y,z in space of a point + * \param point b - x,y,z in space of a point + * \param name - semantic name of MoveIt collision object + * \param radius - size of cylinder + * \param color to display the collision object with + * \return true on sucess + */ + bool publishCollisionCylinder(const geometry_msgs::msg::Point& a, const geometry_msgs::msg::Point& b, + const std::string& object_name, double radius, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + bool publishCollisionCylinder(const Eigen::Vector3d& a, const Eigen::Vector3d& b, const std::string& object_name, + double radius, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Create a MoveIt Collision cylinder with a center point pose + * \param pose - vector pointing along axis of cylinder + * \param name - semantic name of MoveIt collision object + * \param radius - size of cylinder + * \param height - size of cylinder + * \param color to display the collision object with + * \return true on sucess + */ + bool publishCollisionCylinder(const Eigen::Isometry3d& object_pose, const std::string& object_name, double radius, + double height, const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + bool publishCollisionCylinder(const geometry_msgs::msg::Pose& object_pose, const std::string& object_name, + double radius, double height, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Create a collision object using a mesh + * \param pose - location of center of mesh + * \param name - semantic name of MoveIt collision object + * \param peth - file location to an .stl file + * \param color to display the collision object with + * \return true on success + */ + bool publishCollisionMesh(const geometry_msgs::msg::Pose& object_pose, const std::string& object_name, + const std::string& mesh_path, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + bool publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name, + const std::string& mesh_path, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + bool publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name, + const shape_msgs::msg::Mesh& mesh_msg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + bool publishCollisionMesh(const geometry_msgs::msg::Pose& object_pose, const std::string& object_name, + const shape_msgs::msg::Mesh& mesh_msg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Publish a connected birectional graph + * \param graph of nodes and edges + * \param name of collision object + * \param color to display the collision object with + * \return true on sucess + */ + bool publishCollisionGraph(const graph_msgs::msg::GeometryGraph& graph, const std::string& object_name, double radius, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Helper for publishCollisionWall + */ + void getCollisionWallMsg(double x, double y, double z, double angle, double width, double height, + const std::string& name, moveit_msgs::msg::CollisionObject& collision_obj); + + /** + * \brief Publish a typical room wall + * \param x + * \param y + * \param angle + * \param width + * \param height + * \param name + * \param color to display the collision object with + * \return true on sucess + */ + bool publishCollisionWall(double x, double y, double angle = 0.0, double width = 2.0, double height = 1.5, + const std::string& name = "wall", + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + bool publishCollisionWall(double x, double y, double z, double angle = 0.0, double width = 2.0, double height = 1.5, + const std::string& name = "wall", + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Publish a typical room table + * \param x + * \param y + * \param angle + * \param width + * \param height + * \param depth + * \param name + * \param color to display the collision object with + * \return true on sucess + */ + bool publishCollisionTable(double x, double y, double z, double angle, double width, double height, double depth, + const std::string& name, + const rviz_visual_tools::Colors& color = rviz_visual_tools::GREEN); + + /** + * \brief Load a planning scene to a planning_scene_monitor from file + * \param path - path to planning scene, e.g. as exported from Rviz Plugin + * \param offset for scene to be placed + * \return true on success + */ + bool loadCollisionSceneFromFile(const std::string& path); + bool loadCollisionSceneFromFile(const std::string& path, const Eigen::Isometry3d& offset); + + /** + * \brief Display size of workspace used for planning with OMPL, etc. Important for virtual joints + * \param display bounds of workspace + * \return true on sucess + */ + bool publishWorkspaceParameters(const moveit_msgs::msg::WorkspaceParameters& params); + + /** + * \brief Check if the robot state is in collision inside the planning scene and visualize the result. + * If the state is not colliding only the robot state is published. If there is a collision the colliding + * links are ghlighted and the contact points are visualized with markers. + * \param robot_state - The robot state to check for collisions + * \param planning_scene - The planning scene to use for collision checks + * \param highlight_link_color - The color to use for highligting colliding links + * \param contact_point_color - The color to use for contact point markers + * \result - True if there is a collision + */ + bool checkAndPublishCollision(const moveit::core::RobotState& robot_state, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::Colors& highlight_link_color = rviz_visual_tools::RED, + const rviz_visual_tools::Colors& contact_point_color = rviz_visual_tools::PURPLE); + + /** + * \brief Given a planning scene and robot state, publish any collisions + * \param robot_state + * \param planning_scene + * \param color - display color of markers + * \return true on success + */ + bool publishContactPoints(const moveit::core::RobotState& robot_state, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::Colors& color = rviz_visual_tools::RED); + + /** + * \brief Given a contact map and planning scene, publish the contact points + * \param contacts - The populated contacts to visualize + * \param planning_scene + * \param color - display color of markers + * \return true on success + */ + bool publishContactPoints(const collision_detection::CollisionResult::ContactMap& contacts, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::Colors& color = rviz_visual_tools::RED); + + /** + * \brief Move a joint group in MoveIt for visualization + * make sure you have already set the planning group name + * this assumes the trajectory_pt position is the size of the number of joints in the planning group + * This will be displayed in the Planned Path section of the MoveIt Rviz plugin + * + * \param trajectory_pts - a single joint configuration + * \param planning_group - the MoveIt planning group the trajectory applies to + * \param display_time - amount of time for the trajectory to "execute" + * \return true on success + */ + bool publishTrajectoryPoint(const trajectory_msgs::msg::JointTrajectoryPoint& trajectory_pt, + const std::string& planning_group, double display_time = 0.1); + + /** + * \brief Animate trajectory in rviz. These functions do not need a trigger() called because use different publisher + * \param trajectory the actual plan + * \param blocking whether we need to wait for the animation to complete + * \param robot_state - the base state to add the joint trajectory message to + * \param ee_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm" + * \return true on success + */ + bool publishTrajectoryPath(const std::vector& trajectory, + const moveit::core::JointModelGroup* jmg, double speed = 0.01, bool blocking = false); + bool publishTrajectoryPath(const robot_trajectory::RobotTrajectoryPtr& trajectory, bool blocking = false); + bool publishTrajectoryPath(const robot_trajectory::RobotTrajectory& trajectory, bool blocking = false); + bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit::core::RobotStateConstPtr& robot_state, bool blocking = false); + bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit::core::RobotState& robot_state, bool blocking = false); + bool publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit_msgs::msg::RobotState& robot_state, bool blocking = false); + void publishTrajectoryPath(const moveit_msgs::msg::DisplayTrajectory& display_trajectory_msg); + + /** + * \brief Display a line of the end effector path from a robot trajectory path + * \param trajectory_msg - the robot plan + * \param ee_parent_link - the link that we should trace a path of, e.g. the gripper link + * \param arm_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm" + * \param color - display color of markers + * \return true on success + */ + bool publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit::core::LinkModel* ee_parent_link, + const moveit::core::JointModelGroup* arm_jmg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); + bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + const moveit::core::LinkModel* ee_parent_link, + const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); + bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory, + const moveit::core::LinkModel* ee_parent_link, + const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); + + /** + * \brief Display a line of the end effector(s) path(s) from a robot trajectory path + * This version can visualize multiple end effectors + * \param trajectory_msg - the robot plan + * \param arm_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm". + * \param color - display color of markers + * \return true on success + */ + bool publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit::core::JointModelGroup* arm_jmg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); + bool publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + const moveit::core::JointModelGroup* arm_jmg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); + bool publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory, + const moveit::core::JointModelGroup* arm_jmg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::LIME_GREEN); + + /** + * \brief Display trajectory as series of end effector position points + * \param trajectory the actual plan + * \param color - display color of markers + * \return true on success + */ + bool publishTrajectoryPoints(const std::vector& robot_state_trajectory, + const moveit::core::LinkModel* ee_parent_link, + const rviz_visual_tools::Colors& color = rviz_visual_tools::YELLOW); + + /** \brief All published robot states will have their virtual joint moved by offset */ + void enableRobotStateRootOffet(const Eigen::Isometry3d& offset); + + /** \brief Turn off the root offset feature */ + void disableRobotStateRootOffet(); + + /** + * \brief Publish a MoveIt robot state to a topic that the Rviz "RobotState" display can show + * \param trajectory_pt of joint positions + * \param jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm" + * \param color - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF + * \return true on success + */ + bool publishRobotState(const trajectory_msgs::msg::JointTrajectoryPoint& trajectory_pt, + const moveit::core::JointModelGroup* jmg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT); + + /** + * \brief Publish a MoveIt robot state to a topic that the Rviz "RobotState" display can show + * \param joint_positions - a vector of doubles corresponding 1-to-1 to the kinematic chain named in "jmg" + * \param jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm" + * \param color - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF + * \return true on success + */ + bool publishRobotState(const std::vector& joint_positions, const moveit::core::JointModelGroup* jmg, + const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT); + + /** + * \brief Publish a complete robot state to Rviz + * To use, add a RobotState marker to Rviz and subscribe to the DISPLAY_ROBOT_STATE_TOPIC, above + * \param robot_state - joint values of robot + * \param color - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF + * \param highlight_links - if the |color| is not |DEFAULT|, allows selective robot links to be highlighted. + * By default (empty) all links are highlighted. + */ + bool publishRobotState(const moveit::core::RobotState& robot_state, + const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, + const std::vector& highlight_links = {}); + bool publishRobotState(const moveit::core::RobotStatePtr& robot_state, + const rviz_visual_tools::Colors& color = rviz_visual_tools::DEFAULT, + const std::vector& highlight_links = {}); + void publishRobotState(const moveit_msgs::msg::DisplayRobotState& display_robot_state_msg); + + /** + * \brief Hide robot in RobotState display in Rviz + * \return true on success + */ + bool hideRobot(); + + /** \brief Before publishing a robot state, optionally change its root transform */ + static bool applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Isometry3d& offset); + + /** + * \brief Print to console the current robot state's joint values within its limits visually + * \param robot_state - the robot to show + */ + void showJointLimits(const moveit::core::RobotStatePtr& robot_state); + + /** + * @brief Get the planning scene monitor that this class is using + * @return a ptr to a planning scene + */ + planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitor(); + +private: + /** + * \brief Error check that the robot's SRDF was properly setup with a virtual joint that was named a certain way + * \return true on success + */ + static bool checkForVirtualJoint(const moveit::core::RobotState& robot_state); + +protected: + // Pointer to a Planning Scene Monitor + planning_scene_monitor::PlanningSceneMonitorPtr psm_; + + // Prevent the planning scene from always auto-pushing, but rather do it manually + bool manual_trigger_update_ = false; + + // Pointer to the robot model + moveit::core::RobotModelConstPtr robot_model_; + + // ROS topic names to use when starting publishers + std::string robot_state_topic_; + std::string planning_scene_topic_; + + // ROS publishers + rclcpp::Publisher::SharedPtr pub_display_path_; // for MoveIt trajectories + rclcpp::Publisher::SharedPtr pub_robot_state_; // publish a RobotState message + + // ROS Node + rclcpp::Node::SharedPtr node_; + + robot_model_loader::RobotModelLoaderPtr rm_loader_; // so that we can specify our own options + + // End Effector Markers + std::map ee_markers_map_; + std::map ee_poses_map_; + std::map > ee_joint_pos_map_; + + // Cached robot state marker - cache the colored links. + // Note: Only allows colors provided in rviz_visual_tools to prevent too many robot state messages from being loaded + // and ensuring efficiency + std::map display_robot_msgs_; + + // Note: call loadSharedRobotState() before using this + moveit::core::RobotStatePtr shared_robot_state_; + + // Note: call loadSharedRobotState() before using this. Use only for hiding the robot + moveit::core::RobotStatePtr hidden_robot_state_; + + // A robot state whose virtual_joint remains at identity so that getGlobalLinkTransform() isn't tainted + // Note: call loadSharedRobotState() before using this + moveit::core::RobotStatePtr root_robot_state_; + + // Optional offset that can be applied to all outgoing/published robot states + bool robot_state_root_offset_enabled_ = false; + Eigen::Isometry3d robot_state_root_offset_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html +}; // class + +typedef std::shared_ptr MoveItVisualToolsPtr; +typedef std::shared_ptr MoveItVisualToolsConstPtr; + +} // namespace moveit_visual_tools + +#endif // MOVEIT_VISUAL_TOOLS_MOVEIT_VISUAL_TOOLS_H diff --git a/aloha_ws/src/moveit_visual_tools/launch/demo.rviz b/aloha_ws/src/moveit_visual_tools/launch/demo.rviz new file mode 100644 index 00000000..0951a5bb --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/launch/demo.rviz @@ -0,0 +1,251 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /PlanningScene1/Scene Geometry1 + - /PlanningScene1/Scene Robot1 + - /MarkerArray1 + - /RobotState1 + Splitter Ratio: 0.5 + Tree Height: 536 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + 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 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /moveit_visual_tools + Value: true + - Attached Body Color: 150; 50; 150 + Class: moveit_rviz_plugin/RobotState + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotState + Robot Alpha: 1 + Robot Description: robot_description + Robot State Topic: display_robot_state + Show All Links: true + Show Highlights: true + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.639824390411377 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.08517265319824219 + Y: 0.05350474268198013 + Z: 0.45195722579956055 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6003981828689575 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.3004007637500763 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 759 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000193000002a1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002a1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002a1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002bf000002a100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1389 + X: 488 + Y: 171 diff --git a/aloha_ws/src/moveit_visual_tools/launch/demo_rviz.launch.py b/aloha_ws/src/moveit_visual_tools/launch/demo_rviz.launch.py new file mode 100644 index 00000000..5c082331 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/launch/demo_rviz.launch.py @@ -0,0 +1,97 @@ +# Copyright 2021 PickNik Inc. +# +# 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 the copyright holder 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 HOLDER 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. + + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError, WindowsError + return None + + +def generate_launch_description(): + robot_description_config = load_file("moveit_visual_tools", "resources/rrbot.urdf") + + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "moveit_visual_tools", "resources/rrbot.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + # Start the demo + moveit_visual_tools_demo = Node( + package="moveit_visual_tools", + executable="moveit_visual_tools_demo", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + ], + ) + + # RViz + # TODO: Configure this file + rviz_config_file = ( + get_package_share_directory("moveit_visual_tools") + "/launch/demo.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base"], + ) + + return LaunchDescription([rviz_node, static_tf, moveit_visual_tools_demo]) diff --git a/aloha_ws/src/moveit_visual_tools/mainpage.dox b/aloha_ws/src/moveit_visual_tools/mainpage.dox new file mode 100644 index 00000000..92d276c7 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\section Main class for grasping +- \link moveit_simple_grasps::SimpleGrasps \endlink + +\section Filtering grasps +- \link moveit_simple_grasps::GraspFilter \endlink + +\section Action Server for Generating Grasps +- \link moveit_simple_grasps::GraspGeneratorServer \endlink + +*/ diff --git a/aloha_ws/src/moveit_visual_tools/moveit_visual_tools.repos b/aloha_ws/src/moveit_visual_tools/moveit_visual_tools.repos new file mode 100644 index 00000000..1b82145f --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/moveit_visual_tools.repos @@ -0,0 +1,9 @@ +repositories: + rviz_visual_tools: + type: git + url: https://github.com/PickNikRobotics/rviz_visual_tools + version: ros2 + graph_msgs: + type: git + url: https://github.com/PickNikRobotics/graph_msgs + version: ros2 diff --git a/aloha_ws/src/moveit_visual_tools/package.xml b/aloha_ws/src/moveit_visual_tools/package.xml new file mode 100644 index 00000000..72d54fdc --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/package.xml @@ -0,0 +1,39 @@ + + + moveit_visual_tools + 4.1.0 + Helper functions for displaying and debugging MoveIt data in Rviz via published markers + + Dave Coleman + + BSD + + https://github.com/ros-planning/moveit_visual_tools + https://github.com/ros-planning/moveit_visual_tools/issues + https://github.com/ros-planning/moveit_visual_tools/ + + Dave Coleman + + ament_cmake + + geometry_msgs + graph_msgs + moveit_common + moveit_core + moveit_ros_planning + rclcpp + rviz_visual_tools + std_msgs + tf2_eigen + tf2_ros + trajectory_msgs + visualization_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/aloha_ws/src/moveit_visual_tools/resources/demo.png b/aloha_ws/src/moveit_visual_tools/resources/demo.png new file mode 100644 index 0000000000000000000000000000000000000000..71fdcf2078c6ef7cdcef9223c0cb121f565490bb GIT binary patch literal 43411 zcmce;cT`hR`!#qe0VIgj1f)t4lu!jhdJ9FmNbf-r6zLu5QiC8UMU<|ffIyJmJ0d6` zBE5qk9qH2M#P|Kpns3&8^Utg`v9QRxxj8xKseA7y(Yo5I7s)S^LlAUPT}}B81QB+F zf3#$T;2FO^h#l~Q!2OP@B2@g1`8W851f!v<44vcuWHsc!0MA@-Q8RIeAUG}l9|4s1 zh5Y=WMCY>UN5eVLhDJ@fgAa+PyS>dkF*jl>(6S~Ek!^~HsuhJ*EUQ1EQv(M6y zDYkwkhdtI4s2s{5ue?2!k$GS?l$&Y0ij(_TiCASB`+j?<^6v{vM}O&HnY}n;`VbfR z1uF1DcKNcJjg7T$i>IgCT!L+D+^_l6eHz(hpIm)CIypMp-8dbPu@qpL{q`*}StvXB z>qnNLO`t$UtU$GZ0RIsyJU7GtLagBQ@5%r3XASKCdeQ&p;{RSm{J&oOKX?3}pZ~q{ z|NF)NpWYsw$^ug%FFvTA1^mm?P|+KbIYjY z-bkrUidEoIk;$jBvNBUsQ?K>0y>C*Rca4m^*9TPMSsR`l{kh+hOwhd3C)eaU-&r-~ z_vrWccWHSat(qE-Riz&NEPAJrLdkSXJ7fH{R zA7d4d#GNL{AT%1Sk&JS9{P?lG{oy>@IRj-Rf%W!W$Ien;&_aT$heiHsDp>fkoCmkl z74`4&K_~M}&h^`$`ef@4ru;V>k2jt%iBGw-T|Af$I1y?YnRu9+tKJBF_~Rmyu~Yql+(MJi(UqJe>6 z-C9{V9I2flJu3QjFkgqL`S}$kr^cgS1qB7*O?uzl(jP3Zt*veE$}L#QjhFqqQMcK6 z@~0Ov@c6aiaR>Mi$QwVtb&nV8nafByI(z1y#Je6ZQYCJ5g3wR7VwF)@0`?rYj!Gtngq zHra{QJps%5LtxJB0UN+}U~q(wbjEC3Bru*+q=g4H^!Dvr^C#OK2NUjnpA$IU-QC%I zWV6F5}~o53rC zc5~T)WojD?nS8R7*&ngJcI{eRS64a=L_2)!cwbVIDRTcywd=e~WCs>`qtk`STSHS@ z``ZWo{KNR;{dGP80g~Y3&8DEiJgpP2PUA<9_@JkXSEOGQ6#P9s>+S6Y$9BHq-E@N@ ztk@d(c23hk5?PR?I4Lrc*7#|vdCAiep z*E5SZo}X^!8pN{61)dyk*Wb&ot^NBuKU4O};nvydah2n!+rgywPS?W5pAMwelTH6= zeFL!&v#6ZUZ6uVMgRuvE3lHRu8KHak?>}Ra@@jp0At%+5{lU_=^qypX_qs{22b9Ew zELWY@PLBG50@rX|tU)KPP1Eth6oaDcQg8F~<+hrM1najHA3x@YKqR#Tt1z}K;KiSJ z;Mq>*`3$MtNS(K{m)A|(wM#*VMBJ(%SVR%5Ap)C%z_q~T+u3|CEMWy3?WdoO%LfLY z0XO+r@X*$lljO*kYldn1uoHL&lHYXqqjSLOLDTtZBwJuq(_CG+;uSqQ+Dk{k-~aeX z$K=?j{W=^1F({5z=JLAA6Di=saZ($f`}#@>3)^mExvmKc8X1W3@I3gl(7m^}w?8~| zg`b}v#LzAmn2Lf+?M1m?;4GK+hOMTnoY|&mqLp=jDC?c{ouBl@%biJxi+eBkXM>}z zA6^ZZ4M+Ygwpb;*ObwD|ySy9r&!0b!ifi)f>!rJ&oCh3C9)Lptq3X3na*1ihZ7Sm4 z?339`LCr0JAmR^O1<810i;7UO#1t%&qRf;$85i|TP5pZKtLPAjoDJR>w?3Hz{Piw% zlib|gm*h@_AfZlWz4oP~`>(pRo$A(a_LOZ@&r^1$g0&niro5oAASMj?`dZw|f4w5Y z#&NvzE49z^^S^)p%AIY;R|gDRc(0G(h^kcMSx=X9dv1(cMW6${_eVi&kqvOE=bp%)#!kL07Ju{z@X|a zOBxzM1~N`w-17rT`|Nc3Je|!<)5D^#yLAo;-*>>V6FeE8t|>(zrVU%uoV1cAs>T(?o{9JurP z*|TRs=f~4BtbOGUgR(~pFYx(Te8QzI2aQGb6nyw_v?v(Vz3B$*X(_{Bo2~=g!RqW! z3X2L1UxS?N^jU!nkYer@J-jMLk^A9;`k-jh)<{#(Iq+d&|NS*pR^N8fuOO9yBb#R# zE;4ZkLF4T2a1&*?A`^&w4gLopx*RzoNzcKPje8%RE&P7J11lhc-bnk3P?623#Y@Fr z7F+p{pV1m;cEJaPAaG1IPmbKw6G0B#>tu2U7t%l=-P&dL9f0G1zKK7?vw7g-5fqGn zb_aECbXJ1!vfY6cG%e>97eBB%|7)cAjuY?R?xDJO?tI^q8Yf3GbHB*p9V>(%WyA_k zbe`t2U6GSJ2kF?n((zS5l{O2_RoSgB5aD$|4UlAHiDM)ArOIF%XFDS|M-p^lKx)1c z)zc@vqY2{gYDp98W;l=g{@RXFZhq+>Rjlk43n?_dY#rE~Lu^0c_!Y&_@SuKuflYe* z%IN0dDBiY49}L7h$Vqq6qBm*XADAR}#WKFg$T$OOmfL4GUD7`;Ovf&$HWZ&_6&4Xo zH;gPihrmK=0#E;@rCpX}!r#8-x1sji6gAn8E&@8ti};U7#(_8$b`oYElTR?f7tYF> z%91errT_Or`v1M${eP~F|67v(Z>sJ8DbfGeYWsh4@&BK1Kd|YuJD{5B`-@TX60rh> z`P0Ni8c8^@tCR2VcPWG+!8yE%TptR^A*U*)p!*CY(97iHVq;vWHr;=k5Rc~gz+KLg z;YSQAz}-P1_&P1E+MxX&NJet!M}5ERL97Ms0a4ibVf;C8VRLpWC|I8Tc}4~$q@|?= zoDS-rPY46P!6hYSrbn03la!gsMnR&Ktt;YPR8(}b9Ur8*iTw9w18lw^myyHauCIkV zot+;)PP)(dv31c6Mi`>Y`{T!tg+0F%L93~+Jf{QkfeSlF<^m~Q^9fwPL*W80HRiJ} z)ipJ@j!18TEDPd@<|g}pZYt;ltS#~q8|#kIgiy{NA*7i7ym&Q4-g@y%l7;^Y-fP#r z9^Ai==13SD8#~%r>M3KUjAUYBilk(T>HEeY74X+XBp!H`h@UvH$131R-311PA|fJ^ z6BoODe!wAnM@Nqub0|0>gdid>FVCRR(DS-^LQ)b8k~Yug{nGt{2i(f<&w@I{AnCyy z^zbam!op%M^5e&k0W%?_%jw)?p%Wmqr{SY>G#|GuC}TqXgq{@@_x7JzaDn(?M%Dyru_-5U^GZ?%Uy{}>+Dhr-y&+~A=MBZe))6t)x9t|wjV z<5iJuHg8zK=Zdk31y%1ylTTNFO|tndXM^qh@#8L}rllq0GShr=v~wC5w42cuaS6yjr=yIo0=*r=r6VjQD_Z<2+ghT2XZvC zchFok56CSR0?3=i3}O%OdQ^wLNJ&|u!rC{eIWoLO{^wTb736is%BwJF4P-w^RBKT5 zA_T%4xB4Ah+j+i{ddwvw>vj%H*7jM%0EYS_@-V3PGbto0CN}B2l)k$``_JG`El@Pe zFo;>wgG^j=kT^J3H6B4nC)DY3^9#-5y09{V{YGLj%emYbVSfPZgo(Y9R@>EZ6t|Ez#D{b4(a=YfEhrhaa zCvwN)Wc>I04M6m=Qe4fV2}=$QC4BQ`fO9s+T4>p=2CTXBI@D~hI_zmN9M)nDT31lz zou$5bk-PdARH9)2K_Zo~>t%RTlLmRf>I~i&tW*!2$?@><@nul(Th#8pOR>zshWy); zWxsw7Q^7>>*i=l*$JT5*FP`@W1%yy$A2M1Qk{{j!rRJ}N1xQ{)qX*oZ!4~w(V5eDS z{5;C<#e$Mf31!ZrthehU{T_K@R6RTLLQYM(K}|#Bc-=WjW0*vNDu85BIZA;SG2%tr zLk&_ic&x6j(k`iDekp{uu;i$uKAWfl%$8s zFPX29w1^5uIbP7fcF zN4CLWuzh)$)7`swK_j-sRztc~rlm($L!YfF(Y!IgzoK1qMF89U;7s0%cR%DQ3C=O9`1wg5B>MP;Xf=R5_?e%Kqu9XX&_X)H za`!H9!i)^3^MOUT*|2+dcDEC`)D!!b+7`!H7$UC38aUMN$+A?lbu}aU1t%4cTqv!9P^_!j*T?3liNQ%o{uMQV^N3c` z(G=f|I{;A8&$DJo?d%gb`l3L*l^z^v9&J>Py$Rx{=9Vu93&N+<|JKEe7kl7(JyXMEO>qI0_F&%QHWAeP-9|RaYD7GAW1)p>ndQU(p z>Q#g_$NDH>zp0>vumdKzC`zODI+ivD7CV({%NdR1&R@<@@IvCtQF17XJuNTV zj{v$IO$cG1V1coPE-L_c*p*uIeSUIsQhcAAc{O^*7z+>Po<~^td?(hwB}oUDmhPI| zK3w;u>M7$=U%5-cNfT{8`9ut=b~BG~$TEYV60E*gom~Hqjls8+YX38Y*eqV|FY8^-O#sXyO*U|e3*6*AUB~;Y zLOVTH4`6tg#R^_4iEX@+7sOmvN$iQ=D>AlT-AHwaBd$QE4y#!Tf`AVjbS4ajC7&)> zFDqWt3#2+}0j0#UB-g*gctefdSPl=!^zox+X5*49ObVKd|`%8B#8A!)-wrQ z%(bz$BZk(p;as4THk_Ry;i>c9)3i&Cc zTw-+^n>o~RQTfP|WG)S2IBlRlkTIeL)9$70|QxkAe%#ZHT=E_%aq2TXydbN_z$>hZz$5!?%us3 z^O~|I3$EF|)}nA6;_6@J0iN5Vi7)x1UvWU6=kNXHOgd+im2-GzjEBzwdJ zv5jTv=J6nQWMXpsPX( zbf9cBb8Q=am2ifrW!RZbZI{yRhT%H|<$NT0v@(ZqMHW2Qvu_10NN>y~OV5iqZ0eeK zT}Ip5=F03_Z$`E#A;;7&G6-2SRJt&m$x&FepPiJwL%(167kFu~#jzjuPsrky6V#NG zrP5tDKb)v}lcRE?#c7j_Oocj2vAnju#YoGNlugjRJDBf@nxdW`AR3X^o>iL*w25wU z33U=v(V=mX0vSPPM}eMSx60MV`QDY2{ad4WM`)N|NLX#2!@g;UET#3XN(fq-!S)cd zPM^NNZY|^k?x{fK=I&l&pp1nzaWhk{=6l*ZpMB*q-^%$0>czdno``&O=>o^)+t`2R ziy1)Ep=U7($@oX!s^9b~+Ul1AmpVB~Se!A~*7nssy9W?oqXEqFT+LEOE?+V}&SJK(FsCfz<=Wy^KcpMS8v#xE^>9 zW4>n<{v=Me4KVG~;C)5yi8utjX}XWWOxO(u?6LFJ17UvEIxO2Kwu35-&lw1ZW3t>t z_`f4|ihEv&5K z1)q$SJ0v`Q{3z9CnGi$&KpH?Gr%QyNk4;#e`Ze$VyX^)EzfU)zttK5>08xhWzGXk) zj~U>EA*c$)5G4EnjVrE=+9+CV7NTgHz66<7I@YX}^*~t3s?h?@kXOmS$t0Z7kt&#kJ6R zmqIg*AtD5Wb6?W#9C>rgU7GdMH0@IEm=Y54i8)=;9kC#Nx;uEfV4Cyl{lB=7-iecz zbY8M{-l5u&@=G)v(7+7n`#x`jDB^ z5{$NvlkZl2mgi#AL98f>lDQ&1pd)Z8E&4V-LJZVoh1>diFt39)!2dR?_NrvV<>J)y z!lNfrG|?W_`^KiGmrH7!KiM?FLk z$y0hdp4qIARTx?Nav#Kk?rGe)Y0?2+Iny<6zjwW*eowYerwB2=VP@S{NagdoGcCjt0`bO?M#eo!`y@ zD1{B9bvgeCBmh0n|6;UWLk`+f&=DhvLmsFwcUd!Z z4ma+L)k&%)E~J*mh-K^>UFi`iwu#`mMH3yB$lFua@P1Hyd?`al=r-ej0#8(ny2$8r zVqelatECRo}snclzIOp;>y20<7we# z)kiV=4S>D=^zTeSflo|}vPQHm!-(?e_=Z^4|C0Q^V`-C_B63gtP|B zP*dUofXaZjUgMf(*9Cm;b!0G7Uxj;m52f$GWB>bhm;qZ%h)W>=4qX{kN`5K$y$uWB z7#=rd3A-_$!y9AlpZ$_|z>iuV^}K32(EseD(xj*!o=-F-^+x>WqQ)~&;zA<;nT7z9 zM>qp;+Q7(Sap$PoV9Vf6c7x*!GuU5ZJC(}EuY)sz{)wiPScGIIXQ&R3Js~E z4&OmrZD?6~XS#UjEF+ZtX`2nf)3GsAqle?O={x=5ocxO1pucxgcoPq*7%V!n?|Waz zT<_|sqHXM}XL-CaarJGC#K!fHTzg$$7$g=ncoI@Pn6#?`oT1)J+{h=Hy1bz60-!|u zrijD$w*>{)kM*tVu=2bR#2Qbkb9n`Q9(h~fU!7F0fW7yJQbM$oQ=z)miT<>*nJ~xC zskqJ~y%8O`|6M!MvOHWMh91JdTjEEx8cXY+VSN{8XG%%~cuH=+k`4F^xC@v9@Ad0t z*p6C_pR<2fC9uk_oNgn1|Dy%q^YHP_pMk&e3m0s(nJ!LMFBCK08 zr)68aD&n-SMIG)qI9$t<^O0R_S~~IOr7K(E*Q0veLf5kB_AFDRQEnOb-w;*L`AUzu zt0onbz;14n678Nu`U+5Y8jXF{?Kj5zdU5otvb zEI$3@#`YEAPJn?+PISUl4{yq`MjC^Dx@x@PL0<-7h8)r&Xw``o@ZdB^NPFv&5W`-? z>DHLGox&Ngx4w0l$_>2>pjD8F4@Ia)kkY&~;i5F!O$c%r{ z)r7x8jdxeBa9dg99`v!QC#u9U2xHo#uIcBAii)m{ej*JL12jMQ)p4ZsO6|CEFKK)} zl57R-a@S=SJ~D4Q*46Qv0EP9NHte(i1QFrGFhn;Nc5tE2xVTmR#2m#?}dGX))sf|AX;Ho=5T} zBH{k8I&NJul3(IU!;mObJq0&EUI&g7`A~rh(AiCgbMQd_`pDKRZ&uN-)7xY4hZ9%( z4X;ZWD0~V)-#=#HbmEzB5mx4CL%h7afHH;TOdYLokH>FzjsQwIl7f-&L7$(6j7`OT z6P0w3kB|yom+h+9noiAsqcQ)@LLryS)dJ}vjqfAWlOOMs;$~_tQ4V7PZir{a7KwI# ze9#BbuQbl3ywmSI=Ag~$=}{;r%L>eP4bb{GtH@b#pjjpnpd^GwF2hq;6bb=V=q-vH z0TB>byer~@Ra>vNjm?z^fzruvE{zKU5dd!8!}f{&rv7R=dUx7LX3O%>*3rUb?I(g~!j2RIft zK!kaER-6j^hJaeKrl!V=8uw{GvEs?a8)u#9NC60F5pv^+7({840N4_xz)%iBT~SGp zzc|*C+-P@-=1TqdR#z$Q3%E-W{ie z61o*ChiNPPJ(w?k^X8m0k5<~#y?lv|9|DMi0>1{!l%DWCMyDz%yIsPTesulf@cF6p z`8UDXNLIi1&H}BJ4?x-kahKw5;s7uLv+Uk_3W9I9-Vq3^AH07 zQ%}c1L~vjAG;686To=H8aC}Om*55c2)!_ERpCkq*0$4tPHqycUyJ0`^VIsbZGiL6s z`U}w9CLJ!E1bmzBLp4n`M!d>>KPYpo>8b?0LheH|12H!;M2z5A?r-@OupK)Zf;3Dd zeGyz#SOK}?Th!P}=ia;v>x>Ar-d%&mW8h?e7Q%CzIezKm^A6Bd^qD&k!{4?{KYx|f zor~N^{FKNEBq)?Pd1=-@5S6yQ0=d%iY5)qz`a};DgWJjWsK;66T({uJ zxyVW2)zHkYU}t!ha)FRcoD;A>k+!7SZ z?d-%x4Y%(I3hC;Zm|TE>PKb!lJi!>HIS2~;>0feQ48<x&7a)o;m3}@i3q3u( zkY}X;v@Bh}Y6^=9pSJ*z+_E@8WD$KNW_GTf@oe$uJHy9c+KpP@*>p)gdWGmC1-AKD zB$zs$APw>1TJi5PUwnrr>=TOcYH(p7Nq$V}fs3-_;L0-|xyXq#t_K2#?OhH99e$Fc zkHw#qFF0h^nQqQq%1BBA+_CHaH6)EdA zlSeymGI#pXy6}v5ul3OBONN3Zex_VIq_1}MltBGUS02UE+^okxcBS?OX-Ht{-P+Rx z+TM>Y3(Cxx?a9qR@hO(`lO?&XK{zl&#P^8~CxWMSrW>tRHj7swVnPDdb`DAUR))6k zV-XP~MbaIk9O2>N!k5&C_W)6@=FnbN^fS#cYVgCL<&WR}SU~2y6&LudaErELujJ!@ zR-kO&ARk#u%3te1-yl99>WeES<)N7tpL+S67_*%(cY49hc65(}fnsb=5$>=*G~)cX zOY6l|t+6_9LD}@-)ycB^8sM}5uROkPXh5I;6zHNZ^xPCSpLMHYu&ocOu>p={6NIZR zZW8%TUz+4)z5Z}6NBi4C_Y41dk=*}uUlfj(fa+cFj(bJS*bhUb(_?+OjE7$yOtjCk z`9CBGF6xH*FFV2@{prZT8xCmk_!m^TAZ@(^b^sLwozE zstMD%o!B_pAnkqAC5!%z8^7s0y12 zSzwUn;7hJ+dB$9~p27C!GWunDF@LX3eypC18Q+iuI3$E;>}32`NJcg9c5x+qo+Uej zZ(fW%h#+W|Rvu-;*C^-BjQPv7lf7Xcw^4p8hqO=m64&$wu3-coe(500bec5!87`iF zM84`)hq4Q=s5CP#(h}UTZzTGbLDSkn2+c*Kdy%Z<;T-Go$GdDM$)7%bDyg0$_pgJS zBuMS~xpE&g-m`v(Uci-kgebFd;<0+mEg%fwsaXP@{MaX}<>P4Mk^H2qIzWSg!9~@) z|8Qzx>-@E9sflHZ?kCgk&2y@jG86&vn%690N(btlv9Ej(rcDX4kDxpn> zjM9|~Y?5Q9ROLuW)ktyW_c`Ws&b1uryYnX-+jy_F8-ol( z$egtJ?HiAS4w(st$k)mI_W5&^_g37{ciYC1Ng(wZEqnJ^IrF@-(L|g{OKb41fkDG{ zAu9RO)cYpMaPu7A>)c<6{t|hq+Qf>9iq4chEJLX}X31}!8|>}vpWnYVRU<+X=B$zUv}-2y;$1^SJ1f4SY$QNRe@W1bbEPo)0*S@( z+81V*8+sjUBStLrZZyE-X0E*Yh5*rmNBhnDqN0VK#KlZ4@rz-rFxw($asZAX03SZ^ zsAwmU2AnXce_iihHAR(*8Q#8qo1F@z8b7Jk$B=!2!^3echz>q63KJmFyk>}SvP#X! z$mkW${pmPWKZ+|=GcjS@U-ft@Sx{IA*tFR-vED;P9(9p>pP#eVE0cEN{exNovD_93 zie(c1ZAr5kH7gU7XpGJI5Fnrm$6L&5uQ8en&Y=&F!;B$$6%`eah>oS5Y`%6ncqzS` zWIN%p11PtULA!npi;Ii&^z=G+WV(4bpjgNIK)sq_;G}!i}*ZmC}#&LoAvAEPhfi>bV1&A!NjS>ez z=G;%aZi;HP(JKXx?rK0T;pTuNGQCF_yztP!vm_}1@OGL(Ga=D{WHHnSeMi7)OBkv5 z`qqW-#2=opd;+O0yM~k zArPe_gLGF;jJAQ3Tlcaf1E-vvoFr?%{%nj>47wg*o53TWn6`h#zApe=yQzV}eIypJ zxLL+q-JB9R@X+8e@ioZ4q(SJ_1q#i!g->@vD)=SbI*R$~EuF_L#@3bT(1W5xA@c6- zI}!hFj4>kaNf_G2SD78~@zX@|)w2H!3}3%KYi#)N;hKz$ zjAckT3(C7Ws{>TmK+kZPlQ)n}ICc3EP#h+5@jrQxeXQXGJ^ysCP3WE;38dio=+%8P z5=hxgSw7f1W?F$sUJAIVgQKIPlT+hz4%S8X`dBAk6|&fqqH0C-{%VBH7P+ppW63Za zsxp{LzA5!JX+Csc1sTdaL)j9+&Pw#@jb(-3!o$V}$I-G4t3sZ$)u$q80?6Sh@%kRQ zVHgbhOCV!@xpC$aa(Ahz;)PLW`x~;KZgbn?^N5;4x8JnUS=BIM1|VfDQ9|u+PET`U zs98GIPzWl%lz}kJKtk3lV6~ZTvp^F(c95F z7#DyB@N%gbkAPf+jQF+!MO*V;FJUH(OFib06AJbtAS4U5Hc6%sfj;ZsJCHtT9fH69 z_W(MCJkV1IojrL%ZZWKrDQf|A<@%_#380#bGZR9T`L(X7IS^l*?`3*`BMKpkoO#Yp zc01awUE=$!$+|(qGQei;epbgLnJ}QrWD~|bEhykc2&5FkK^-GT5mgbPrrA&`=rb_t zg7yeO1DN!evcB}Ug%k#Q^fnOtF3R~yvqJCRzb_Sx{>CT1U4<`T(F2Xrl4&iGR>wm1 z*HAGz1J*$N+P*)eE`P6CS>s-_xxU38=A*nS=kJ~YK=vFfTrym^3g`r&=B=oxV2Odn z+qBmQ03HGlA3ruNr z`0#24s)rS~d^TpQep$sMhD%{-vpr&99)^IZ_^DD!)$i!KY{!AJ8%RN?+-aFy-EcF$ zRC;uJC?GUP^|UgRpY@H~F+^&#NirVVKJ!>Lv*jAd+F3e!_k-(v~v_%^y}BJsidyE6R|u9DEM~BR>tdr zy6q?!u*{hS>Y}>#pT-g`L!$Va<6}VfYzeAIq~%_-i~5a0QJ9jD)(vmVjhuAs;VT84 zgy+iL2Uc#H4b8o39u5Fa%-w(1nw)O@=v6R?%&g7JIOp^0w%2W)@_O}7R!`6N0{=4c z087r%IqKF4489fm2pyW`$OcJMA?~35661fh|_+`lO{>ydZm}rUlH8>CHlJJQf$Wy9of-v@1^5we`!gB}EGSsayNLgNuG;ObP7DC`qil$BU@*U0Dw(*6a5wm*9StkCzx@9hBR`(zAV?*j9XrKgb@DF5)G+lFa6)R`c-u6?~9Yc2E{jyim1Y&nzq=rVaY z=r;g550XA54mfSpH3XF9kj5JmB0sa2I82mD(&}krZY<{bRE9OanK+i4ESZWt9B>L7 z&67}nMHafZA7=MhB|rX~RBFmG+Gzt>1F`F*Ox~D;no(DyX&^ODZj# zTo>D#rurs`N_shdaZ#fdxbT4z!iM)PDdC>q_XSdPAX@9NN*~V(w-5xyj#O{Fgr7^t z8g&NX1U`pZenwRFo8$l z!A?aMu2`ZSD0XO~TR5nRuY$!ru@wg15sKL^?99>16CE3*yQH;D#np=lsJd!nYT4=< zExl**Lk5!07sLkEpj{Xv`4RVrGyS497wyxijOekx9zpv=GCTldjob2T=;-K}?|k-J z!jih@rib5qQ5`&)F-7w6d#25akx)Q~7hd zT* zjxp`2(P>u{i^@OChg^-uLlh7#7R-VK9rak@vw5ixRaY=4=)BXcM2>(+Db4db z{TvmX!f1>k&jl8#PHI4(y(tj>v@0ffa zd07B)PIBJ4M>u9Q2tn!z47P63MS&&_50?z;m#&ImxCe;jw7l7pHH%Mcn3ksP`De2( z3|6^$4tWl4xPDPBD&nef3nTmk%(Y2FKG_>EsQ=WDmrl~spZQ}b8(02oziluk5is<| zAR#^B^Ea}avr!+A32INpH`*Z*D?-_BNTr^!ApOgux0Xp`a)N6lynEwfmsWWkHqQkoB#;A@!$bLZ0aMRqVUrRlf$OVYr>J9ZvpHGj|=(9_76s914Gp z#5^hjpyIq!wl3MYBEUF_lrHbPk#{l}I?c7XN6uhEMoGD-$n4WP8462EE`=+C)H>34 z41f!W>WqV#JUq^j`I#3xYb{u)=fO{I-zhM*5Ck{(~&Vb7rcO;_tBg3;A5L3Q<9>fngFQJE3Xgc7~(lRZXBNsVXI5p`Uj`;FGb zE#Gw^G}@Eic|5ZDAa+}wL)gp~U8&M9N{Z;z&hBLyfT+W;5uQWCg1 zdwz>zy%5Lq0m+N#vSZQsv$lPUv+3xMGiD`xr@kWJZOt#?3DoAqFd*Kkg(c2zibhMn z9~T#<$`=^brvx)%d~b5ym6S+9kLFp_tey-+HAQVHz~5!t%oSBou&;b!EX?*cnP6$~egPIB z)JZR2mRDAihi0w%0m*2wd}kXynN`TzkBj+-57(gPc&ZeBv8=~p`&&G-vzeEi&mP}^ zW{l3?B;D$6LgH0=a_int{31mR2q5mJ^#JO@vB3Kd2q;zTm)ZPO>(DrXeDY#)GGYRV z9Opi^&r_2T1MS3NJT?F;$d>=?tmGy4uLW~(zm;#CfQ^auXV4%5P*Z%~nwg8{5LzTk;tk+MT#^YX5?3DJPqK;aueR@|8(;WwMHRD5h} zkun`+=2R12MYKc^tL8a=+cvP@e<`THet)PirT)v^O1V-tHlQ0L>Ijt+`$DXOI`oNNR{`w-f{b{p ziWgzb_7Dk=`c&>f*dVl{we$3YJvmf;=UZ(5#^{DXy_oo<2d(n}PUP|Zdq(XNjw3)y z|284<^x+Rr8Oh**4~zwDlUsV$=;puup2t@KEkMwR=jGlR zLtqq7J21)p4Ua<=Rns(^h4nzTC72#st}!t-uAU2}*UA(n2)4}6wMP z(h3OSJJ>>vBs${^G79L>!gS;|f+$w93*m|d1%Hg5Qe%C0ehGtdtRJHM3>2NQ#`3LV zgEf}r>ExvlNU&a#=9QuZV1VfjXRrUxJoS9Hy45*1hq1RJO~NNlUWEJ7w~HiUK3mE6vyDgG$ys7R@q_OkR2psG2$hEM zbg|YhE1I^mHqVSp2iu5tq|4p0un5?iQGh=8_U;}%u}IDSvPXE4lhrS2>Kw}-EQ!aa zl;s|P6i|OPNGP1IpN3h4S}@p#A;R{fV0}kp&>u|5YK~T>>}ln=lY9B1rHsvx@DP*= zQnc8xIW2j(vYs>;?f?>gOG`^vS6rS$TSWTN9VUzVQA>+EpZ`Y-kZoD&+ACqBX1dm? zwcn3Ea3vPf6{InL1_)aQyH(SWrN4#4?hW>J+8jjACVFXHiuhy~%+)n>CWzRVR>K6f zvXqv*0_83;0(N9z|80gm?9Vw)k5omNrM1cD+L7t)*FU)sd+U3 zM*+j}+-$g+@?rpqM+84rhePOA24rz8CtXzIW`lQvS zzk#;B5Rg=`@PY!MjBc@;+6N;e%>`t5vX5nxNlsei4UF*y!;Mtl82|kZS80h%Ha?&5 z++TWkBLVx~CM0{ZrSzouhEiisMK()4Y>&hDa^O@Jo6MsrW$A`x6T>27n@iTx{)eM= zmDBGWOr_gOtE_)3Li4#yvN3m+5ATPq{<7ZLJkxcYP36LiR@?3tH@pg# zEoe^&-CZk=vgD6tB7uTQIN;^{qa2IFQsFaBknl(r!ZwkA8h z9<1;mbVw-p#a|cMj$m4Nqnlr(;g#}vGF# zHNVNI!3_IN&$X#_e;Acd;8S6ewO+rC69Nkdffhby3Kjs$?fXcB7v%7+)CTS@4`kHP z+~!R9t|r|UHPv)XpFZGcBOXh+H+Q%_Zy~VNzCgADYd7`In^yeKimcoDA?OAtkv1yF z_Ch)hcA1f*e@Ye`e=5vIWoEm56#?XB3;K#%CSWv*5CVK{THc3(FT*#bR&})L0LKrS zWgC@QvViu9lbT-ZWSbC3(|X%-?EOoG*rE;$6@&3ffXN#eXsN4T6A&ntF?|F>*$ZxgDV|Tp$Kx%K5(-I%gdiVz zHEK%YOQPc9Y+w!s2+RPx6=_3p+N0~l07{f{OJ4n#teV)7y*a#cUpClvuKj{QSGsBF z0x;R{vX1^EmkrY)7Qns%DI-5X*jrB4IP4uqTPzU&03^0A>nsE=fwK9KOf@XE= zy4Q|P9UNn0VmK!qb!r*M__jZTQ~|7ie|Ur!Go3dMZOjSB+AdnK!5L(xo`ukEj|EMngF_m(4r4xeMgp;mi`-B;meGNCi{ zH(5t~0I1PE{jH9Z(W?y<|NTQ}(=26R2v!>=1m?KMDhT`Hoes%vaMsgyr-?oK8*S6i zMUC%Op1+ZBZ57pWq7R^|B_j!o$yu(o)Ya7mVhZrTuI|jvk&kq)5^g4@-vlRL7&Y41 zdIfQT^@WiWrX7sNwCXW`$$y$<{J!8zN9bn6>T?48cS}IlJHNhW>n?*`8MgHQ$Qu*$ z{=JjI%l0FybgK$n41sO4ihAxhzj$JNss*X3!&Ga?kIOz`M92p}2*FUh+)%if_uA>9 zqoby5@IVE(+En^cvFk{4xcjXoWx>PmD_#KaXXqybGulXk=0zh923;rGv z?QU$b;_@|I@cy@pfLN*pUna4=4

f`yiX{3#I|4X9knaOHSF_ExHiJSRCc+4@P4g z7*hS3pTsmQUDba07y!C5HF_xKNH7_O>X}o@m6>t@$Oe#NXQyPz{mx=aCV)Vj-2z?m z#ulgzf$kp+Gw$hjFV>~54EtiOP=dUYZBMtKs5S{V`|RC$X^{8Z+hc{*fd zM<9rw-giU?bVm;0e5_)#yk&hcs@dyXxN&acU_%MC{IX z2piV3CV;@C6mX*s$_(L-){IlvH<7>?qiK0SJD>mV-8LmN#C{vYOP45}cv^ApJbEr^ z*{am4l(3#K^%E|@_=Tb)8`)?U&~REm;0I0$)pJ?O3+2>PHUYlC;nJ|@$_J_OYj5Gi zKZ&6%U$YyP(pBQ$Fqdfyl|OwGz4S^{{V9WsuJ(d78ZSY>sdJxwW|)^FYz9;2V2sf7 zM`s8TDfmVZU69l*Ei>MRd+;I{Dh%nkX`&q`Ym-zz`Br^(QbFZ(wCdHoT5$Uy%qNap z%DZoJ!>+fP5DVtu=9!-;FWBh=%)-<(EjiiH)O4)BKiF>=;1aKr-@f5S?2}}9UA&Wb z>SrXwqnS9NF97MDg`R!EWYbaNymp6Sd8+jQ_kBNf3`oz=4We`@-6e{Y zbT^2Egwh}q%AkaRf`o*0cXvuR(j_U~{lDYq`}^x!F4mHn_uhT?o_o$dd+%E^`kNwm z&Ll8sbmoGH6oIab5f|$;97=|aT;$Gn>;iZnw$eMAb`G8si-iT;eT|Kc{rYteP@_e= zOjQGco)dVYoFSF=OYc-CHWGAARjkS(EL_DO8Zqclj_3~_K9*N^tDDQsUFO?@) zD;m~;)Ou@a?eMF4d@u)PieJ-RE0jAwC+9byZh}C8@&gq3m6Q_i8$Q}e+C^2ruWQZeBHlP~EqJTU?bXJEYL5zdLq#)HTBh!{4CuoPKN7ny zdI$YZ`Ss%NKl`@mQsex91CM9Q;=GQohKZK``WLymfgF|N zK-SFoeV>a!GB}>5s&WP{!{q$2Y|^IrnXcOs{EfZ;>D&lR_dkKqu%h?~0+bqt+Uc4^ z&`@2rurxMhpaK2bMut;d)u~JmyUtM=T{T~t!UFvD!G4hBM0B-vq!kwzS671$bz5#O zkjWqw|D?D-v$D$GRGWQB`>+NYe4tGU6w{{IbTNLAO0oeN{qOq+eoJBjOUWy{-yxWt zJb+LE0g7STjTeyg?@uEk&43)`30Z+Roy$jn_xVGYd5W$BHM%?XpNw$V69~U4EWpgF zEnHFL)Fx;bz?%iqH(M$S3W_u~&?O&RKl*(VNTm>XO7^B?v7p3*(el6WK_emdI#k z>-v%J=2Mdd9y1wc;uhJ~7|?z<4X>HoZEbBu`pN(gh;LNDzXVib{k^@27Ye!o2N29h z%~B0gZ2)a{BnM0XL}9z{HXZI;iHG3U87t{aS)5$3+vb#&%5|b9IiX{g{E3*3OvG2 z?mJPe_tNU5Z+B;VI#8R~>=Ue2cNb5AgUmnItp3Lfmf_=*lLm1eV6_1uHqwPjiMcfe z2$948vyCbAfR3!DK7K|G7q?x@d$R&{1mqC~I3Flu$|6jXsWv9p|Nv7^%0BlW$ zkn8b0oduU(PSfv|Te>x*uZUc{2UQ>d3m0m7zh`J-LI>K7b}>-ydS_r7kek~GJVnmy zQ`MA4%K-@-1n}lK?$posccPzi4d1ihJj)EN3ONy>!3UeSSOChpieH8%jaXeh`V)bM zD7sX~g@1&$A%UI6miImqxoF__R$w6BHxqgNig{rAG}F{tE}EH|19WA%5_4Jo zkZDIQ()t#~;=IK)@+%2y^LmPZa(w(nH&XSO}H;^L01u2qKX0EsOs>YWAu zR&ej15djs_4$!MjHCqhzuqiY6%~q-Ju!t;hSG7b-K0*k6tg!QLoT=68^2RgG1|!eC_n|`^z~H2zfx4y8+m% z87cJo^<<$Dh(Xo;9p)gdgzx}Db}v_5-+@a={Uc@($!N=WHoah*73730-bQ;5ZOCWg zirN1Umm&QqCqmY|F^xCAF(eO!Xx^<0g7je@X2fEl2SOothrF#u5siIw|lN%d?}@E(S!#IoePuWDjrodGq9m z+p08Z9^ti>=7SVs>0=ULATO8!62uz-cNY2lVgFnO=|HW>1pAtQ_@nSuG;-a=kJ>1Z zLuQH-^C81NBes1pk9~UO;=y%M9sU>M(OpcQFYfFe0{Dp1wvqI|K0q+dOaoU+M&!|B$d&N-g$A>z&bk5$hE0Q!70@=Uul zV`hSTp&KZmiMZzVuH=*5Xr^a_amt_#-O86?#PdJWUb!wLq4CJd7i7WL>^V2KpdDzJ zaQcA3kb@_29%%J}T7L%kIr&t5tBn&1BXQMcZWKlSKG31>Wi$l3E zzZmE1mZU+ECEsV%K~S^Y_Y=9o8)t@!QJL8O9b*Sxc1Q#+A1)NYLQ(7NGt-%iQ3wb5 zdbL}fxO*TKl0F`8!zO~K(6)y@1hiGbJ~a+5 zV2cG20U|Qpe64I4S9~L?Lzc829ao zR4w4lMr_jngTa7PNfZLZP#}B@kOO@fuCMI(@82iK z3u#+IaMiMjic)>W#tCf$JbWC?_dt!8IcaV$00mH}3bkoy#-6OCy~UC2V;k{-419_*&p!5h5*ZYtrbWij z6wa%q1(GY2#fnK|gd8KVi0JC->F(xHA-O+|99xlx#m;gZ=cj-~vs2v*d>K0m(#p(R zPEe1S`b;y;^i6w1&W&s`BNLM-DDo%YOyY{Ia7vG8^1s+gg${tb8nq$f{Xqn%g&Q1% z_mGZs>|uPs^AITXEf1N_g%K(BwS@+8?!biwaQ>->SH|_1cg`ib13uWdq}c)~iA(Hv zLwjnZ+?!~Q5_oh-Y=O$0?j3I>c>BP3^WbYZ5u%IE|5|k6VgPJD^~QUj9?z2>_40;w z+&S&_r?1rRiVp4>af#pJ5l`<-l!yX5FW;;dAj^&Kl@s89`rtq8r8 zzc#9SVo5lpLiAOFwUI0TIf1%=6QRs#`Fkt^$Eo5aV8OGq zvs0q`ce3fWBGYbeZm!bd?BoQ)70Lo@y6AU$@pSX)9Q-F~H$~Iix7hw5<>%k)Gb3;LDrif7u^~@s7s6X?0ahEN`i5(p9)h_bT77=n zq1PnWkWHpb!tndG(c>tkZt<$pN|wxz@@v{QEvhtK>ip{blkVL%bBMsqAj>z7nEtn* zhFnefK2z!NJvn)R6k#ji1t3MBZt^UNGG{<=9PXJ^InDw19zI^)gD)x9a--!)4k+=* z?kx86^gwq)7MRj__%9|{j}HHqIb`j_gAUXH0+U!J>snDw-y|k~VU07zb5!f>)6>)3 zm6{I*k==y|GSz|~x=JNHqW#xxc&)R6W8}cBv&t?3c&5p!ajgDv9naM|e3(q3vnu7l zy6l;(Z!D9eAh9VgZSS}?YRb@*!HNO;mJOF%dAT|k=H|;udp=oV?14>6*t}w5^=_LZ z!^6YCQ3h=a7&o@Jw*%b3@9xZ>PMC@VxPiKXhRUQnNEr;a;IFT#^t9D@zW4p!<2g1B zSZZ~M+%PYMFe!Qw;qh=8R-2NCh~ysOAknd0?GIhlEW}~*z5d-5QMl>QT8td5hDaS{ z!DL(pvB58+Pb>}9HwI%YUOPW67Gr0+c&xihhIY@DPOgMvphV!s4!vI8@nw-Ce<|vC>c|{V=i&CD%h$U5)D#?nX zN>_5s6UL(f>X6jwINrBdA@#gY8PW}E+9*=2FgI{NcJvLAAq)#Gy@=gF|IB*Ryr+Ic zctM1ECuu(hRQ|BVf9wE*0lKs!Tik~&peU^^OaZoY&4A+mkR;(`dlHSDhQ`6k>8oAP ztgakPadiYPdG&iRAtB+B3#jMZa|`^4_b#ATm^ z2;lvMe_2S#-^(1J4}RqF#?3AJCkNljGN^fK3d~(xGzv)lZm%y?eYZ5j*;gGba!`KQ zDUzH7I!bTFi|*Br@`S&wU!jRNNq8HQ>oC(YA6U+vGa{Q{Y}z=jxIYSr(p612J1y0W zp}O4~-K(pX&QdT!*AQz z+vDEQdHgCMwOvL(ILJ}DiXJ}|4w*kGd`p%Ke>f89 zMYaNfsAV|$sl)v4)amZ@_N>Xo*GYiV_heirG$bTfj>9D{zf?sDRNI7$dtyV1I<$f?Uwq@YBA;l1%iw%-4WL(GEWrtH5r0@0%j9A zswKU5F&@XVhyerHl|Q3Wli%^(o)oQ>!=fsDetcZ+Fr&e-N@fc>t$}S>7 zk{>P1#1vbEPb_W<4Ir7?f2SDmEl@@^sk(Uhw&_P6S|{%d9|xL)p>^*74l%d$XN_me zCyf0(F~;3A^)4I1TRAcKv0F1%Hdd+T3|T?vjYZqFE(m1$&X<7`7JSXW$~>aP%y>VR z8ld(-TMFD1V=7Y%#g(NqjJpjjU0e>4?x1(KSN7XvbW^-TUOIUb85O%Pe;W7MlQ%Rp zxHMEREiC~*QH8>6g@$fUnBD7+J|-~;wiCW4$4k;eC+S10g+^cCMd~xb{l~bDzw8gA zEmbN13oGK}_5KR)+=Lgzh>p+yJYiq!6?Bs#K7RZNypI`+*R0hz!0O4+rLS{TX#ULY8wx;0hBS^Omez(rZ3l-gHb_m%>BdEj8%@$oTWBd@ATfR0;{|IIcCwX58> z%iJ%w$@Z>QY-SuBJ^|w1o4xs@%AZgdt^fuw;lP3h0-MnGU!)h;oglOSMdi7hs-LbM z^#G>-u36_>GGb^e7fsJEYICf&vPTa_Zq5(IF%rv z>eQtr4tA6#HoQa?K2s~$tY*~Y7+DTm?LSz(e@k)K`jwmGZr6o25^Tm*Vn7%Ook09B z%OFz@JQB6M7gggD^5PRvOS0*n54jsn&@3NRlafM0%(@Eaw?M8?Aa_5a3QM2DUt4ifK_Y1pp@S@A0wQnxrJ{_LZL%OU*54~zf=4H?3Q9da)tYD z_i*5=s3-LV>~9r4l-gkdOlhkd4G9v>vNej_1DV|S;cOhexY{hVo~u#7(*jsro9LJ? zT{U`D-RH;Y0EkMl$H8(hca!0*)C1VVbv3XXI5>HK&rb(a!epVs{wtvM|AZ{o7Vcdk zeM@UWKW2V(lnGuNIqr6X1+qE7{di&NverjoC(P!P;-=wXo|Ho*Oe;A_8olSc=M5Sz z4$@7vVl?DvXx3U?c&1{GM_wEB7u4u0ft%WNoh{bCeW2XgyMu@1b{?RFD_Pznbz-o>21Td z5pem8jiIW+&M_>Kxmmrvy=`oi08nZO(>EF`V5hdu4U@g|%w9&hCFMyg<^BkvydI*i<0uiaUprHKc zO8_Hn89Gznr~H)5KXurD3@T`P9lM4uE!{q(0L#rVs~{e7qdN;-Ka z$RROiYJ4vPkv`*KLq0t@=@IS&6Xppp7$7=R^L@oO zctPJH^1=JpPu(F-?2|Mhuc!-0g0eHt@Ad>`8DxT1@P3g*vS3rRI1QqJA{{q)w7EIP>Qn*3+ zAHO}wPI8#yV8kS{ngwaCUD1mx2Wn$t#}fA-gpQ6kH?Pc0X@C~OG}NzXW=dN`rhsCUf{>z z+6Dm20LSF}4#GkC=Da2TGLD#;%uK@Q{GHj-4Qy=9WiYOmzOT(LAc*9-pEi%7pX=EXZ=J1Jr*>D=1KGo)}(q3(Sgk z?di*fmnOszP%bPv>l#mLz0V?CUYES{++P>{T;_M^UPy^8<pFS zR0E=zYP%U=(rp_%I}Ka8c}RZ+&^|Vuj;h3)1U@Z4bE#az*8l-tb|Fge+%@8w}aFSI7z!5T1{^xa4e zjhsH#GNJ;|VFPgUaAyH|IMQ_*Y|_yzPg|GvTPx|PZd^*;%<~1TlRkZ;_~83iA-rC* zNBy_w97?lYp4QV;{ueq0`ae%Q+3)bi-@Ox0fK8M~X8z7yVEjXJsCJBEf=%EY4E_dG z6CP;g-9GAn7+I2JC()}@on`n&XN;XS1yy7ASZ2OE6@Dm^)W)+K@>i)KJZIZ$ZFwR7 zqrA1yYk*GyEWURJ951mAyxTq6@E>;*w({WBysS$QBRDm?8Fz8O$LEfKGut{X&@bOPyTW z7E|eU)T6Q$aDYR~Q|`d?MsE07OroT}amXOpiRYg^fYlaab;z79KPVxmqf<2ylbx81 zmFEn%x{BkRsp=E{D-=otfL4?%aeFmx7>}$V4AB6b+*>ZYQ8Tm5-{B`So-AdKkuRc0mzk8r36On>R_&byM= zt)2^LA&XSd)CLgzA#$CU=BJ509pTBmHG61FuECdKR=iO52LE5Ic@0d~7=jX|Xq#ag4E`nXTK|d z2NW*^#zZ2grwc{D=h^lzb;c-rzuwKv!(`K@4y&(5Zvbh;X!4KD90c^~m^%P9q#Sf_ z%gdFHuutuN(XpL?>I7KTJ$iHs4EXIY4*vi*`6QobuC8UXCjxm80KJm=4!Y^Fl{{kY zV43=q+JEoo?@#I^e`jlsWa%!|rju%_=%r16pPOPSsHzSE(iraL&c&D_Cg$#!8<S5JB3h%`clXX_k*=D!J1+EoYI6H_xC3;gXi~&_}66%`?Pd22=>(Y>*AmvVXyp zMU0qT`h`4J<*Wb2S1W!zFwU3{%3I37x7D1ZQi=gQqGlSTIsbH_?1~X8q8`mU?!3T^z=qVNCCEfon+wt#laS#D4J6xO=fhpdXo71XG5rdzL263#dQ{ z#E*vnPW4S2>vz2;KV;!-{8h6O^9ayR&dzc)RRIfWTe22BBwFB_az(B2MGojc1NVqr zt2O`JP zr|DNjZRzdh6-Y2&Z1&ztFL_<~bNbp3kct$6Gj0kBR0d2=#8e$(X-T0FkOpXysExKW zh8py-z~mp}6|0Rzq5#k-bnE27Cf;~U%N44<-!_*nMygc$}{m8fkKHn83nBRRq>0FPp%jJnygH9gc#w!^!EX zS2A!1<^pov)6+LIl`97xAVxYXYTA*#G|C4Xwx5sBwPwm?%nP~>$(vEhoM_MC1wO7R z9v3AA?KabJ^>nWiualSVQkW;^cegito`c?NH|_vt`1SlyiThdeE5Fn2GMhwPh9B)R zdKlJ|(l1^!7dU(=5(0bn;p-WPswY%DDw>n-?mYydZ5l)%O~-4CY>0Cq+68vk_6flB zqX--YBteRM15UY!#D{^z9k0?873l&VObD90OaUb&yn{#JjG8f@$ex}a05}28k>zr{ zO>)f|I60=_RNsBQxmn8IejB96V^!Qz+-n7%I;}|{&O)#Bw4MKc z?l1>o%g&r(PUge~>MJ&a zIo|YCi!kGJ8@}AY-Fb4(Nltd{lN*rfG ztfXs##Tb3)G&OlAgI5+mi&pmifwic1MTD={OlQt#mEDs8<5!C(JL#SSFI7v^XJ5T> z`3TkWm@lyiwM#%lLvtyQ4O#L`-n%;Rm_55$RHKUM0#pIc-sq>?SWV1|TLM*)=C3=* zbRa^0*VT3jAh_yHNY*W%MERJ9&ju48(Xc{@$RLEe!HHh#6(}Ku=5w{~!l>aM^`fMK z4s8NvYh;e)lzZk5!BXX8F)>JcwIRobn(Hh;4#^U zn;AM{V@DBg5f!de-YBSXPd7~sFqKUoGm@KTY|5J~Vl=x-H=#`@oJIX&~Vryhgl<5%A6d?evjoip=|X6-EVaJ4#b#=m6!<;46yIx z-n>KN&ON?mYj&!tDmI;D-x_>6(smH5NdP=={0yQ`Zi1@+<){*8oK=D~Pct@U$TT}3 zHVYe&1%QD4TgLbhnT7T(iEd|e<}rXRz(q#~9$+gV^5_vOcQep+J;syeoOpmQE9g@X z4vPxj<LAnLh|vP&)?DD&Bf6UnMKI0ZhnVS2)VQvL)0t913ki9|3HLF+ zx%1*}2e;qB=ePA1Yxp;Rtvam!SOJSn7k?7ZEYQ;UGbG2Tj(?Q4e( zH2yQCHs^@ySM}Lz5DNgUr>bRFy?vrdBEyhv{CpIh-jX{Dtg-49Pv=BEN1{&yG4hzT zSmni-rU!R%yN^k3NI=A^#Yi%Y}7o zZxNoU29fk4DX1gg{?0cT%zc6WC{kNoSBNyDp!Qjk*$htd#{Kms`vC4A@_aXH;KMjl0vXXCHM-+6?Dst896 zE;1wxzRP`~`-q{xzO*rgwK1p5#=}+UVk_b{LOl%mkQzZtqW%qPK>q{4WTAP`u$i7t zI1QNfB6?L1QKJPAdn&=Ue$!e%IF(od_JrQe=G5EzX2K7eks!kV#6VF@<$=pv(-_a&?$z6# z`UU@u7Al;_!*A@V2)>2Ur9}gj#670dd@jciDoHvb&YfKICT^Qma`|<;`G>BuqJf-^ zg)xF3H{8>9WMGLa5rynI3mIk`n4U(X#fPY?y|;OCvfO&-oBE^6z~08XB!Q=`u2oDw zSDJgIR@A6!rt(G&J7KL`u<5qsKThRrITAA$v8{GT6L2 zU49&}nd;XT3C0{#Jjx>e%%R52#!2IsRo9}k_tPi|)N9`D=iA@8y=7aSc_h#uwe6C7 z!fE+Fc)i8_iz89Q%7AFq{spreROXk+n{{VspEyHtRn=P%|KqRn*NWg)c@c>>U&`#x z)RM`s;A<{qq<&zEA5uT)m$V{@LU-(K1bgd5+^xb zHahB0qDo3iz`ci&12isWB%HLG=*4N24a6yLX33DQP_121xnGb%hMq z;Upj1*Coz`CH{>%K9$161Lph)LKzS)A~7gC4M#Qg=oo?sLA4I=sDFBo4S1{aN=W#D zV}=HQ!cBdH2*E7`Sqg^FUTKv|$Y6u1CxbfyC7tpk5(o$6wuc?U|w1y9?BCkV}Eq)yV@iynk0zn1teNGyS_&Nk__ag z6fXBFf;mK}f#&s2V?q&DEP2&lb${0Hu}6eJc;^@btMUvSY=fVG2_YmzO>RzV`rXPD zkGu;l2XeS3-Gk*@CX$8ZZ|Y${td?mk=Hd{M(>7$rRt51vQEfh1@)E#9PQ0;9Vprzk}ve z+TH+%<}tIELxLL#;hfPm!R(73&SE}?lVb?fZ?lr|Z&Z5=VaQg!j1tZXW2jkm?_BQ; zmrhDDxg>*=+i#<-fm=EMLNnhtzN?brM=2w;raq>u^HR1GUymh>C>7psbTn>z$*{h{ z&qUFDUnqGzGGlw77bxI@Uhp+Li_SGRHlBKbCGl2+J5$5U4E?yZ z_a_|C9)WK-1*BTrne+QNKRMLJiL$!Pu0i1l+1s z3!?~>p(5xjxT%e2#<0zKa>}*$<($_z@&yHP2H^+c>0$=nz$od_eVUJ!E}eau>oW{y zZ`o@*T08!Gh?pnbfg!xTK=|`j5!H(~376+M!0a4jO!3X_o2*9PPELWRPzhZrP|nY9 zdNQJgY!{Age~VZ zSZ5dKSaeuFg3HT?P;>kc8CuUSA?AY}BDXc^^U2rS-U-iw@UOw~O>+zeqt3xrb`MG- z2fJ6Vu=rgOGn=ro0vRV&2@6htzAp-LUFojAU%f~Ak0)K0sa@ADiT{Lv^-Qw`a)GzM zsl7^se!$anAWYOe;kpBtNWsLEYv(eLOo znU$o;tJdASdm`ZEO|5SC24g>G+N&Du55c8j0wvu$u;{RjVDFKnGhSdau$YCf+lb5` z2>t>GIB>*aJci!iN!7yZ9o%ZabX6&% zgEq3>6kV>&;BPRN3k+)tNj`tiuRZG6`5y^2ijX^0;Ws-6lGg{|RK(ryQ#$^dGJK$| zOBQ!h33zuKE+rfnL`UKmkFFzE@-GOO=Ks12uNZ#iCp2X)k_G1xy%&tC{xEpAR;{YD zV8HT?y|hj~eOwO2eSt90Neh40fK3TbyG8aiKQ9PQ^G&}JL@Vaf?RN4%6(NK|c*3g$ zRoP2ET+u>Q+ERl?Pq3KyuYO#9@K*9L{A~_H6U7X-;&&C-znzS`o?{dmtXb9hRgVuQ zJBW;4ylnto1E;yVf~*Fmb7)9SvFn+k`d2T|d4^2LWyb1uKVb|d88{>e@4k@}qf1sA zkp81!Rf85T(C|${JgLcmoIU*E#k%e`$G^E0!XI99-1?fjxAaB8Yd{n^ICxDeqq zqAGU4eRs%x!U==WqX&H6&&Us_ev{bo_)~VnFH3mV_R8}(nL})zKt~kQ>y86U+Tw|P zQ%Cj4x_{fB8h2@AodnR+GMy>LCP)3=i?6&>@0n&*giHu165LPBd>cVuL6AFZ zk%SMs*p^Q)=#!s#>dte=hV5z;Ma9$hID|@%139FH#*52aOhxsF%AfU^!FgE?hrigj z6}S(Hppa1Nbtg?kbW|{?t>sgU%6Y2TXrRR~mM5QTvM51t$0XPLRwZFRa|(`)JGyJA zFrZ7-jalGv*~WM)^%G=dFSWRtg}0b4HiF$^rtx`z+mM;d9did@$6)6;G<2Jh|CbAF zje=7t)x+d(z0_|`qOj=x^5WS}hd(-(*sXVkcBDQ*3Q%c&QKd5j{B>Qv_ziT|A}ItfNS;IRx|HsCE(09%XHR0bBdBV~n(ailS8mw=g`-t2O4M3rOND+cEpyCkvy z|LJODdK+IkzbB8(I81#DwsIoG4wWJt+Dc2SJ5=w=-M?Ew7hOh)QDsJfCAS&}b`RaI zmV#0`(k2g3xCBDGFT0(EW@l!o@OY5Lw?ImFLxY|g2juTU(;f1Z)Qh_w0`V~mz2cqK zP~cT^;8dqyCtI0m*5F|0GUJACKk-bH1KSKdM$HtcwJw;Vlq`qS1k8R0|t8 zpmG!@(cL#!_IW1;cr?~&>p|4X?M zAbEr!PhSN*T1L*!?j{M~9iF+EWi<+I3ow(K{PK8pht4m%!XOOnJ^|K!lvFE?Sf9+L zl^Q{#8@oq_W>WEVXWQ)4`ZTa$ejTqbxw$?K6!-ULnkTXsKJ5# zWaV<(3D56xFn@bN|g8RQlUl0@D2n10}?xeOfW~oe!u}?1_9>=Ww&v$afZ4`19t7d7YR0x z3|(8462bsdD9X~xNj1sER(Hw`1pv=z50P{DK_3rpK?G?A+$uYwJ3)yp2rd~#44oN) zWYa>Uo^h;xdXHl>e8V)%=2-X1QQDROnbiG8!&^0?69$~_3F|?PNkv+}>ig84>~AX^ z31$*r&5uQ!r`my|k1moEeZAOrIXo?yX%#s_W}LDNA`@POYD}Rab1LlyWU~@b1;1uY z)8-XO95$nghf01YMvS5ADRV&D#Z)0Oi)!Blo&^3M7ob(|)>eaK`Sq;leL$a5-j_nI zr#_ySB@(4aeenb)OLig?GjWB|{5FW-J!vUODDgm;(`O_$My^jd{qLeQD41(`5q)`Z z`J#wyhtY+%!Fm10^z0xmTACm96Lq~?p{G@3d{?)dX7ImHV0gU-gcVRY{mG6Xt(Cv( z*a#*@fPAYzA=?_Wow54Rx>c{zsoj04R*890C_f~UFsC1$ z?Jl?>jHoQ* zRYE9wrAmsSu%o}H9QLg5^nk0~ufeX%kFPqbf&` zlmn)Ozu7PbB*`la@BWX}h36BB8tiUpIL)EX*0ama;%9C%rvUb1gPo9y@$?~IqY_tJ zcbNp^s{@5=d^2p@jlP8S;6zC#&WvIZ1JUpZLH+gpw~key|Qq<6A+H!KSLk!MOMbY~So@i{^ht&=_NB&3JX6=dSP z^QWa+u%6jIS$(O`6K*-G(15^r-1YFJ?AQHBa2q+iZQZ%>j}t+OCSLMgVCWvVQyq3* z5bUB9kC&n_%cYGAWT_^G+j=RD66a}y_1<9)VC+JI64U)=9^`W{z{yP6N`{`{Bb;kq zdnOnsUcC3N{N8_b!Ip&$MY@H|K&Ep;%!`y_r8^nC2W9vy`aHfP@KjCO4r1z5?kd(dFL5-Uwt>-xhg@;MxttX?@(_>^I7%Wj zj+d+GW@|bsAyWxh&HgJfg_}cP7U+YDgPm!*;LF*2aCMF;KE2lpw=*BU5yzi-J9&EQ z%YQr$jQ*Doz~ArDT;FCjMf`S;w7d$2K$f5o=O3Komka)(k9ThUy7;iCYVXrA%~KBv z;0JJ{2k}sM!BLyl7r2!;0#Ap=EA$<+p15aoHJl$21!+vj_w)1d#!>xwjmXE+V4{13 z4A&j)t$zhjuFpQ*`EJJ*v{cJ%1rtN<$544Z){_ig8BCeyjN{Q0sdl%wO1l3#^X+=# z*uhmK!8=%IH=K4|7Fym5SqRZCGR~ePKk`t}E@7)M*k=RrQRX2BtaD^_qe#$BwSEOO zSfieCYnZ3bqDtl2O;?K3AmTG|=wXPDRr9Dn@|HiWf2yiPJ>N$7kv!lg5D0%M{K}D9 zVDkGKF8?Uch!|jXjoM@}^)mf=i%^LBH_Cjku%udqI?fyg zQVaZCSAi_J&m3`in))*WMrrLu^ zmL&^W&VFFW?yGGGHdCkBl!S!i!E;FRrAe~Jg8QFX9pG!_vY918hR0bNCcpnS=}Cpj zY~8mnsFYIN)>I*G&>j8$Giw^M5uR{Aw1A^pw19t;WelQcz?!0WYC)kR_dP9M8>N}* z403LB{b#|5YfnATB;Tm+^iPs#)zoRCId7;|%m6331%NCe*!@AU>5A!aPfQl3eHX0= zvmcEQS;5Tc?#Pvm%(qw`>I&geq+!m}RXPPG)PUj$g5v3{&yj)(IN?c{ml=k*X!Jkr zix()t*6-LO#(^yCZm_Mp%JgJoHVl2;fH-qPY=;6(qC*&5q3F0Q8hp9!JXuuROG>m2541Ut9h4PjE~4US>|Eu?BsXWDj~G&7+Lja&pMi zwXeaeR%*zRCHpMIbT_GFZMc-|B5JZrO+JNYn={vgPA?*Q$3?DR&qVyLP!^m1w-G}@ z+RNBLUImO3(E57y5VLvO76YG&Tz~AH*^8rLd9ZV~$ZpOp-;qBdL4X%3!Lv`H!Vf_k zilW}Ypobv2N}{4or$gr+c@FF)YTD{I$}QySi~zM@9ObcuW|i_}N}+_AsY@tsr3)m!;EQ74 zz4I3ApAdlZ2&%y3KsPghJBU3U%0S$5FWgH!$fG=lt5#`7N{&)NI$C{{7xOUt8^<$x z7m#CECXpPD=b>o z{MQc(cMej*j@tNptt?9uDK_;>fe6Z?#Yo{9LdYUM1t#UGT64tm2mjZp9keP?y9E5H z9EE{ZF(Q?x6LLY3f<&s|tpzy;D?tFitZ_D=i!-8%k4gc*gIqq}JjVPp&K$_Xg_ghe zb4sM!Exk}_$2)d;8E{Pq8gXkdnQAGS59iEli|FLtXAWL`nZ?!mU3 z(qGc3f^3B85?-c)5qIiG*1XQDcC8(NJ zuog^Y)2U(lya3ypMq5yc8|9QqfGMj;^*6qWBcxr-V*R09rIfFK^7Gg2!Oize*~dYB zPgT5=Xe6nf?(*KydW7?qQYyH*LLp22(RgPWj|BzeZNk-Wm@ewOX#Wkt5ON5ZyoLEM zIZOMbwHguu3>c;&yR$G>R5Vmj5$Bm>S5PKkJD&l+(#v1D`xBD)=^sN($}$%34`DXK zE)Obt3VQI^Tg}6tLZa@!e}BiaEfy7CQ6|P<*!;!b1_wOrtU-p}4^Wo5|)O=J>P(kb*ml`*=GZmDlZ1dv}d;+7e_)N6^tSYYhZ*8%BP4KKInnHJF0>=Dj0IpPSKGqCpH>XZ8wV znzA__XN=D?!R_E4c%7R2;u9IJXyAVmg`$AJqVgcRL-c!z}{CIoiaDi~kEN3Y`H%$V{5Mso`ENNFLCP9YRXIWaO@t*Va z6-Yff}Er z&}PGfLZlB@|Ar1y=HVYjsL(IM3L(*s>JH}WwGR~psS~p#J!&u3taIRc`04k=n7UfY z-4Mi>1Ra+R)Qvd!?^icrm&7yy)^-rN7ancNXPH&Yq3pRs49dP!FT^^@FOG;@lW^!c zo4W7TAI5}~m_04~$!3sCVxwT#2xr_}Q@-v!b-JQg@evwY z6gXS*_u3-O)$1Hn$#1?7CjdZp8t8#Fi?*a1=*84}0yh_?>HOYS(^uK9S{3yEzoM=@ z9_rxlFNM{Tqgop(R*tZC$#w|2){%495s4^rlr35}kt_ERAtctirH~f2?s6uyR47Ne zR)i4oThFZL`Mn5a5T=N1>1Dq{ZJ$Jq)q3* z?Uno#zOxk?7F|`s74ZO6)Q`FT@qKSnX3V(cb@Y3M;D3ZhWfF|RA+8*MNy&eKGT#X$08?`zQ%i|Ti<~Ty<%mU8 zDIP6XgmVMad6i~4j`EgpVaDHgv$#g4ej9$@hBZc~**Y1ZN6gn#PIv7}-_OXi??-{n zPFs(r?J|!8bd$Cr?|_yD)bIFG!!1KQ6`e%syOjn=nCs8Ceo}u;g)v2ZM(C|U01c0Q z)va%aCY$22)rU`h?meSt^u%7D7TtsZJSb@{kn3I#OMcWO5wtl8x^-;E9)}5xd*O`4BPE@Y^h5ow*8#vRyh^8K_eNO|kWC~h65 zBp3tni|bf?ms#c9^4f8|z>pNOie5HQr}G}ghocKwU?xYYLYMU}HMuFZkivKE$%_V@OwsS{T{A7;u?OLBEhYI^ATJur%hHyd3q z0H21CK!A5K)nT&J6yN?Se|q02m%R1- zZ>V{Ic-7g_Voza>=ZN=f%=v*`#Rf*CrT_8fEm1qUXZHTW#R-L+?6(4aRlh4n1^t5olzd-5f0rQ`h{<#_)a@sCYi{+13S+#j+f!g=7C;Nx2Luq|OglVvHV zeC?lDA#L97XPXfx;w8gQz3X+hq=b}Q5Mi_7$ESsCxM8X~)x*w83O!8=z_*VhMo(m` zD}7^bZ3yCoh7TB)J>tQHQXJc_#eN)(eZc-k@g{qGhay<1*ZWHUL4}(0Y|YTnQ>|Cy zoAY)Wes8;*l)e@J1hacqD6nc+Ek!1NR`kO4#oq9E?d6};06+7jdPS@65(=)qQ^ae0 zXj@vUNv$3*3y44T-{!nz7EHHY4wzJ_td-pQ`>cr>qzM2}@p#v8mnmOvP6SXUGW=T& ztA=Qg`)dQABSU|0dvUWn*mBhn1>tQ0mxpJ1&yqeUh%o$x9kkt5;*%WR%~7!k%>yurC7~ zZu}2rMipnyHCDDMdw*WHOp0_oDT2lXzEC=E31Y_ELyc}v3R=PKt;_34$3ZZ$9dCgh|0DM`N z1hB(S!+On}AlJSgrfdPNasYX!p2lFcl{!Gn{oUpreAJ@1_VN=FtivrY;{_oh$AY58eL#n|n$6I-;RS^8*t90q*;)wou?; z=%iYkCN;S{_;NSuWa_pw+ooYKigJp&yd9*dFlMe0x`Bq*TL4sn{icgTC2lCLcMfTc zR^lyK{NaH+-*WonG(cYobFz5$G_freowmjzFIx*^!=YUXJ zBjRnU%yQoR$nlOWM1N*;E#lOOhe4K3QRj$?n|&?mqqd;hhmc!|BWK_^$Il~11P?zxGN<587gR%=O};s zfWb?7!5tiADy8nNqx@OHabt+}YN2G|aqXX;eJr`uvsc;AK@Kuif5u>tV29sK11v?d z;^PAA0L9CsNgYv$deFW4Q~&mdwm%qT;AT8?C7Lsb?sY?e>q>KgYPuq7m*9}K>3~vO z1*Z$rp|9pzZDwR>n2%2`q2cMaP7NFo^rNdXy;q6u!C8bS?GJQzW>+kmu3F~)5N+P2 zGOvA|sE5?~!1Fbd+0DuYSDeeBedUQH7^L+G0e#YiIcGHBLgrkV-0lh5pYc3mUcrzN zS?l=V%@fj&b_ajdFQP5cfm2Mt0D+-c&jokAY>$|41+^d8c&@*s2jn08{dBYc59*@l znR8p9`isWW-Qmpjl!+1(s?1DPC)?SOhZBb^x%4hv>sYN}TrThPP%B~@ z>90In;S}{as-;|aVbxMW+BrK$_N-clvjC+ibbq!=Z4rI-`fPtgxpzyG>s*00#KO7V z&3Eml-CNm;4fzH8k}3UC5p?y=Q|iImrZ>klkU3_91&vo;tf%;6k#a1-*M@4fm8elQ z=3U4$J}??ih~LYj{|ocW;5M3pENf7p*c<)FRhDE2)i6U3j_Au33gs{6&!pdk3=4v4 zVh6lmLEg72vt%3Wo!^(w9whv6D#YP+W{Y9z->i&t$ujA-@{PWptGP+Mk(Dq5YKbgO z!-3g3MMZI9uvp7`Fo(lCBG(fMTLVf7hzVyx?@ zGxPe|buhtWyZtHmT~Kq8wRCqPG2Pe1)OZ$(ksX?_aZkvzRVLfNj0GaJ&Vb9T7}1rf z;JJF+MT6{t{Bm7G$Py)RWdOHf=yzm+LkZsON+b^FnHn3DwO|ewI^Y|;%Y}Hvq2C(v z5pRk8BkmjpaZ0bQW){sWc-BT2H5K0Q5xIdU$Nd9nYSf3W*TdkwpFymjRg!K$$~`cm zPy;{=PNEuQ+NQ?mGX|6RA{(@42&n-nr9zpsb}Lp`M8n9$cnZmou&e~Z&yjwZl_Hck zT)r1&mc^lR`Ww536-_3^7J}IRhX9X4mV$Na<7jo(2Jd#vlqJ#0V3;y}G(rpbH1!D1 z7b5Xvf|@oPI?to#kW_mauz_bqZ8Gg;+8~$!$HN+i_Y8M@WB#h{es2)norg~&OxZ}@ z&zC^_c7pEA+I(<=PDH0}8GJdRY z4*u?N^qBa5JxeWwLs|>U?3Wt;Pm+S!RJZ>6LZs^3Y4%A-g(Q5&8d6YLj~mw0y{||J z0nj~}YK>)M5#aB5HE?uSOS=Qa1fx6p*Fw%^boWa;_oeUAW8mcMq@>Tl-4N@q+Kzrh zUVA7rET&HrizwEPA;(qMUOg!hQ7Zn+I%%Cm_8s0klT}FzB6$j2uIBwL$=%cR}|j6yqh&Z(ksd`%s6oQ-A_ZA2Evq!gA>`&TODt z---r=&ERv~w3nroVhZ1OokRprdI)HAKlY zXZpD#^<42EW;^Anc|Z+8^Bw7;=WmihfD#fP>`_r)Z^OX7l$|uG7rO76PdTK!IuIxO z^`ra=T654)a!_^Xfm|t;U$6ZDLmn7N9+!osgHVX!*BC!B6$E`eluM?GM@~ zc*6_7E7XcPg-fd*Hk1#0VRzmXJ_9#GLnpTg4+n7Sr}2ib1brUs2o2SU1lB&4caB!f zw8hZGC?!uG;kXj9ZCw?9N}?ZnMqfYO(h8fOt(RSR8N_Y{B-0 z7E4@=9UJ5k8&OJ#?bG@2T*$dHJ}-HZ;6HqLp<>T4n2a&MA)K$j&DDpsr67q9x)&XF zX(fK3mkAjI=y>c(MI#;UJ?4PJVgk;+e?TI{M05nfpreb9NHW(}G1tGEHx zp3`;CR!I#pdhT(Kpf=mbo@g)wk5_fYP=SKyGt%_F^rfqNM_bg#mzju}-#nZTS2- z2z;`RW|U@vYsDNp_RXaJ^BldsrmBxi1da^H#c)ZhT$`uZ(~bXkK?eKByn6H?AVh~t zkO{ZV%VKpu_zw6NK$GzPU6M7Is5@rxt|0Ef8T-)fCr7q*3RrfNn|#6qx20y;^U~A+ zJisOc-64#Z@Y+?*HBUHXXNCqTg#gVG0o*63uN? zhE87p0s_LSWv`VU728_`!2$v2$e!PQKW{kQi1a}9wPB&{o;hwN0c zY9(?>nW0|?^}@18<0X#fF*3eVS9G4yCSD>?yS}>ePF74r-%>Zr7yIWc0BA5cPQ~ah zuag~F91O4v31R>gzqcO=VyfFLT-d?8bSU9X2R9!JEdT#4!2W<1T->Y{9(3qnsNONY z(|ax4_lxiKLBX$w2?JXrBJD)wmND4g<9>O1pYz@Nl_C7>jY)5oMp}zgEDk8ZO&HuW zaLPthO`L(m@t~I-%J~ajd@&^0_`})B#0#zn%IZV#mN7_Eu9Jna$kc}?mK2RMrFiy5 zrR3WF7dI5e8mJ$ozqo)a4U}Mk{;C}S=&$v!_k+8)INs+XD*)#OVj-t=?hF5-_Iq#MUtQ7aRc*%j0jjIY*%qn2 zx^tQV_=O%*wVk`BGW)j#eognPa9OU~mW(0cZEAlp4Pt=y0nmdA*P98fWB6}-ylj2N z@Y7B2%biIR#e=AyA=6J=I^=f6C zC5tyafJGO+gbjn_h-kSFVh|1>h*>I`AoH$*9y=Fyr5(8`5ES1ay|@xVkFXDw6a<@X zSt77rCRA-;-lt@#mhE+2e%bKl=lN|?tn11nCy5?ep3Jb(evE@4npLWu>`%(59n){KY6 zaQxo{%+RY-%t&Rg5Rv;)&nG2Ww z=OmLb z0^$@O^l8;nkxNnv%xQ&2&{CY`#VEav-f93pyNf`w zY{K)YC7lb8-+^(!d1jdnB}Nu)3)ii}Cjfh^RJ_jmoHO$57}w*p9npz?ZRkf&f_!QT z_cM?#KByLvjJ&$zmzAL}<-QUF5W+y=M?~RKQuq@e#rKq_xauc4sYBjdqyZF^>zJ1Q z$_oIzt;x~}S3ON)Y1)W>%q7V~3FoK4|M%vdXLLAq_rxz$eZ`lY(+UvqXMi)&snl`^ F`yXB4;j91v literal 0 HcmV?d00001 diff --git a/aloha_ws/src/moveit_visual_tools/resources/demo_mesh.stl b/aloha_ws/src/moveit_visual_tools/resources/demo_mesh.stl new file mode 100644 index 0000000000000000000000000000000000000000..eff8efb072cea6829879101dcf9521ec43b32b26 GIT binary patch literal 52684 zcmb?^WpotD_jcn>LP7`ug1bwI%uIFR!QBZg?wUXdkYs{};BJe%1eeH6cP|8Ych?0L zcYmv9Qt(vR{hj}ZHy?89Ox>rhmCfyYds^2FEcpNPFJD?kIr+V#Z{>szMqsBjMloe1 zxppXt%)9Y~@b;N?$B4H52ASYLk6dA&uV$YsN;hMgZ=)R@jZLW)%6qEbJ4UKjOs8_h zsT@hRzj{!>@|Q5uykA$vi#{4)Yyac8xZsw3TwITHh{{ z{1-E-oT?ma=V5Jk_IXVI`(#SPOsb-^d{)NydY(^ui}imvq!LsdU5`mfn1m>;qJ%aM z_PuBOrZ;Qf$)UN>mLtw3T_#U_FFN$t%dBvMSwT?>u9#&Bty)w&^_SJ)W1V4Cj~P{F zhA0^o-Pb8&k|V?-LoVO@dXROn4% zH%0j|t@NJTUZ3=KjZ&uB2+ianb)(eCLspx89Jz zM%SDkG_uU?9AE3!?f4>WzCQB!L~7Q#?bzYO%)jt`1bJCWQ3JZaO-9TLIUGOv*qV5p zeYm=M{1`z>6n#O~x4WE-*wtlGT$#C5(82hp4^>OVCXdkIH~J;sT6xw(CuZ?Beqs4b5BJK}2QzWzH8 z{o{{R=zrOR^M6J+s_w7fGEu6@^jD_dvU zruGLNxufhTt)dj(f69I;ya2sWw61~nK{V>5C}#hS_F8?WsxDuL8My8cjaFBbcBP(J zsu;m^)T(Ly9bHNQVsdqG%IDqYV}1 zpZ6KagdI23g$sL9v^%2F(tLe5{-XH)+i^f`(&3z)v>vgG@V4K_eUA2tV+%~8olNfp z(WX;9HGx1X{`oJ2=iFfG=Q>qww62(1?v&Lr?%Ml-wuT=EINIe?#aoJO8s^{~zA4(G zD2WX-(y3L_8-0tGHV)jZN~$tRO4jz#&X}4ysEUSqXf=l0E$x}Kvkwo}6s2Uf9qhlN zG|FIQbGN6x4eJ^3PnZ^y>(e?eN!*(VZMgnp9ha8373ImQ9>(X|clEa~7ML#$8m95F zz9%fTys8}Tkljz|W*=-0b$s6G%PhN7Ww8SZTj|RdIcz1YP%vko0N&1edLYwdWJblP z@A6#jnbvHa-D`1-+jL|(&ArtgyAP8pbN#{E$giaxHhdpi-Jt>J4A0VRANI$23g$pxV9UQc@D)-01g+6g;6P$hW-1W;I;?R1wv0OzS zVhwUX|F={q30hZ{iYp=X5UZ2V-G5uPo*joWJ)D2enHYEEgPqT6_a@B=T2!omK9Ykz zO7P%wiQ9_LCBbjyvJNEsz3X=V`ID8^90^o<*I`r+xMD^XkvgjDE3lmsPIlw)_=?&gS~ z8tzqbMWIf(<`t#ueIJL`k;SS~v^#ne{iG<ooio$IwH7m56Kl2yr#RJ|T%2n}tOGHZxDWX1!nG+&h4Tj` zl%-1M!S#s39S40bKL5GSIq`3rBR@p{l*-`~p$8ue=a~4M&$s_ZpwAKg6XDPU_db+F z=;1s$otWSCKm?AYD62Aea%kQC>~|ISUEH;CZ7NE@_XdXBxy^coyFYYX6F6Vx^>O>k zDL&ijh@_A`I7{%{#gQbzl;L zvmMVqJQKMdPG9gwN~!pnfip&U@Fzk+*##OAV(vQkL(WaiS3G4zoAVRz<)8Bv%s*F@O@YJdx{#5&E#GE>dm^q{+#eOC{hCPQ!i6p5&F4M@ z&n4W04lF5O7~qn zeQ*UUO6rJIH0b(TbwF4{1EoSqaOOK@N|q`LPaKp4{jVrP0}49syR%Q__rW!Ya)ai613VwO19`H?f; zjyv_TsS6~{(I039Hv3}Y9lL1Ji&b``KX6AyOUrs-4KwNKdHI@}>a ztUYy~-G06AW41mve)6z4|Fbhay7ZmOdyY>t?Bo7;>7Y7w`H20B`#Zu%I#l?OeX}EG z`C;)e%V+;EdOYn&i3g6$5%C{w_KjbkG9FcHr}yoA>Vw)OZD)g1#d$om_d0sRG49)m zr|mTk+;+4du+Mw2&)GMx4_$5Lynm?iZ#zl|)PwU7Xhq4ARM;|oUwvAlW_$I)um z)1j1)C8#+1$le$Bhc!Pt5Pn}?+iSG_%rq~OC!IxcJxQu#KZCO z5Us0vXydoMcFg>W`e+v;w_9IHVePC!O|w3P+ssi?-~)7;2mv8SRj&Y<@4TdC(IF zm5`9n&|gxOjaz0p*0H-WrpzNrb8bx=z*)b07p2M~FEldvM-@VU{+N{?>bB?rxwYDS71T zMxA1H9o7{#Nogx#1r{l)vEDwva-^X>eXgNRI70Ye)_j$2vafKhVa$DA+~E)5XHF%? zMShBs?n`BJ4<8HXD_UBtU?<|v_@k0`DPN1(Q1 zUj5F4Q}aLGBdmy2ye)jg>7tC9#d_}PntJ!Hl(R(mAJI+(|Ih!hhqnL$`+OA1A2anf9(-D`zYYv3qC-c~*POx{~98Mq1E25($y!|ZNn`G_x%puh; z9vSa=n5P%NuSS>aK{-{DUs@8_?W==o{^eBU<-M;Ce&?8eQ9=B~88~~5?Z~2}l=ImpvyDG}2++SDRj2 z>~pU=^bnTjBTf4B)w2K3t4xo-8!R+EWw)_wKMEPVNBEZWK%WcGDoW`l7W;?yd&t6f zk&<;eV%ymlmN_qFODjr?cDni2mnZ7qgS$C27aDSe@B!y7e4{9@G7qrqr<>KoHwQcP zK-qiSZ4gmGo7MxbhLYSt!3n| zSk!@ddnMkIDdLTT>SNZwESWpLci2s!6(zh}R(r&TVa9^@(;fae&}Wy$)p+Tk5@^nE znSHw@JDZh?;+AKry~gC$M&~(M9M&zg#%8Iv=&l1%$Yr;sX1j-u_JXuWE#VDrGKA;l zGxkHHx9Z_N2Rjg_E?%|lKkHl{f{ODHo>i1qbt>AYTOy71Z=Y+(owo%KZmXGTcUYp= z@oy}H%3iSi^`L}Np-6Ryb;Y=xO7P=tA+4f(JT=iWcuOH;>7Y6e4+<-C9>P8xA?&6o zPbc_W!rZNfxpAz+qG&5&A6csR>(f}qv0Y(y*i*-f5*FoD0xk1MALMUY)7Vu%I3U8I zhp-8!5;L5)g-(hxt88)0fttZa_#bN=`<1XsN?ZNXe2@O8rTX6=H8p#rLvt}lIYP`a z&O=zlu~YNAkK+&R!`ot=^S0RSIN`5I5%%!5nEAXd{PYvyXz(7f!|^uq5We{xA!JX9 z{#S&Ol1i*vWNSyJjm1DkC?Ze_%4U^+noo|76F1z8WZPB-&* z7Qb4Q9KS;QRM{6rMEvTPty(WyWFF1pSJ{%|SAvT3Xp=FFi1^jAexbw}zY1XSD>sN= zU0yqci1?MT0+%Z5xq-wPzdA5_uH_SpUp-EaUkSUX)Z>>_TbIl6E1?HROei>-i1?L| zR#8ShsknP0#IH~i;Wb$*|Nb8n@3Q#S-Q@U{7z=rz6@HIj-O<|;5x=^3uOkuht3*?0 z;*4L-?v>Yakj1b3lH*sdIfICZU)|f;o{0EW@78UIGk)d0aE9KRBFM@e|Q zZR-BS8NX`9;#XM)Kh|$0$FD>Wr@DT=D-rRlJsJ8EXZ)&?f3)Qgi(kbh$FE$DwI(8d zB_!nY_?J{;S^Vk@i(d^&j$aAQxkZJxye&pjl*(f|+b6L2RcvznO4x@>)hMVJ5%DX5 zW)b1lW$nFK{K}LZzY~wdC68Y-u~&-tm9R-lTL~+$CqLEY_?55; zN1zpcXc48HIpbF+S^O#wi(dsL$FJ(`@2iRUmEgy3WWRW%j9-ay`S*dwU7Bkme)Wroe$&Go z%;HzmljBz-w)fLS{A#u9KuyH2dNKW+@v9-T*4VDH_*ISM_|<}Ttu+z95>#BOlG!_H z&iK{U;ri~iEPgdLIesM&oN5w7i};n0kY(148B{fn#jj{`{0gZM{foy`7QZ^h;#Z!@ z@hhQWO3lScin5NyuL4>8Dm*!URdaTzCgNAZ(pD`8!Zxcg(cCgNAZ(uz{9y&S(18gc|$_ZQ+Ui(e(O_|=i* z_!Y`7B$V~&oFaZDB;h=+<{hVr_|^K+qcvyzYKdz{dv+GTs*oJNDx}9~B7XJ9t7uKc zuLPRqu$0Yek7Dtw3CZ!RvR5Z*B7P;%oL}hp$(l2MbjU_p|s_WODrKu{v54@hd^ac?i!c%7fY!?NeF&%Kx1+ekHhb zD#3%>YDCCn%^APSUhaY=fyJ+?CdaSDxSUGx<82|WqTFWjs}L5yT9h2W5>`xUU12vx zxg0IWuh3RPLRqSA8`D^#*sicTIesN9%BcjJw}0^{#^P7oA$}!n!l}dz=WU_W@9`^P zla#jlrTIn{zY2u-m6)R(A?6t8AuQt9sl_eB5z7$sha?OSY-LE@wju0o= zzw+P+2M;^}#JTjJ2#4n4#Nkvz+y6v3c%baa?_c$BAW#z2>0c2JsZd*#@V^mAC3Ygz zNl{*_op_Bq6*caxrc&c|?YuIO)hKGb-kn#RvRYfsS>xVh{&ZgBPI--ca{Ztt>e^YY zt;Xx!If7NVYR($>bm_YA8h5&#)wrh|S5V13l55-vuW{!+toBvob?qF@Y9BRT@19(P zr#WlfgGQ9)HSQ{}apzR5T2d2r?E=k55)vxPm5>l#wg&5d35wtNc6fna=#dYur;xB77ht z(t0^++^MK>7hdCNR>S+#H^S12GQ5JuYuqWXapz+Rn;?R>g$)(u+$=|pyDDnjMGvPE z{@^1COS7kztj3*{c@tjajy>qLx`N&r#z}c#S(+m-9ed2}}R1aZkv@YTT3S z5;b1ejy~YLe<6}<+)cd39T9aH6{`)`c)fday`nDFxbK<3YTWr)!itFIp9|kOHXRz3 z?#Z5AI9}t9W2HpPh~D3F#Pb^WIv_ho3vbHSGj?HSYer#vNyPa<#0+>)n&LB#N5t=%$JL%3&uiR~3emswV6sJV`8Wc1A0a!A#OZ!MeU0KhxR;3iihnNl z6({1w=JtM7S&ci6mC_0_g4MXk^BQ;bGoob=@)~!ynr&8h7N5^H`p{e>!X2jZUn_9rq>VAxmXtHST+F`Wn2( z9mm4;fjTkkdOK>|bzb9+7DYeHYp{m1#@)!sYTS_utsw6Vg;9B|gS8;x0w zJ5r&a(Kjsr%30%X@EUidLi?cISSAOnaUZ|qhR$o;kqUi)R#%kt<*c-SrPW&FnyRVU z=uu?XmF)V((wWrSrYO?v;14bJ-Cbm4uPD-H#8>U~NKf|mi;felbot;BTJgy(1F#1X zg{F=rZ5kZWN><&&a#*I%wbIA_eYEw>K9~@JRPB0dTB&3J5mYNZSB9<%2T zCW+@aPaq9@jnSSx&aGZEPasueXJ|{i?k1y~O(YqMq|#;=@nVRmJyx3GL3M3I(C9>@ z!f}zGqO3h@r3tNbYvT+b6C#iwqS+qx#Y#uKt79+wy<_5xMia^7dG6ZM_+2Dzh8W_P z;jV4Z$X%pTqZrbw*S+{oUAZ1@KU!({(-HR8SH_qSfmE^{-|w+EGd*~2zt}6S8GBGd z9Em+k4YJatvNNT-j`4X7{iQk|k{t0=aU zwQaFutu*@1C=ID%(oZAN`3{h+A<-!i{;8tuYqnaAXFaQFXe+eOLVFO&d1}A5^}z(D z$C1TuWZQYGad^im8}=Y#XVGcIYi*KtbKqEpDBp0GdMvxYv8Sh(fmG;^?5n1dx?6N@ zTue=d=(jd2X&&O^-~Ni#sKM;M@ME;*)5#?HAa9YGB!5j8%cK1khUlLq16lIrU}D4T zR-=5I=_F&O2=ZW`>^1Zzt6p}ku9e!9UwblM=7Cb7MHFQ!t7dv~HkZlgoYg=-BR^TH z+xiePE_%1^uPIg|N0Aw1Uc+))+$lm(CmC_Wqayiod53xMKC6L}AOc5Xt9Bfrg>ROk z9jo5Z(9dQ1&LS6=|3zASk0Ch66s1e%5;P)$(nVuFYB+xoEh9RAd8!WmZX@YGb}`%< zv*#+)pOZp2CJ{s|=s1eJip!=peh|(4QRScV^y}v;YC^xN5(24K92-Mc_Q|RaEH{NA zUKYtgtpSOo$^2#p%8opcx1yvSU6IcGkcPggp4z~1Tdo*E%71WEGq;~fWFB=|_M@?9 zhm#7W4(aGMoEfs_RWihD=iW!sqWiWc(#$^kGLwfI&(4hZwX5j!J9wxW9)~z#v zeD0P(J#=Cd$z&T&UgULGL$hol*WyNzaZ6m(J5#nZM6)?9sdbr`TJDcbw9J5LGN|qs z@-}QMX|j1Xd9dOnnfhZr!I2cD?3)I(K>dvBk!QImQXx8S)(q0w<0vWTzlHJG7v7H6 zPdl9qy^>Bs%Idd(RA);Hiq!Z^&C*C)+Q5+W~l%?u>%0!$0 zGsa^2R-WRxh~8IYA$j*T6=4|(jEC2{Ao{FHPSPMO6-9Xwfg`c_Ue;1{^OlkJB|gCU)0yOYkeT$|u$jn+ozrTlQ_U4^KU$5Vs39V7Bt`M) z`jOO|S3J=#AItuPaZy5`!*7sD> z_kMPDX{~622;5y2B{oeI$yD2)UiwKK!qt_n?~-|%cM z@woA27KsbiwZ^q#$n)6A#CyzJ?Nt&#qjEPaMWZrh*GtD)jL%I+k^vss^ma3%3G(=n zJ&I(@?5bA^oXikUdU?F`QWt0uN#~){Pq(W^`LbmTds6<=u zscQJWe5xT8-nHcW)%c!u>Dq$c23dAS!?6%8BW{n)M|0Kl&|*8*m+mEa7m*QfyG7FY zy%mV%$TN}zcq7@~~ zEs}btwvnBF>omMG;Q4`jKD#YdjiAE0Q*gUb!_qw~?QGlgT$~B+b^ixjMZ44m(PM z2pmaKzTE0V&+I;>?k@Yc1;@gbDkEa^_oeFQlj_um7cEGICxeW5)^8;JkYTHN`NWnw z-fK~F^roUb3LHUK-W_X66W&`w;Oz|2>RkjWoX z>BT}f5u`%2q7-UVTwPW1jM;ZVIBj2oJ$-wYSzGgLGeIhpP*HN|!|9HlA$E`bQ7Wz| zq(UnwO3|=>YGTi>_Kw~oq`MFDL$spYj44dpo_L~FpI=kbqu!*kVV)CDG;4Ar_rmYtwgodz7hh*MSjeKxew|Ejw^KW z`Pvc!sqjQ)?}7Z1q(AEDPT5Nuq_Y-JYLu3J+aBqs=Umg8TKA?ga9li{Wgd@(pE2ws5nH`z zQuVzLshfKT8M`HhEbUsIROxw(Y|h4S0Jo}~)<@m$WHN5|F*sE_e^&<;qUlvavK}?t zy074fqWkJ-qv|HcAKPIyum|r~cz;looV9CcJm_9+kBmSnKGK!M!J3oDjByur9(C^) z+}k15xWKI%mmR5i`}JQ&GGfi~%Jw~(UYnbIwi!U>P@X%6WyQvVH_dV$ecIX^j!I(N!M?#WMaVpoEB4lw?~)aa(aJ;VDGOdJG%T(B8n^WLw2- zg?>g4;z)|J`tu|G!b*Sp%*x#*sZevY+b`>;lnN!`Z86_i9pt(b4xfJ++*gtcsd!t= zV?{X>k*IHdFy7X?&p-obHB#XUX7BxQ|Dt7?l$(wz-@s@z(?>6q##Qa!X9LOJ!duU` z$3wkzWFwh2J+H0~byFL)U&DN!`yhLYKWhMee>zUbI0MEO5Y7Bix0pWWxP?}_Q^LR= zXYp5YU_Es2qMxB^3xLzXH$C&;IY^1yp5-^XuTn_;iFFO+j#iNMxbmTeas7C8%j&T%21dsompqfRrY!iwY*nuN}D?{Yo+}b!;@S5s3 zCsrC4sh&LW)jdnQsr5Q%W{6itI>YT@3)9)qFU1GwKVELt5?kg`W51d7+RqkitAhE{*IhGf8UwF~**CN5fzDB8 zJ^O%KS`2#viil-N{`&M*9kgpI_eZMZ!;K%q0`2?yMX4ivEqeTj1LigEUTW`9i@wxo zZx1T&sp3d11G{?{W6Yz9_Le(eO9-TDcFUv>Ui-kl`amAW!?#!+qg4BA_D8P^QS3p4 zEY*v3d5!g>r`U>+2DClfUuTbcYx8-MS4Fgp*cn;H_@P>~{l$GLYKwNeeaS~Zu{@Q& zKA3C%W@m20-1WJAbE`%a$3nD>m_59)9=x@md7(B^k_yp%!~JyC?}I(QALlW?Mq6Xw zf^#{l!G>uRN07P$xy1JI`tyN&4HowMS2aK0x1$cem)}=H(+_T5+FG zw>2>;Y;(07n(sl83i+M!HR+QoeYLoS%$KT{t64w#R}|?~&P#ol z5TIvXKZ`WJl}BAZ*radUvXiXM$e&c-atSqxZJcju@pg(NJECQ&tf$);`7T6RvaiZS zkqRZ0rP}0^%a~=%vuveJC`y7bDxJ;^BTEUO|y9h zHIRIce#SY?o)xY2GN!hVOT4_VAw}8IgEC@8=XH8O=h3F3FNRRG38EkL@Y6d*|82?Z znC(&RC+cNK-8QcS!g%8K!BVtn1{LQh&Pv(mO;WKnSZI&^L(CR}`z!8OxHc7K@$?Ym^QSCY z?X8QY-4fBbudve~F;us^wTSm07AEZ_xPQo+`+AMmV}qucZzc?<=qdDvjL4HSLLXKs zon?2{Fv;hLmTeW-eT5cKHj`Z5MMI-NOjeCaB zem-+llnNz5-iq@1eY93(wF{l)--2GNn_uraJH47di?@nAP(t>u$(6IUsfYW}BCSuT zI2KBRyxBXD|5>S(E2yd-2_vPMIHr!b{xJM2>9i}qioDrPrr;B;?u^}P)tLh%sZbJ} z(=6_jQ`gqM-$yDXMo=6VsbmlG7z3MKj@pTOPcmx!$@ud@%e1W6L&;B z*|^ul5xfV{GJ+NvllYeXB2Lu9Sg+lJ#J zKlvHtG1q`v(~5O4`igP!a~XSuxx6>j#P;Uu9Lj+1*Z2MrWR+u3y=mF6|UAjfDstiG3gJ5vO_|aibkpSSXHl zYHeAvzkho5+LL_hpaCPu)WfOO#r<+I9^+;usRK3?qZ9uwNwEjV#gSO{PKE@v)a&!= zvz4tVj;ojTBP08!QUmMcPr>87^{6`Z>;yHsdKWsQzm+`P@sLD!%p*}DT2VG<>7Yg! z18KT5b0qtqMbXlVvcIy0_Sl_S_sde6Vs;zGzvamGo4Wn!0G|okv=aMN%=km};+|v3 z;NcInxFYcMNuO1wWD3q)wRX1 z%0Pn;O(F}uPuP1+*-C1Riy`{pxdD}KZYP<8Vu)H{vc1O>o>}ueQ!DzhX)|+`H`(Y3 z*U6+q&@1!iVOt66glOi$4((|7SJiDb)eIDSkROhuC@)rp({^DKZN(ZEQxSoAmak?!~?02ANwWDCnM;{lNGhl5$&YRRh*TGW*$7+ zjyCJ~+@8=TJ;k0ZZ6}aH>ug%u=i4NmSiHn7f_Bf`RIAW2hl&W~Aaq zKCTiEq(XkoA1hPQ9_!Ls{?LLb&OY>7Hq%s=p&4s=@XuyuE3c=`>5zod_C_u_D0&Jl zE&HQW{5gt8_>4_aW->pd@{6`_%-4*fytq;k&Av@! z&PqFk)v;#@Y%TFXw2b(=wIz)Rbkk~8$S(0fTj82#=V1C0YGQa$f4FoMMJr+slZ+VM@|oJ{-6M1P^IfG?E7>P}Dw#R&oo(@D?vF!-a?sCt zcbit;Z7%UZ4=NI(TKz=Q%d3^`{fkKHIVVbm(y}wESr+>F{_@1q2U<%zgXCw4htmF{ z>UyKO?ZT+uk{;-PNhjsf_4_1i?VaZ6?vWHdg*&y3DBd@_+WSp*%N&+-fHMOTI1>9M zf$oRZWA)eDkB;jnA#j$+KCfD>E$ywWG(S}`N$UgmM9I?1GB!J0CWhM6*DJ2#`oPR? zTr2GNaULYG?B|fg5$9xoNIsCPdp=KjHIu!%Evk07ln;m5bn?6!v1=qc@V)?&EN}V(WSPv&<1HQsKTU zYfcVtBIk>jFki}Tm7eNjUY|U7tB=c~7CCm(|JLFNit7$Fm(Rf-Gg{ETyEBtkcUbQvD-=e_faG%?>kcJ-L4d;^O}WL+PL08%h}gK(%FZ$!m~?JdVJfWmZ_eL zOzSzAVzw&slcm~GO;@kX`C%y-GeD9GZv!%7ex<>5^RoTqdZ~>n%8opcH+w!bty?Y}kWYFT-!L+N`gxs_mA-?xInf1L?4$1!*jq zV!^yeq(U@%3+?(GbmNq!^tj7IDFYMliHK(3)eiTkmkzI2Yn`jEVh`ST=eS0ZcX?B( zZeIL`JZa1|^=j}0HFtxK6npTtjJH}vnP3#7vsPuH1FFe4eMI0$iZY~6dfIylS<$!wT^>7uT&rHeQnEuHHH>AAlTVK=jRtwClLAJQ%Gcgo z%D?6@vOgx~q0^iFW47mNNU;YIWrmF>bp2>jJrj@CmTneCKQ?eB-#dh=*t2z7G%4oY zj1*q&slJaLPx@3^LblA|-{{K(M$pll=UcAExl58@9SfqF=9PO;+wNMVwsBYDVXixl zRBf3^P8ZLu%GPZWKaB3kHk$-rnWQ2WT2w|XpO=QFDgN5jW_}0yc;5taF=?Xhb`Xnf zg5A&}in8HwYZ`s8todq#ERuB*jXJUWm3J#z{pB(L`xmlF2&6(wv#4|FP+GCr-a#k`e{Ek-9l1Cjr zaSS;e8LKU;l}GI{$VxUg*{3}_#(iG=WCfbKM=Gtu;{f(6kH0Fa-k%spZY}oLUM z2_kSLMY(sh3~lAo#r~BYZdmPv{^)*u90`2A%ii=cm+DBrud3$?d%~@RP853(A^Y5I zR2w=Wd!vN4ewn3Ng8a~%ELUyuFj^vdpq9JFR2A#huo~>q=P_iJzo8Ww&cD&u9bJLu zJdw>F)xjj`ffAynSiFU}a$kJZF=G|~0>OKE840aD34YHU#G(sW0) zq+1;%TcLdtQ-zV}kk?v|CVXBkxm1GgsE|Ql(_EDhNG01UEHu{Qkv78o^`%wHVZpo= zIWK%>`2`mDMw2YE%+i_gv$9@<2L!u>PhuFkh&#h9HH;#+^ST4@?nf5vP6x*10vL2X`B=azN zM$#ki)7vlR`lMlgE9P9vd6w&jN6?@=1GTXSQtOz3jZ~O<$}-OCMbgui*K0m4)@qm| zif9=TuqT`j8`DxdpO{BSsgMVv+4tH`5!7#3Q?2;Dj1mH=lJH8*s%w*KXD{Qd6s23UA}#c9<+R@hE$kOFC&JBkDxi;ys~$i z&`n2qFQKH`1-9(TF=BFY*rpM$kdT(bLdzYNUbQ~A+o@E~WCl4@B zN*75VrT(sAbQvSJR}SCS3ck*z4m-jlUi>fH{z#UYa9F!uH(m>==&2t0vYlW|xBlLd zKR;)ctX1mR*LgLj`_6b8IM$&Bi?zCGSRPri4Jq=-rjET~J9jFQ-dwm}Lj>|b-s}zh z1LAFkZw#bwipT4y2cj{*jJ;Lm=3&dSx5a49vo)m5GepSQZ8X?ysb9Pgt=jRNj#MZK z>cnc1hkDqDpXyD0@1EDO2ko;WV_vP^_gv~^cb>6TJbPjD;sFtKK$5GDR2XlU5mRfg zv5YGePWKeAs3R5Ty2^+V#}C*JJbs`~wskj9s*5jMX^U^?P%Cd>dB~mzwIWAc)!XHG zULXIZdw#N2z4Q`~!t8bMEd}>o@8zNrjTg z{+KjxsA<8?jbv0_tARa;7`5l4wx@!NTHyxIr{;gbmkyJ>`y&mcTB9xdNhKqKMin%F zET_AMcLgerzK{}bv0+YUPkHh$L+$3NQG!cF$!0=7yOcjo@~`Y$_vMw zZ5)Z+miD`w_b>cG`u~WK^gte{6RQl%ZjKL&ILh7`94SdvIm0Q_hAJ-V=Ig7;<16id zT7gwBZ}PG2q@Lx!s zY$j&elS}_wADtNJ8(|<-v;K9=^-_%{u~$a_ZywSQC9+*gS-kP59U^O)YFj5Z;* zdg9!JRs*RJE&HSWRad=q?gOUB{n@;NN(8Joka?WwyiGs&$s9kY<)d>?h@t>F9t~ntT8||`_21G3-qOX zwkIwaFjPXI9*95LQgeDtryJan(tyCf<^vpoGZR%2D`*5t1XGi{jHgSnNM@y(vyeYYAV zf?k+Yeavgd3?a-Bm-BtTzOfp$cPzGhml}|WJ(z)udA*8qtew@cCe73;7-IrT`T6O6 z!?Nkh9yb-Ud}Yn|bYgqqyy@EPslF!6VnhUvq$t^0<<6_&fm(={w+*=?0yC1>_rW4o z<8GSu+5y)ACL9YTnRe1w?={s;Z=5P4`zAcGt=0IFaTdAzc!n7*iWzj6-O6(AZdi?m zBrUnO>_H;>1FefzXHhj)>D{&HDpT)bk0k3N0xhj5Z`rrgkIxhAaaVJg(I039S@Q+6 ztVYht{k6c_rKjI|nt*N!5&kJL0{584WC z$nwbIe2k{kPU#c2=A`J4{6DJbO~bS3Ra57dGJzGPLF&rJI=!SZYsoBiw8ck%Thc>c zsAg0VEqicxt+YmuPQ{JUJF-hWvNx@wk4f*QPfMFaMShAhCTA%_+fv@>)qlA(F7iV( zdv8=FPb1YQT@S3}N71@CZi?L6J6pA{!N&q_XR}hwV(amwjQ%01An82NTgALJRxxsJ zyH@?~H^1u_o-|f z!rrivaPk;ID$Mbc^ZMMUpG`c_upV7qGC$4VVY{|(jJs+o;ijS_m`BK-E)5uMe%h1L z3QB1SfmCh-nrU6`WmRq6c|`cJ{wmR4yepmd?1iK`@<86s?;kJTh0}%2y;bY&bU(F4 zozNoe*&Ur_%icbe7WSVk*$OS)v&0zfNbX$fv65MtR4#7|nddzVriI3?kfg$KWm|E% zrfslNqj7VBIf5t^O3QXQ%QkbxOnvC$XXjMZ5G6qg6=hK*wKYgV>?WC{kfIX5}U4v^ljhs6$rp9N`Qri`xEp7pRtT8$|;%-_~mW{hf53 zBj<0*dx=NnpQdjU29xX4tTg7UrqyNn#(X^^T1GrR*WKK$@}FwMw;>d{qf{@7Ez?$& zO07;S$yY7C+RRpZT1_>o-YAMah(Lad5@#E0ioNzlU3^#89Qjo&sc5_EWl)WYT=Tfr zb4}i-V%2J&2UF}p#JgEmt?iMtYCxDbL-4<6Y!*Icv$20b@t?Lr31z9SjBRSGn`^RK z=8sTG4;&ZyIe8p!(wy+Qg*nmfV*^cunIMr&q*fE?uo--X5q*zfn z+hxS~UisqoFC9+nJR7ZI9wJg9KSe3oDt&y79Q|nCgGVI^qWFC<&t3&d_UVV%!BQJ&?hjpj0Rc z=EEw=?6$dVTgQddoDE7!`xV+sMr1rS&(!Z#7`<~~po&yzX&J$-b)wBq_1mFAbl?7m zW-&)`9=q*M19LZfRu5Cg?HTC!ob4oAp?yr3XIXZw%%tY*!k?0vBD`(hnLeneO`R$B zAOiV0Jvf%lLY2)%oZ+}WJ_gLO|61o9b1GR691GF19t;r0dC(L*aNOSz%Jbw^i&Q8T zt~93}*BaLk`u8udDW)tZnB776vy3mY^Oaid9QRLxKvNrIz$AHi)i+Srdp>1 zo}ZR zUu?1Y_@UXPZ%ZphD%7LuT$PZG@5xqHt-x{(>P49vv?xf76IO~nh>$&)c%k>MnQWJ? z(?Grf;Qb1311uxu`CQxCIoWC0l9m*2NO-E@>SB?iX9<86jyNIHfnCZky}{Q|C{SS+lTmj5?#%I!o@k zp;BfIW}(PD%7i{w+uJu-ijM3mAu!(q(X7_;=>YOzh<9S^nN~V@R~6D~jiPtm!)_TM zmCU0k`<3QWy+_(gd0Ht_Av&&cGqV2HU9DX)?(?e^W~oOXMB7I-9!9YT^9yh!W~=zE zYHv?}d+I@hrQ8q9_mB~Z$L6Tz9{y?1@Mx%nKz{i2ovjboM{4h7SL`d=cBj~b`3xwb zqFg+?M;(75gFV0ZAc~S;=722K{+Bz{vMc@Umo5&XNQHSBGU9%%oodHXXKgu;4wCXq zFsDRDd@nv%J$LAemcC1fWL@+JW)UdLru_5Oh)$=pU49{w=4dM!vG22$?yuCu5?XPC zKjx}nwjpMlDoVkdR$6gk8e+OJH4%F-cMvm@6s0Ar-thV^8gmL+#mIa1w%%0}EtS$2Gov0T zyDU|)i&k2qtsD6^!E8b*lnOJe*!$sjTUkbbO|ot5ITQ9ELe9VPXkw+QyUZg!d*n<+ z4N-Q~iDes3x6(dW2a#Oc9>-%3j*I+Q{rUaobY}X~`hy-v)yWk@$%FM7^n)$jReZ0r zoRQ*|buK9ykjjYg2{HqQ#XGhIDnMV7qM+xHr797b-9 zaMyph&n7q$TeSx&6YKL}gS=X%Bh`<4)ybi(9{RI%3sWFWCyJ6g?QZMp-5!!un9qU| zD#}Cedh}DX{D!f3fgSS;B!1Fc1as~Wp(Q&k))!9NZ$<=G3HdDwCG%>!>)8u_WY4@@ zCcUzyxi#H9@0oHoNVX|h}6aS9IdGXlpk}!*0-%hct+V7$F zyv5&N@=gucUX%_omOfvsV-K!WT#xL1uP5*8*^axbE7Ei|a9l*-NbGl5@1)gTpSCcT zBxEsmSSnfC5l?+s@OFY!GLHrea%dGll{VHill4FZTAIBH+Vz?~qUQ#^WQ#V^&VUl4 z4cTuG_}taM%si@3&)-|OWGd<}hf6{AMc zq9%MkfO=qjpJgtWYHbXxoYk23dZ-8?Yp(FaojknqP&Pe{aEz;-N%3$9UsQCJ&~fk zCs?jp`n`(YcT;|q?@GU(G<5l~J7K}!WfQ+J1oj{T@1>6YEy}seML7`SxdEc(Z(f~f zXhQe7SxsCjjDK>3K;!o|MG3rh(C_W01MDnx@(}W(Br*@!ccs`X@{kc}U$zWznRnEW z^T2WOyOWIA5!6c^pA>97uGU>mO3Xs;_`B;hbuEvC#+w>@_s-RoYWTZqMt?tdiahWRCnF}N?W3-|G{9Kb zWUeF$-d+%`D7w3$CT+T-*Z$U2k_yK~i?H+DlAw0?f2v>F*hI1wBIKLpt&(>2?B(_P ztRJ^#n8)?KftSR04a9kXTy{WLzOUuoh zR>KIcYr!5I7f(A@>FxQ6jLIEp3_Z8qjOQKRGI1pKZ6Z~7@_D}1NYy#39eZ$G9Em+$ zn)8Wl$`xr$>9O97cUpfTf9?O*c zQEbQ0wxTrBOs48nmg+0}h8ReNX!*q9JQ{2{tSw>riHN|vmdrzGl-Bps+QZr|hCnL3 zA<2j)R)3QbzFNOOAlSg3vA3FQ*{Zwi8!xUUC?U(3TszIw;N}bMA^R0#L?Dk!EemKl zm*>#ahw^;Un@@||mY$lRn?Hmac!NZpWX;b$O>Z6-nZ>xZuC;+ZD9PT^U$mkFbLzva z>-l*;{-mwzsnSNK&8mStcpJcxSlvY1Vy4lbnDW=lONfQ9wrcxpdg|#{Y$1qdb*UYQ zZPJ(WMy~57$>(S*M6=5Kr(-QWw1UR{DuEIY)I&xL*;3s14tsyrftL{mS`_cQvUPV) zNMo&l6+fP|++d?&c;<*8m2N8`rvG+50wX;{q{6qi#rlZ6I$IU)+ zuf5_mcm3RC{!1IDTU9ovi9fApOX_3bSja<0Tw3O0PoHauQL0$1j&^Su^iezA!c*V- zejCBtAgkNHozq@tz!0N?|3(Rc7DY6xYOGQwVAWZFqeJ0J26BIX?}g2~WiEZyo>+ot zMX6A`ZsNCogN(RpyLF_(y~N!&P)n%qsrUHG-_H=(agD8T|FXtk{zTFo`N=$J)K2r( zPCkZtLsdyCv@Y^ycb`JkJf$Fe1MRK#8jgjMXjl4ZULl@(G<#DF%Qh@@JYe3uNaNGt ziyBg)9@7?Vw*NvD++QpqNApM{qt6o!dk}%UHhb@GxxekcNyUv*xob#StoZ#y)`RbG zHQx4Hqn|rn(Sn<0(yQ#utK$7y zz9ARe-Q9M(LWsU5HjLtp74N=?W|c^1CY$|NZP6`j21^K}Lf-5*ix;%EweP!9zkM*6 z9$~*3a-dXtJ=oP-MSe13w^7%;B*RPn&jMW~1X5XSs^+&niymg;wPcwm2L(*ImCiWk z)1G1vB4o`2p6pE=w!fkgR5pNO4}Lf3IJS;9>_`s1!D;@j=yUo2``*KS4L9Yu+S=`$ z=2s(^{?R|HiU{-*%UmveCE!F*HzT<5Qx(TT1dhb+C6%@WOl#cD2p=yakP2_h{5=yx z&6}H!Fe--hmZU;SmW^nnm1^Rt^GqK0Zo`}&w)GG1>&w1%m#m9+!@D*+2Pemv7P*%; z8n&jAbtx|<_k?L86Io=rF%Of@H>N|`0qSm+x@AxF{t8s$%E(v8Ik)) zj`)y`{pwi>_HEHd3DsWw>#f1)?zy~wZ1&c7_RwA zJP?g{c)sg0zi+S_f3cjRnA>d~yFMQa5i$?k*%67APg{-gf8>+C4q}B@(i?Vp-U*%WXaHszG{;ET# z_6x2Pw3}0N9)093JX!F>K{WfFxS%`%91**-l0Q=6*(W2aw(M>C%%s|sZdw3pi}pb@ z`wgBkI~>vARe@dOu?G<{52q$hJ+KE6vIp^UO6Pw*+IoFrZ}{(4%Yszs8m zHu3woPisA57vb#==j@L5iDL^);`5&f)`+6m({!q*CJ?;+3*kBU|MhkKF;$jP_%q3= z1!>o8S(XAW$A%60?ufYe`;ZN3h#Rdr8j!V>lqJHr2<*;F_pgc;{SliYZqjVP8oRBW z2;}{~Z%R3BEdobbvs(VBtuLdAufG zA$HaN*5Wj~xB3Vn-7i$~{w{C2_>whst+U|w&HE+ZJmZDQxcjTUB98Ddvloe|i2Lcl5E4mR*bqGc7(V zitVv|uXehpheNK$^VOFm=cS*H$77LJ8Q;X@;{UTNdkN+Tkswdv^Yb-N$;jWFPCbrU z#Aq=K;{B`iO8b25u;p9-ou))oA7(wlheUk;wC4Yr(#M^7r8o9cf?P8oH=9; zqT(<=x_0x@LN=TFL;8iPvg{TkY%NPuv@OQ?y=!yRff_Nj6gL-L4d&$;;rCS9yrtW>N=4V8i3kwHW&u0;Cj znE|S(>LQ;Xv13JH{R_^on39jy4SpM0Lk_U>iC+ud=o}Y5yxy({2T%>D5O|a?=G1vYyQER{hbJXakrIvhw8Y(qYDMX478O}Uk#mlmEbt_?pBSVS zddEJ)w z{C1So?zN1BPT+h+m7uj2c@!0ns?<48o;Z|up&s;zMU@dUp_eEzT=_MsaO$ z9ORv`Q+DU}_GAm%wUC91JefiALFXp3$-ZTFWku17cP(V0I>VQbcG2@I&)Ryxj|#_* ziXEIzrkmtFiuu93;Ys56+p9<1)UJQ6^v_E?oHAICSj~p9II_sDxY+B~wv>739Z_Kw zC`_DT^o|n;JCS&&D1V)NzS}YSu!(134eFDV!WMtBYPoZw`iM2GW*rstLA!WO9AJ0V zEwCrD1@#iWiz*w13bB&~D}&dnQ=c1KE1y$$w|APz2U=rBgqGTdWEai+I(Gm&4)TFn p*W*A7+@<&3oE<*NM^8 + + + + + + + + + diff --git a/aloha_ws/src/moveit_visual_tools/resources/rrbot.urdf b/aloha_ws/src/moveit_visual_tools/resources/rrbot.urdf new file mode 100644 index 00000000..0d6a8402 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/resources/rrbot.urdf @@ -0,0 +1,125 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aloha_ws/src/moveit_visual_tools/resources/screenshot.png b/aloha_ws/src/moveit_visual_tools/resources/screenshot.png new file mode 100644 index 0000000000000000000000000000000000000000..80a57ffe79f54d933415bdf1f5bc450f25ab77aa GIT binary patch literal 65395 zcmdpec|6qpyZ4mRrl@2ON%oNJSxQLuBxM`2lb!5aNwSBKq{*ISCy7DHIwAYM#n9My z#`;_{?)yCFKIi<-@A>=j>XjKY^ZkC7Yk6Pq_w|_|b=6xZkJBATAP^^SE68df5Qonp z5C@x%9)!P0{W|>zevvrI+}1pL^l1O6+6X+Qb(VYJeAmv*+4Z5LDZ<>=&c^hzld+?z zsjZWRo%1|Nr4$0ejJPd(L(}c;%wU6^o~zIL*4<@^Liy%Yj^HST+p!WggP;9bElk?$ zSwdE(4lMDYtaUa zA`nPjt{BCvPOIWRt*lOKVLlJ|8)Cu#CF)7{TV!rYueEq$&Y-QUcp|)PTawU}zMbEW zTk$>=qnM_ZB}5mdn$d1mOcy4<`+-9-Qo%LvbDJ5l|T2-Qg6JT{9z&3H|ye#hgm=15vB_Z8RA4#aR+Ua zl~2ed{VJQUm|n)ifM7Z?ioMpt@yc-7yGYrOomN3EQYbqMi~7w(+mfx}y`AAeLG-;TJ>4cwtjd%)wfy;x`9z1sQ22CmUEc_Kiju(0s#*|SXEE3NZB>t)eQ zYhBs8OgUa5&458VwH0irvlW&dyGaOwFv$<`rg` zpjGkL!MQTm`Jr<6LOtF|$I{{L36Zk0vf$(ool_#iUdwgTyZOl+F!!+t>D@mm5l9&e zhU;80Q%&&V-p)EUf^SxRyT)Lr$qdV^&CeDvJ zp`~zwc~cA6tZhj|czDHf^EFt?hb)mt1FKQKyQPQDg~>b3_6dnu4BCoNg}|EMnq1T& zMd@U}OH7QpnZ(gJG&EGlM!3fFo>QUd_Nnf8!OHRxbgk_kRn4pxy z!w9IUsT*1lev>zSNRdu-VYL>5;tZ$!zl}JHPp^6~Q`n0qvPEaYZY<*~A`AuT*xA{s zsi@f4*>5JjY_$-i3nTSUdcb2<>=nqMlG3WIqO$Ynmh_Sb+5_!VMXo8zaEjEQDx}6j zFg!F=Sy}lNILcf4ElWYV%3)t@BH#2*yYF;*| z%6bN_j@}-ab-`dAB^cvS27SMrS4;$G#dhmjan4#<_sl2+6Pg@L^>Vw_dZc0$i*>T8 z%i|CTGU27G_CZDcv#@E_!uq^%Et6N{m92_(WL6{ll{AHE$QUE)CYLP*-#C_9^V1-$ zsaf>CD6sxsbh90%zfA;6oS}xcBxP`1BNLusm&XgsLGDV3!ROqOKG)8QdiH^WQ;I( zbL+u6(uUQ-PBxyJ^*|3<3vYQlNM$zN?AQjAB{zwiH*dGt6x`0hfS3`n1#e2?2M2sbOP&+DP1*>f7)*$Xd?H!q6J2A zlGGpeKT{Jdrs{aL0E2!PN38x-6E^n7%n=uy7%2^Xn0#UHCywA=>$$<#uz+mQxT;^h zM)@2kE|Nc$Pr1WGwZ2C9`Ds>IV|0wvKP4q4V}?JY)#CZ{=ip6X-#XdNlgkk8AR_P0 zXp0?18*9K2JEwM!M6S!gZ)fphoy+D*^Uoq(D|Zsl4l$#Vb^`GxW!p^7usnbe@ zEm}3>E+wo=!=b>7C9h0dJT-?SeFuGa-}*SLc1M&uz=DKp%-XDh2I!Im_QOjhOa@bFXQ6 z*kG9}AH~r>tDE+s+Yx&k5nCk?dPx1hVI9Fa@Lp1J7}H`Mn9notz&9{1u#RQQSAAbAv8y0uqLVI%X%%7atcB6H_R)i_h$yj#Mx)HwQlz zV+bD|9SsT!Qebs_GaS?0-`?KdCZ;TIXI|L5(_g>YGGD$NzP;G4CcOjkbGh+XbWn{^t3{Dk)_GdP^z`&0&v|rG(gnFYUEST%`0;2o z8hzG9isI-t1Wh_7ru&qaAfj*LGNz}ek9>|S#Fz-+cYL%D4`Ll}mpR_nhNW+{07T#= z#S|n9!7)kw%d?9@9ahB+CI!9Lke-s%>qqVDGmu%Gkdow6A~9I_I|SjtYOw2hI!ggQ zW*RaG#a{+(<0X>Yajeg%j|b@SQis&IxVoMQ_)g9+|Gw42Bu9~hatZIX4}Zu=!{jf< zmx4QCu+HL%%4`rL#1mubFSYE1f@7b?(|(Z!rxA#MmF9i^R3P}up@V_#G7+JelL0bj zg{5&Qwdmui&&Qm_(a3qoMs`Exv}B~Fv)umQ)N{KZ73)9}sWFORl8!A7_mE?CKNL~7 z+>YAquomu~ew@eX6m}-n-j4~TcPdh|M#CBjKt8z1|`j+v-hc`SN z)(0wmecvxRWow1VvLJI^U0k}RSD(uy0e1M)ni75OCueZ7R@SPAH!+OJvN(uJKN5U{ z$GeM(LjH}ztWtf6O-^nyqo<>LlaNs6FjiY%ufR`Jq?6qySkY+}gTX=!IJVbYXx?OY zHM-#y23vjF!^C8qT^ir;^Ji!>2W0BSwzjqgeDCD4H(2SJ874`OqS8|Lk+ZGW+y`ve zS5{Vj{`~pl$B+8@7uBPA{j(a>HiC2l*lScFmoHxi00=%rE9h{0>0WO4dkpqCssHlw za(Avl95dug7&sc4tH}k4^h}V1*GAr;ZALrp>{P9W4d6vbt)DF|N^EicR7;m2SQ_Q` zv}dTDlM9khfq0~qC0SP=Ika;+qVB4cl+VuQDj+w!Pu>F_B5LlQUNy>pt(pNV2Dw;} zP6fiBCYR$0KN&%U9^<(A$Qj7;IA@)lRY*2~5)U0@|LGgM>BLVX7_YqQAsOAE@NjM1 z{7zOUM1rwJ9v&VT%aMZzP%Y;94dVD-bo-qh3R z4wPw-Xt55=`uwT7@$vDUt##9N-rIpzVv0VMzA%H$)oGtf198eXO=h&Gvb(46am5T! zK~!Hl?YXh=kT>qy5(QKVahS(rwP67PhZ{zrMj;KYAMq;InO*gO=?&U4F){hB&Gf=* z@=#$Wm(3GtFPv}hFAnhMvBEV8Cl(KONMY6#@v-OFU9y&*p!J!|0vWj1nUUCHZ9&+EqG zB_q^RX;;Fev94?)L`Xi&A_IQAJVt9iIEDQW2wR9~TBofW_#Yh96=&Xo?ZWNey^Fm9 zhz|{^aOdgWuDDc%G_UTcuiYQYTP+wKD7yaQ{Q?CaLy;bDw^cC=HT7sIlf%Hnd6n5U z4>Z7%PAegJT_e*%&|_2iQDN`XZzBNYeV3w9G-2|^I)e#DeP!Ag>)I!m5BuN9q6-5c3PPh_PUgL(=HVb z+<5uz#C*lpoUR@El1S>f%B!meY?rSwoGR&;6c-0D3I3a;-ZL~b=RM^akfO6{uU~V-C@#8{YjU0JtD{BO`_#8oVcT%dP!9^k zgT-nbjr!R99cc7db9P%k7tk=Xd|u>e~NWf(vkFG zwfTgfEAiBlLPDLhE@AS7LVEh?G9=4ekswgXyk(0w`G%|~d zi_e`3jG32@I^zGWWXNNBZML6Sr2~AT#m8f?bshFzha|5_nKo!ks6hcdV|OpBA+^_KAcrZYoe7h7b#=um zK%{SIXjolc9SdF%PvoT`n?vEt^YbH{%&sLi<4Ar2wuY?{5)-rJr!jf%1gT0s1*#2d zG{EUjtMjz9@o(M;#4A&Un3|ZF`1nB9&&|$G9Huz{;l)A!YY3pa2Rxti^G^rKDyBsZ zD79G#YPi6n)0{sK<|UqJ3keO1WvrvStLysPHJj@zY>yxmmP6GCb!Kp~Nuf6F6hoLi zfTomIe!4KYk+rq8iHU5*gB-n5$V|TLWy1}rQA15fA&_-X=jxTRxR!;=8<*77I*WvWJ|$(!}gtoyl0 z^v8DGw$EPiqwb$(%UxYv6BFsm-z1w(#|GY`1oIS4Y)1MZ_4Kv_eK!IND~G8xUqPKZ&Klbq0jM)7X0b4Ae(lEJ9 zPfs5b60$x&99Me?a;$C1FvJr0JS7CmaCH>~+=0Ir-*t(*T zI6>ehdC8$n#&t}unh0zus&RR9a}z>H;pfk6cqq$0Xnuq&qf1iy zfCrNFrk4~D62ihR*v6Qqrl#my*_FZ&Az9BX;=MF%V|b0!P*Rd}p9A0)%@0onj(ur< z=pElQ0Z<=Mkwzu}tNRAt#f61Xop~<>}pp*+(+-Y^H6Ht79zQg8~P7Ud}iJ?r* zDO`I7Iiti2`IG>eBmm9g=f(D-8%#3V0nNeXhR~j1(~v^}m;rW{Am>LI<^k!dl!44; zKNI980rd&QPhwSlL@PgPn`h8gZP;D6>&jP`S*YLIEd<4+*ua zesM@mGK5Fkr7b(%mCegrJG%`Hx1$@NG=aLewpJeCR(rIffinbqn-(T%AD}!;FSCf5 zn@Bw6QKyWkgBk@4xyHz$Y!Hk>Zy6Yu=r#b>O;|_p8DR6=St3RGvr2)vvY+#kf(ZRN zFOT`+SH7D*T{vf0J#fDmp=uMwWhiyF8rV@YdmR|@W!Qcj8eQAtMFr6%=#^#tly@y7S=>@IC$xMLITV z8odJpub%HY`pP2Z3VUDi8}LvmrD-rzybL&&8n_oC{R!cBL(Yek_k2T3kSwH|+b8m` zGus=!$35A8tD0e)|DaS)N-XO`r&VC}g=slp(jV}wLEQv((s;1IlE|XlHrSTH)VfKn_fm>RW7QQtHDf=UH+4Nf}#r z02O6}w%TeO(G3v#oyEPobi7kqAud9x3OuE<@^&w3rx$1f;EpDjAgDl!qIOFQq%Ps2b*2O%-T_ZX3KxtDdXuy@8;e15RZTp*q$ z@knJ0O7JMiyMn$z=YVAK>AB# zy_A{Kj#Kb~veBm!;)<$@3f55!_jSPY<%0IWx=#frEw z6LZKNrA0+?NAMn!Ba=dGJ8i6_)FE>5i23y`k|)<;OfONU`d3Q&FRLWLbb%ge9% z=0C{pQs*!!={J@5-qxleNGEB_?^4p=WFi0qkV2_7?iS4TX)gn}lLJvYZahq6Xu(Z_ zP~aID4EOgdWwdj}d_?AA&L7}Rou8S>o4AV5Qw6Zp7TN1BU;)|(5`lHT5c>$KoPeUok37clJG=Aq*fu21CU1xD1n#>9tbcB|2OZ99Y zKfX61*FU=kRXOd?(!JBE%Pvq`(~vcoXov@{D!vyGfc&YLktMy;E{%3yHp_kE{JMM% zh*Oxac5}B`VIodvTL@rZa_b*p#33~GW=BUy12Y$|y!JV=xgSc0$mWMYG|fMxgD=Ot@d7`9NNQu#G;05|sR@Qz8URdznVFg5*7!1BCW-lxhmup-#YZy;zixUU zrJPNU0f+b+gZwz36}^ zP_;lsLFqiXoQS&~-()5xkg=ED?f3KPP+#BD)(#Qms;H=B=jP52oB2+HAzZ@a@m=De zIsov3YS7FKq!-1Mv7t)HX^y4kB_-5Qqqw+$1ZC<`o|+p+mE`IEV4@ZyUIivIxePo$ z-T>0`d-ERC7r)wkf!fEddT56O)5`Q<)=L_1_EIJ%nFDA^h>nlEd#eojduCmLE7r(_ zG8+iI`M@zo7)HZaS9n8LOTRDqoFL4-ukX#EEqQ)KJ&AA0@Ol6QK|7Af z4_TM=hslqP1Ke^pQNor5n3#5QLresw5r~@p{$;!uSj!q{ZRbKDq(jiTR05@(mlR5w z4Qo|)9~ryjUJ?I6;Lv?!v$pSgj8<;9aefbQ>Z{Y;a1+fjNc!IlUw+I@r9`!eNJv=c zKY%bH!8qL0<5odPSNFBFOwJVlSly|n7`F{?nT*!+20RC7)L{qsIf3Dg^1DTVW=i!y z{eTjSiZxCWxGMCSqE{&}?$WiZ^w-f5L4xsmGnBr6_X0$l*==2;WazRjA@`bEuNxe4 zeW92JuIkOCQgL(qqVLAFalwzBQ*(1UGqpoQ?|!}qpAf*@%+>2RvF-8C}W{DnK#8(dq)lzdxk=a-_JX^bY2davsw#AJ{@mI{+Oz@Zr<>}+k{ z{dzrnC|+5xVR`1#BQ%+bT~p>#(Nv9YmzAB6}WC6on9BkVfm@kj+(t+(Pk!DWGv zL^-iGc6OM710X$@^k;9qR%Y}6#_Xh5S62tQKBIdYCE@P)rr~g4X6ICOO$!M$8q|%^ zo@~zX;MO@Ohd+ zSl<5KtG5_cv~;?F-9jVjmYArMJdKqp1WaPCK_#I0cQCTe)8ZfloQH^;x|VwoUIN0ISlP9?Z{ch0luU24g)|n= zHxz#xdY~eA2kK$jNRsXfyqC}8#~=qH%Uo2JoS-(<)`qqUfo1L7ZU`X&_I47hpUHwX z?5_Lnt=m=L)Ae{!oH+f@oZ(O^Q+3BYZcf$L;-`5F3>M{aKJ-01!7$c z;Mfz51pTn2zpl1+JXjdj0$62ccJ{nm4){86oT>Xm$dW>rXsLF+>SmYzJU;Ctp6IA# z3M~4gN8E83{LXG>CMRJ2t0+{NQ_q2=3fD4y?N-G*lsE>q8gom^`DHvO8nJ;=~}UJMNGe- zOW97<$&|BgThlkFDtM_vgcDRC(^?Bh)I~R$sqJ<^T+`M9j0j+22jx-f3J}#~s;q7d z)9u-%w#$aSiJa(VH1&<9jA3U#t`K7I09?Ys|mHZ(9Wu;;v_Cs5(KcXsuw zOJ(ipuYXUbop)TU*wXt$q_J9~cxPnsUOb`S9D6|7j zE(2W#k_x0xqkNhm*;57_7@8Rf%;3*N-Mk!0u-c6ckZ0n~SU?BW=l&z_pu6g}!4(Ll zg}p#HrhPziP=^21grG!qDawmO5-lPk0@6{ln_q-meq)f>)Z$nj#1~&a?K!Aa**_sx z92ElB6j-BLCa)!ihqBQyG6K*?f6wJvyj=YnwG{-UgRqDQRA8a!_`@8RM}OdE~DA0$`^27t@ZVqFgbz`?GpoTqv z^w7lQBy1lKUnMjRNLypp{WRZ!!V3FfVzw0Nx;Q>G46F0!&j(~K2!NiRo(17mID@C% z(}@WQ(L&=Ohr-we=^#cpHqZD%SJ6dir)am-zL2|>*e*?t7`X@igS@#AqjJ0NW@-w= z7wKe!kY-}?1BNwd8zv8%4J!!<1eZV$Jvu~DI+cxqjMQ%MD-ih+q2U^DmOaq0soxCM zQ(7rQYV1XUAt9GY*}f8ov9YyP#KIm!qz5&vY!I2L`F%7`Q0$t71t7qhu|*RBF~D$z zy#$trHx6>(8MtQo6cBky>J~meyVc~<5c{D**U{05!mPJ5^)S-PeOgHf;AWI?Ti94M z#+MG-Ru;vCMF3}ut+)v-H6{H58qAc(8@A2kFrb!#I?gk`d{aD-kZrIAyJ>Y}prlpe zo9tL~n8Xsk>STlEf_xjaQh%oxO`9ih4x}~E1)!NkR63z)qP4XZnouAY3{>0>>$57h z5_}_{asiL8k8A*D-_32KulNxx->&@kLd{CP?Li4?H17@+&(o{W({TIjmoceT;9^g9 znvU1+Zu6YSl_Tvk+JRAo6>G!KFR&{}!Necg+JZ>XVIc^#EObMuX0*IPNpCMyL!TU! z%Fqx2eL!C|G8K#qNpwH!WJ7yfg9-4__0bfF1v*0S56~TdvW-A;f@pk+?)GFJG#y;c zR_MSv|6bf!T3P~5Ija*=CeTgLx)U{WI!qptJQ^9-I@m@x1sV)kEhL4uzQ)Gj6@>Km zNZS!8Kbn62{4zLKS}F#fCP-(BO|4EWk;~MCMlR^;fjrZu=xSv2JJEH>L6o7@LUT%C z(6$?B&{-D{{K!b5=L&wdeSEVzj|Wx*ug4xLE&Xjb{RdIUX8~EgR*Z>P_x6HkIJ-Ez z##-D{q$oqI`#XS-1&3U-Isw4}=LwB{D;FulS=}29`Q3<=^ZKx_ue-auscD)z$199! zX$-UEPv{2Q*w_H(-}F*8^d2>soRVD!5CWAo9%K&CDG0M>>vt5u{MHqybpv92qA3By z7xoSmGz7sq)U<)CcXV`o{|=2tx6?G1K0>JaIRQKsd=k8J6U38wLIncF$3^@=ojHMQ z2Gs|cWRZ)hLYqMTx6Zl%K7^h!&fkzZY*kN^lk@TLJd*)>c)&ss_Myo>DQR zz2%+c^41Or_#*Mj-wYREFlidM*(~bwtU%<>nrm>a2gcgjzr{oVxmyiJ98$KW10Lh7|M~ob?V08hEqi^@# zC(uG-xbMS~0M+R*2&9eJl2h4W1yBZH7tlkRkWFZmZO*OS)nU60Q4Pu!@JfS9A3*ZZ z&p0~z(2_PrfmKUO3p)86@b2E;-bO~rz1EOrLTXqJ*FqzYqYp6K_GN9pVKH99 z76r+&wDo8=(`K~0&-N7q)$asClZ9YoqgA}}#Pzg0s)NwwXfMioqT$ygZ8jLZE>EmF zhcoEt)36zUQqwn?O6&yclEB#vpBkGD=;Pv+G2ca#}+}xAAg~e`5uZ>0-gsDvIbU3Z2TSi z;&9WJLcv@gnoR@+p$G6y(*)=)(2fH68Z?y9PKjt}sW_W$dUz}|GgCF=;!Y2x+?`C# z%AHv|*#GstMo4^AR8*`O(5b8)92_WQ;zG~&0J|OFWpdaoTMJsNp=2hXXP}CxW0Lav zV=r1;TMOk7;5`r>8cYC73uu-B!->B?)k3ItI{A2cZLO^tL>0km-QBgnURNqx1@n@- z0~8pD4IL;b0Y7!IEzd%swyvf#1Z?c=%*>~poMwoPdO1L|Wov;h<2>DUITL76&~UjV zXJNLWNCFuF-9z!p@XRP5;wZpex?FM|5pB@i;pG{Zt&R>&W(pv!pgp?H8*((%(I7k| z*z+b%0{rOi?gla=xaRk+$eqnVAeDUAdd-2agRTb`Q3hb~h*~y)Vn|_th$X%Q4EQM$ zS-1#j2YSUs+;?A%^Cm!h5@YbvEP!%Y3eb+ADQ2^0En><23rfHsgPLPEuRcliSKrBPTCt^-@LZb#?O8h2oe}60Xb0yN3W}>Sx`JwW%J_xrpi%lLB5U|>R^v!$;OFt7%*_`^7;rGb$G)m$eVh-Q#cfLyUEZcNs4JT?S< zxX(Z;>*?)vUl@7vY;uzREsw}ysjZnp03en~U_PMvleOjI{OLc=;vmMMKdpG|I7#5# zEsAyUUPv>#IK@l;-=MP*s9+u+a0{P}aW!o*caY3WDtEF6EMA_;`58Sw_$%^UH&EG7 zL(8&&KnIvPkUOmHj2(EttuBbkK;49BHqcpiTHQmo0@SGvB%gKJg8c3mF#>K12R`-CmqCS4^;5D%{Y}dS`FTD?20+ZOqY+qhNWJ zGhd$j4kbAd3EWpP8uy5O&-1ghlF&&|Z#J>i`qJl#z%6mGBrhr2#tZWSUO+TPF?d;) z<&~Geg}U?s=jOAWxX5u(a^;fLU%!5Rl+<5oFp`y+KgrL1>d1)zU&GHi~ zT?9hZ4ulB}s3qIv0LD!?ij``BX+@zT=oJUouYh$y*C160G{v7daiZ;^ZfqdLTi9Gk zGAVZ&ieQ1juf97C=W*%(*zaP77yzm$57o48Mhs~m*bY?)xFaM;H8r(BnF|qh#scxF z%v#x6rkv@wA{a08@j(Fx^4#RRUeG~It(^*cfpCS$qw$QAi#w)K+fG@J5|{Z-ffY!| zBz4g2p^kf=4h6fj_)c{KIpzjZUmv(mffwFhUV8Gp3SoEdyn^zcD~2Vq9=x%wEsCr? zX3pq{%k^4L9zH%0D*z%uJFuFHN`qCkgW7&-%XbD{2WX*$0#8Br z8MH4smMSmvBfeMzBmcFR_EAr z8z$z}NdO)Stf@{taQ)$Jp6)v6{aQkTVmde&Q;@1DnufS^e=kyj6qHY&*0w;DRpQ{Ej)hvVCSUmJCkZ5#F#q3n#Cla7qpig?K%vSRG zb92$>UhuU&9BTJAV_}KLeKl~DVjWoWCbPRb=@`97nbJy=YXrT?42W+~ldz4aP*)48 z&*V_bLH`pJ7#dBn{sz2pSozy5k{G@H)HyS+ zlQ|%}Kj3M!WFT;TZ;QA=#05>l#^y4rMJd~H&luVeVJ;0x>Xg{llv^YP2#x)-7bPVn z=j3|(`>%Ngzq@|QGo`iOtWY4BHAYd4;Z)h+P?j_Z#+Emre;b4g2-?T=SH?S{B9eGc z0UZgj0GT`Ai2`95ct!n~+Z}a@FJYsGcV~y8sD01T2#GrKYAp0-lRNf28IzdblA7bM@sm^NF?I&j(6_k2 zDRk?RAuJ%ho8jx`snvW|dzOWJ%5y#?+qr`4u_*Gl$&P%uDq15!IyEUuYMGQ)ny}hv zayf%K@2Qr}Q^7-9JzG_Cdkl$7Bv~B6EA-L%WRR7h5x;lbch9oDA75>7x?u0qHrYtv zmN!={irQz->Cx0yrFHoB==y@|uz32O0B5k#wr|*ExxeVa{j-(G*U|oq-!#2Ew&wb9 z-^Hg5v{B0r3ncu6)07Cr<~4>;H7;rt`z)#0oRR$i;jpD1(wy;Dm{KV5#N~*PpeWX; zTzLP0CX)75h{Kcp$Gk)C#e)Zt48)U~eoj9`r>Ir8UNX2|5uGDkxjEFKith0 z;GibSl_cKEo~N;f=IG$OZ%4pm>Jp|npz-08ai?uchzH}~aqhX=J0zmOj>F^fgEY5J zj(jH`Bejo&tC%MbGXK!0IVA}%A|)DY5J+j_8{Zy)3+U+(A9EZZ`*7er6XAAM(=ZRj zLh?((IaK8BS0PU@x3^wWxIPqJfeVm4bG(8e;~~BgbG`pE!mX2dCm#wJLDi)jgww2| z2jOC5Y_ojF9H^h)gac!|)Nohg=`|QC3E}ZFC_iwqu+O>xSBW2@_J_OLbN$CGp?MkM zM;#1*^y`Mxe{cryIMxVJb%}UhHaM9B+y>@`us=u`iaO}Cg0JNDmB{rcVabMdc1@zMOxFaNi(jGQA5^#pP8{cfxA`_n_4UsmGAX?#hVLzX z8@4B(T>m$Q^7n~t8vg$<<_(g6afq8Vj$i;s4*#3!|HlyQEt%S+t=0E;4Dqkdh*yoY z?MC5S{DIvknDpNRLj3nr=&yrL!CAbgJ1PZ_*}KFyhrc$K+{jMm*z2z|rSUU%{@apA zm@B>zqCwu?*8K9Vy_@&%d~HP43lHxMHh)6idDWReY`_wobHLDWu@mlSA6XkqkKViY z>SEhtJ{|7VM~E-#UU)dvPnMI9m50ChzRuy)BdJoV72$z+gK3+sY{0Ev)`NGBbY={W z7+<^Y22SnQw%L27DQBTk{DMByi>}~gFZzym4A}`~D)<{Birg0wal}wLBDa|^{(d-A zlp_0_96p-lY7Qn*dIwcH<}Vl1aeC&5w9RWa_p28>qeQgAKA4h%c;P;$htuSawyY!{MxX`l9RX$AM3fs&b#a7XRGO2J9xzJJ~5Jw z6g%2`eR2Hkr*G+E(3f+tDl33si1wv#8S*fD+FHAn9W^twe$uHYuKqC2`_+6J#@kZ8 zbOIwce{ezE=x9CSpHX8tOX|ZrN2PX~yL(}X5g{3(|M}fW9ei)lX}G@;o>){Ip9eL< z@H#OYEl8}~U!g{yKrCqd@r> z{oELjrH+Nrh;ZxuM^vbqzsIDhR+@(UjMG~`$H)u6j;RQ?oXW$0d_ol3=-R={8XX5ZXLK35%xGQYGe?xwnuaLq-;p84% z81h|MaV=*_(e`)89M6LYfB32Z!VUlO?6+Ys9!P#}JC-6vFl|w2D1=wx%MYgfr(jVL zWP<;QlG#O5!Uk z`Pe^@Ybvr!lT^i@7CKPwL-z>2C*d$6w{r666v;hRctO;Fgi+{R8yn(nB$m1b?|lNn zSBH;jZ)mAPxL170RPQ;2D3?v|-!4J8(efDB=UR=tL@getU--^;=PY93c&3!7$CrzU z_kr79+7v>B9UKW{mS%eL;TS-cgryd>vgh%;!Mm{02+}U{rWRQh!kW9~%jExpMGb!s z8r}5HlOlMx{V^fegpA<_A|J0+N*>@Xo|BtLY_>)bd>^br$-DI${PsK1_27{n!4A0n zh<4Flu>JI~<&(H?#aj~I>Gnhau(U5TAIU#v4752XbcrE&`Tntgyk$!*7@~O}C%`1c zNYTEzSEU*17VEF@jvW0*y3w<08l1q1bcIbd+reg6{lg{fGljb7;6C7frE|yp7%|jd z1El*__cvYrH)ACkF|Ir77At6}jh{j*`KC)*%Kj%-6nZ5=J&m@DS zcC2ZsO|j0-2o5(#OB(f%?qGY~g$oDFtPZZ*4(ccr9D8hc`Ps8W3bs)v$Jwj{R4ygx zM153#$8|_xolP%RGqQ20Y2tv(YyJ!5zs?%I37|E5KIi@iXQ?~OUTlrIRp9JA$3$p) z>y6@35}#C3J!Q95TTi~O8rRk z1Bj>3h-P%^ccr`#8Df8ytS)Ll3A0=$J~eLK{W`l!fe^&-nxPpFuKkU1uM+q1HJ8tq z9_+>HjB1ke$N1;;a*z1U{T<1OwUFN_nYx>BCmQNZZqD35KS`qH)ZW1=*1$ckR}tS_ z+q#x!Lzw#kpd%q9U4td^lstr3IQ*B*ChmVKa_4aw=2q=o&`DT7x=X|b`7Z+!g#rD6 z;W8WYT2Y>Xy%S0AvyIi=V*ec}Ci z;y&~oJ5L@E9w^JY>%G{EO6){^(#!l1@Nv66HsdAArdW+FwCq z-=yKYiiidY?m?V34B_wF&6^Tuex5kz@C6 zmBMc^hUiLvh;a{=(ER`TR{K*C-*6si?h^|LMkPIEu$;e_ao^lv#rCgn|(3IZ4=$zrT0tue}^e>xg*;dw&04 z*u-CB%9H+UbN{|4QgMF@L_qi7R`$B=_|FicOI7@3nS3=&gka$OKfnL~+L%Q2{!aXJ z$7WgrF2+I}J=nbJf%tRmKYJlKdi?$rO8zkgzZdmE=AQ}6v_bdJ_y1+S*LAInNXRsP z8+`}=)BVQ=8~XZ4+qe@#7h>KULNU`@Za(VMlf9uYUS~M67%}d}))^l}7;+G9Xz057 zK@ZJ;ktJqW@w3Eu>WdfUs=&w5e>?>w41B9EOberA#>UX(&~E0$)G9()8M4l3wIOa6mOWho z&hWx2v!zc3&QrN3)HkWCB(M2DKV?sa zIO69(NR(9uQ%PTi?wm;KV|tK9hNvaX8&R2}eE#UCd!+A=1ce*}m-i;PD}{nhV@Kyy z2#d^zHGf3*Js08os~$%YBd&z)mH&|)q$4{tA*#e86W?b?xYWU52+&paZ4O^m;@`;5 za>MlyQiNL?mgh-n=jCb9^7s9Ys3Bxb0*6rb#n`)`YSAUTAcS3p#KR1+AVyf6p2Mb7 zRmXnmm?9|D2{-z6uCe3B4|w|P7V+t|#*PD>U$uJs<#uzs^?V<@+>;*pnUuwz-r^}F zVW_;HKDLOtXQx9a-Hbl_{YF@>a)|Q$HttpZ`ioPYx4?>+iRF%emjnIf(-$HoC2(Ql{}Nsc%|Eaxh!Hcf8S>6ljeq~)YSoLUPc zWTowp)dukVY6dwal*(1IY$+sIa@g9PL&#WwG5bT;H3&eDCRHCsX<%#;qs!5`~ zAG(vyNb4D3^)Ooiag&H+#*$(6Y>J)+6gTqIouNJp#z&lu;fI3Wnt*_d$cTd?Ghf4E zU#O>_;^^Wc;U}(=UxM>n+q>y(caEHD|9D{K1Yr{INxInSCBu@$DZd&(_m5}OEvd_O z(sQ^k`%dgP3hX~zedim@i2C^8Lv42D-7%vwa*sM`tr=JQsfbd&CPVi zh?LgXFJxI-Hx1-iPM_wC!!)E8=Z!S{;kSR4xx+*G^(Nsia^tZ-2mW+)2a2&Go)ESn z^ix=v*lA8m4VHp(2_caZq))TGrTN zgJUUtClY*L7TWo3i|acxWk>nEfj1xI#%;n2Sy{%^)6?t8s>lhu93d1Fo_2}RwOu<= zu}N8Fu%}T7WkA32{($cekdk2Z0Mq^No`?SW5T}2>< zY4zgYuw3W(Z(r+v5G%h*c7eOj@++Y7c3> zCKv0qkjD|THxQd?7c>i{$^9as(*i-h@$k2UcXdYo$(MMqG#v9$&s!v8GweBTk_Qn! z;36$a0Rbc@X~2I89NpI0fy;?~@5#7J=FSi9iWaHW ztDs4;MQ(w4`5_M?n6Lj8j0hRMWZ|ZH@%<@I4=XP1z0i`IOR||Jaf&Vi@o;1gw=(MJ zUZz=aseb(yBxeJdacEpPx;T7HS>+N23&(~&hK|IlCWhS$?vm{HJomD@`vmB7ECO(P zf`_W^bLDvi!U7B|WsEtovyCF+ZYJ$cz!*d96sP!^AW~Z^=dt)-@?4K)e1GNpm6v;6 zejf;=-U<|p*A2;Re&?&+GOkFC)sm+CUFiMpYy1l12U)S47TjUF2?qx)uJJtyqfiPw* zdkyjY53riT^5Z?ZyrWWv zxW413+zg@r!_-?xMftVw+cqc)f|R5TB_JS3r{d5hEiD4lAl;x+LyMHOw6t`CN;A?W z452VE14B0x@NPfP_qW!&mdpQunSJkj?<>yhIM?o%cqSGzdHH;LkV6(r2OrbX87L`X zTeSsu1E^O$wF$zMA79&=SKr8xTJS}g($iz=)5~O;ldiZ0Pid0Pat*_OFWFzdB;@WA z^clpl&J1Xf5{_lnvba|m0(&G%uoX$K^VR% ziz1dT`XDL)@+Denq|K}DBUe?<-0X(u%n@H2-A`O^xXJKCKB){9Ox&l)-_(!eOWt(t zs0Ec#TX4Dk5c$QM)tif+lfIzIi;%tAy#86~qYK}myrnqM`vFvuTeHwwA4o@@f;_Tg ztt_%HU&~{QpG?Q2&zRF}=xHVMy}(jp8&y?tKZ_qmFL(XazJF=TV!m?eJ=nW#luf)@ z9=H3gYDy+==YG{-j#6TqTk4`fvzIH6Rcx%H<#IQUUV^mtf064U*sQk)iCxx{;JoN4 z%((V2>Nz7FW9+dW%vs|3oDj`5%%Sb%=RDSghYRpa0b7$Z?M#<1b%GBjy{~6;w7X8( z{aQ;seXbc(mejaYMV$bI*S_vQ{?zdm^@xMm`vZ7%o0Y;$#wmJ0eh@$Rht_#LX{2g> zE4Z&AwN?G{Y)!cb_M42w1}-Re{7w>+P&S)oa-Q*$+UJYgm!5*spFq*EWG$e@^sGbx zLHcJG7eqJMOAR_DX=)sjpA?JWjM)W+?=s=MFxa+nkzMoHV;v8{S(XB`rn}Pq4P;BU zos#bbF798t``>jkO0xe%Ve}mb6L`NQvhIKPxmwjBt@(#LD#Yvy{KDVFx6swH_$foC z%y#k}@ZqEJF@N^#m#du+s;bQ%5?y(Q6#)qNg8Sv>c-hz59`qd&VzD+PH^F^Z2a)aa z#BD}Ntqn$Atm>UqJ!jSNB2yqxo zti!Pu`i3F$CvKSLWk%K)?{4)IDC8-2&gFV{O?TzuQYBH zND|L@vks4>$^G~noXk7$0o=lgDnlO4MEm{SD}3u}K<-Zvv7F~rVK-h113an$E0y3k zohY?xG3hpP(7h9Q8q}*ElGeEsNSPMS;D=Ywe4)2(mtENaLdFt_64^hiSL2{Z6HC`g zf4mx)bVFT}E?&X1WHRO&Co`$LXCpR}4U+F&b@|d`{r{zS(hymq5QD6#FgY2Z-%gk_ z<7@nLK)e;fUXEecZ#bV*4ZQl;e9Lt;BC%L&#ezj1d|MV)S^tp{V~N=Y0DmB4-VXeU zAZAPgKVTf3;5)ZlkkvY*Yn78JrLHt~3A^*Z>*4a#Af~1wM^=BDx7{`dU+LJdGB~9|et`P_|>Z>M)GdvN^KqhZgszW>>)Z0D5&q^-H{2-Aj8# zRP5+&US2>2QY*3AIpL%Sr&lM^jl^$#Uyg-&H1#n9HnkG&wVlvBFdjl~76LXb2q2rU zHh6r>8nh>?emnoWp@P+8f^sR27n`5u_V<)XWEm-2sM zH&d)<(D6aHWv8JzKU-pntN+x3L)(ZpUn&@~ZtpMy^BtK=PH3uP0RLFH6Fa9SAJ@l; zS{q+=vvYFN6N^3`{ha6C9B07k-bH&4RAQ*|Gjk-p^uc@2ms&Pe`q5ru05!9Q((HY0 z*^;U^QIT?NiUmLG>uYCkU;1+X*D7_m)a0v@Dlzb$^r2qXEBBAval3H~=XZuml5VY2 znwBlB9z6vsq;&Ibkc^Lw=eJ|Sqj}rZFRhu9+>J$avp9X#dLz4-SzF6PKj+q*C?KK% zyZE>^tpOJE`!8MQ>+1QlXW$(&{aXq zFK3NnlCxDuFY`V)3Coty2;DZ5zU$>a+Ljfy$y|V5EU%)mil z*B)TX!Ot(Bg$bTe!=KO$xv zWkCOZJkkeW*^1;N=UOzL7prA~oZa)K z5PqkylARM$AC_E|@~y$C)r2ozP0Qgko;kNwjbHGvr2oHoi8GN)qm8g{L3yaGwEcB` zbf9Ut^HQ%0zS;+Da+npHd+NM==8YHNGZ_Xngz0@t`Brpo;R&Jv1c~b&Yx}Dx>HAMJ z&=46_esxDggOg-35MsH`Z2wiNU_7lM*2HK>L^IFpKnEIhG$~!TMy<&vY z-(}ElX-;RO&2If|rR|c##x|x0@BXsLk|kH&>L)6G4#>JrM(>CgMroEpB>fhaqHiv5 z*GmkH!WTlYcVIHxuEK&W&8*r&k7c}!Kkcp|jLtA%1UL@f&DAXJz?}Q^P7K+8Eli2I z@!Ku^%V&SUX%CW+bFM!`(H(bp#O2}<1Jlq{?LJ@WgAjCXmgF}UU(d_7PWTHt(tRGtM>yp6wqHH4_P}8R>YudR{|B_v8r|9qr{3-8L|h z{p-sO1*$W{LpLaP1zVs0@nws`P3*fv4tsa^2rw-3tq6h(T}tWM@0|YVXgIyMasZ-v zx!f-oQ3n;O1-7rOp!rK`98dhxzISx)Xo26=`+F5NeWk|QaL*?olcb+W@n%cAGhGFk z+5yvEX#YjzpkQ+uDB6OwdSZlvL?U`;1A-%L4KDw5a)gq`$s3iy*UhA zsIk3Go7?K`ss7~+4I3`4^<*bax2OZVTg1alCZkD66vHJYE}7cl<*ENAg^-#Kvxmu> z&&-ilGRb!McUO*xCmJ>GzW4zjF}*?6>}g?P=l>TZ34P+YBl+Bq%v%(Pcd0Ss! z5YC*onf$TK=#<4{&EWZ_-437!xIfKy75_a1-&O_I<+YYle+LbW;;^zfr-~ZGO}YPT zMfyROaexo!PXn-3qrL`K&sa>78(oN|`WM5H2b{JWHv%f_Tm45T3J_nO-to!1I1`6S zN+rV!Wt=ZdHw0*ZAjMYp|KG{->u!-zY=epOvxtMqjx15`Qi?={>`u*+%2A(6z3KUJhvlGYOsU`RqYiMM9 za3kQ?f!(;%i!V2BSft_^x~~WZN+u3$#KzHIM15@IKiW(kuxd}=7p2=! z6r_k89?6*pD(`ud)bN9EM{Ghd7u4}^#3Dz7(-UjD-P-mdWt4)-uN$C(qEBoczUk~U|LkShBW~ld9N`Uj=);6 zvYnJy2dIQD(hw?t*9BP!(di9-&QJxjnDzBj&cn}()ULiCr7ZBQ@BQCeF@2b%;)uPG z(Njo9`g$hR(NJGPT2?rjq0211gH_JEDnn%-AUw|199ZaZa+2QuN11JylHaE(zbAKR zyG(C_Uw4i|{cqKc&YUl7PG30ufJ%7c49fKUBynNH3t#!Sra{UUyB8?1#?`Vi+f-E) z-kU#jus3OF1QkrvOenm}e5)I52cvtQ+gaiS1ImdjtbA%NXK?251g#o1YS!qU%DTuDEn<(7##|BGcz zel|_QF|P~F_BetLQ)0{Nt|To)7lt*e z!99d=`FV+?smM!eXdDl|BelcqTgqjK_8N10g0>}W)59BQmG0r7o02lXwa}(q!(o`% z-4hBL#qU4nxDaj$a!GirUTa}M8l-!~ z$b28E&I~?dN6n0o-7GHf%YldWMZ1mmm++B<93sfu*y#{U1gIV5U0VRbM|)io=4`Px z#vFtPOviqb-+t>!?^p}Y#v$fBYgv;{-?-I8gs0`zPjgLJ*?s3ZvK#+bwE$&SaJv61 zBLTin_-Kf<6|4T2>8k5L33H`!B)=A<#4tCZcALBy5;YQo_mC| zj?K*Z-r3qP5}G(BD`e30ztVwiAL1S%57A~AbbEKOaw2(UPZ@wYe}dT~D4%)I;)DR{ z-Rt%~(f9xz47Gl~5Y_DSq%TC>4O>8&J;Ve)Msn_^vIvI<$KGEFr)QIgd`5h!6YE`l z@gp?R89lGH#AI=~*?HDBVYwFBjbN>Blt!!%zF$;t5d|MRd?eOC$hoZ{x{q^ZuPJ6* z*NRN*gADBW#Fe2qMT6PQsHK-{&b)d4O8fi*CdjR3ky|ZQjcflzYbD|Et+X##X`-<= zEj1cH$9p>WrPUId1Sr1>yvPUq1(!NLYEjk%rQAO!v1%V+6dsC1c8$~SIlV~c9*exx zR%K>RPBPb_158MO+tQwL)bwY;*h|W6e57ZRq!#e6rp3zfdLJtt{$12-hvw40JBeKk z%3cDJzg!}o(T3sY`%3zc!d{MgCw;MQq{}+YUufwNORU_?jo&{Lmaerd_iK)LBJTUe zpYG(BYbKb2HcWwiE?Pf&>o?hih}9(J*C`Sh4sUMVH`{QzqQ0<@-6`(Feo&&eLPYs> zto=I@#TOF6;K=QkiVMplYPlv&%3}4DrCIvejE*7j!fYv*w)#A2JEPM2)r?n=>R6Fo zdeORA+zNms4?T|gQ)Iih^mocDOHe(f*Y@jDa5;a;%GvgyI<__Au>Z>{{lbA(kh9L- z-6BXln6}N)unBOx?EN%56nXq|-~P*Cp~6%DWs+@eGPkP__WP?|*+EeB%lmKr>0pxj^>(ue)m|t z1z5$r5S^ct?(mUHQ(F&S4`pRg+c4enGhohfHrGiwauVO0K&S(xABT<_gfa8q__$3# zJ*Pz~%WOn3#!N76Q*6YR?n{fF>(P@ylIP1KFQl>%SdqFUNZr6j=^>yk)4vb@8Bv z2{h6bPbK87a}1Jk^4{K>>g;_1=^iI@J$43AP-o6Es=&|Is2Ivr$Ku#IsXlQ8%dB>nCochKsVlqAjC{dbQ}XVICTB=6Ux!;B zJVZ)V)Ehm@QkZ0y4&GBR4_m0czTAV8!Ob>q?@?{f*79ddFmT7k}4~kR&@QwgMHIgIil3t`e_&5ek zg*G&RIVOF5v-SorMZ%8(1fU10W=0=)~Gu^9jZUf9dBBDQh0VAII1VNr;Qz z|IfJKA!YjK&j23^{0bi5E=-D>fb6gG2m4xBP^Kqu9OPjn#YD147*z+eJ7;U)Lf8*U z;|bXu6|!&A58t6z=MQ_+o;-t4;CZno9HDMGwHgS>)joaGEGam9ZqKG`}p>bqfeL6T0Nv#5tYQ08@Bc{0kh~=Dh+e1|rs!R` ztPo+9H^A)ViWoqx69t%>*_msbne#h3If0_fTQcfkx1F4R zeqZO{ZL}}NV{BescOg5oy|Br7MN)QducI8NkPn(0&bL^6UmdpEk1Xt?-B4Bwbrgrz zi2q3z9wwkSO4nP4H#d*R@-r4>yCxmBlzx}%I+U#ZRr+Z%qBodPUxCpuzcuzo>3-2< zCjHiYMHm`X6SYeU5_k9aYZi(9fGt7J2<8D}Bvd0y0^kYq#=ct60$lB4e8uw`K7Lg= z$GAGZ3K%sGaS@PaV6@?CY_=`a=YcFI4powV6;LJ7DSCg|c1}J09cO#d>MZ_`eV(MN4 z6EfLfbkx-!q^LG&^g9nQVGG)x$ByM2Z*^MYPkTcx6@Dso)bT=cKPPms3I+<*d0FFH zbnW%HDkK+Tq=YP9T38!#e|r4MorV2VI!VOOaQ4C5CD7lckq$5;V{Fb6HbS&W`w3Pq z*Mx86*Du;|fRoQfiNH13oAt}U%%=Z#Gb&>V4`Kl@j0)H!WqRupPjG$DR0i8i_#cbV z)uTBo;PE$|7A;FMk{?u?S1c^BK2i6ps@ojUCa<`&xbC6wTi@?h|HzOdVj4B?L$|*% z9VVAOz5JK9jCjF%qW-*Tr#Od{K22@9tgL!yXaJQhdAM1df|))xC$SG|(=NiaK&B2* z#lu-o5Hxx2=KXBJyNKG{7i@2wT_hpPEfjI|ik(kHgl%|bI~k+Cs?1Jt8Qp*VnVUiM z_VYZQHRxcB`e*%a7&I0M?XT1QEtvN@N=etbPEQk70!_<1d>CI~so9P%#Z}}V(NhCL z98)})SOsJL01E}?+{uA?n@S0wQXm=tTpNrRn_Y6uQGD|o->`dRTWlN4HL+6OkuDV0 z`ivMWWM^Kudb|-~WJK2%;#Nn9Ki@Kq!BdZR7%ik2Qm-zjs_lHMJ09 z>tU@B?cfIR%>kRC<8WM+zvDCy#vl_QmY`(P;<0`yVp`Fk%iFF_vc+3I!+QoFWMgkI zySZ%YO$gu2L_h@fiyJxdZ5bZ##@$kl;zbOx55CIo2^=lLW>*P=tAB_Q`&Y}r{w_)$14)ERHE z9$KDXL*XhPV@$La#RI9FoHsUCZI_S`AfG7tEu_9M3>*d-`T&G~-j}7+EZEuTrE##E zzkjQCNjaF;mGD4=yOxIE?ANLrz?s?Etpmy`#0@wwkQIfx*8%;}BiAsxRboLUShm`vb)% z#4GB> zXQr)v1n;A^^B$UbZgv&DJC=*C`VPYR`iQ$FTax;q_=1pI}ZN~4~B6!e<2@chBIH9AjD7mYoKEGUso{3Yox8p zhd#GG3#>RO+`;#A*JqDdG5q}`*uQS%^q9<>zNYX9{HdpNV>G$ zqEWbC4zmE2ug@Nt0RQozW@;49Yf5CNQ}Pj(tE*P>)k2MjN_Z#<_UKU)2}v#y)9K3z znx-BPzxWf*RSX=I9NRp_fv0K);mPkHl3!KL6I zdaXn=Pz4I2Ark(!SLth-7l)d`NLFTFu>9N*;L2D9Cczo|e7ooucLy|s1QQ45fs;ot zwm8`DY=8A0=b9{GU9+mP^8DCE_H;kB^R+`6<>)83kqs^*@Akpp1-9y%X>ZP|U(ND} zG};e&T6&h*o~D!fv@6WHS30yL8+OKv%h)wbdog0}fHiM16GFO3?6WlAX@6Tu#*@eT zDAjnh%=`BCXK#?uUQ>BnQ8{d8dZ%I|-eq9Ge^ywGTPua)`FxGPOpw`Yw~IkSVm;{= z2;afc2;!(vXes11?Sx7X11cTkJZ&#O2j+Tr3 zIW#GXP63Kk_`^%x$-{JEzcNt~%%0IE%^XV?)>C@_(`+I+`9KDulp{AiA*J8i)g&Rk zDXys!MzPBD<&CGKKLHuW^8_*-MlykY0P6?wuU3L`!WnRd>1;jF=z2W9ewh0_LTv&( zl7=k+Hvo`mY5taNYkvP(;9JHE0mf&+XGNmzTv-~J>n-xgS(_ZfFB_SA(LLuIRp;Ev z>1TkO66?S1yNUz5ek5@F-%%3z)k+X};5qs`I2yRq9#B?nwZMf8=VWVU<;lfemq{2c z-FZG7emqLS%oVIAv@N3ZO%dy1H+BF5F~UCcIVcBT8`?oV{;!q%>$1P+2T5_?yVTn+ zPgRzA>b2DemnDdl!z6Q!BBQt)H&pb_`~F_II@emVV8d7yI{>`iZbD6bIO;q}JK+Vo zZsk!l61rT;Q`yS(`o4l%30GmBieY%d3+~Fv0t=`-B%GXvoLr|wDKPxo`2{w>V!^b% z+SOPmh)?By|JYIRxOj;hU@tqdseUv4#lVO-H;-bmG zCejE|A~Vde8^?}+sGIjz7v&GVu|r+n+1{F+`o}ruo|}VzsZVu_=Q}GwLk^ldhM*i! z`~=8I;y5ipWG7yDu|{-J)S%@~QmOOS8EU)rUFV8tC^b7H>)}1kw$=SraFPhdG_4?$ zYoJN6k|eIzk(0MqzF7~6YnVZ~sHXN_auE*nB!k=bBD|iv$wRl>4mz2{_eIr%J@Lia zu|+d-T3wIXDFtpdM>T;+kTUAN3LD2-yWq-p(l-Z5!Ks<@;ZYt+$S2Q!AWs}J>`;Gd%*oU-ey)tCu%<-A!G)oCc zgJCyqWKViUgLY)`U9xk>(-^ap7DZ;lhZQk0soi%@2r^mxHyA*XbRO4N0hX1`Ki@UL z+lYjMJm_|pnAhgZKKAb3h>mf_M(dVVfpMLpK%tA~&T4QtOnoq${_G(j7F)jxfXap` zc1CeJ0Jz3*#&LOLIpi>gx@`d3AYJw%5_^wtvJu!~p^N;EED6MKH6F{2ab zaCR}iUqT5C$`z86>OR6>a@!tyQ489hsk1*Q)0SBaM1+PmZPT%<`aSQN^Ic%Jv8cG z4#`TCMvBE{AK4pDcW58lEFH{_27JX^8Kw>Lwhn*NfzOyA7TOd1J89Bd9x&)=Q#CG^ z-aJZyyQIy}v&#h^WK;pp|2K>7JFJBV?qUJsuG8GfscP=MLcoJS=jiLa9csnPYyTiv z)(y#Td~w{m7$}|Y<0Z@+&XN#n5w6=W#L-i#>yVgo#Q3Q^()b=%q|RIJGR&$@$-qy) z)m9lV-{t2aMvA-nODSomoZZPMY)h;VZSvU^Q=P+ERU6cxdjVRe)O_Vr9)jdN$&UY^ z)}XpsStoBLbf@=+!C9WfMx4|JuDR1J;f zSn-zyt-o<|%l*jzpJKGg&J88!#XSP*nsKZ+E@&&2s0W*FP|YIl4P4o{ zdN86{tCF-;rbx9l+V!LXbXOQ&^Qi2gqL=f0hO@H$ES>PxNh70jrSv0(j{RBs!KW2{ z(-|*^o|M1$^W;?IlfYXv5gx@Y(5*zv4FaP3H=jc%Ln+9qv>U38Dpzw9oGo@Eqm3JfE_C9rZXi$2H$0cT5q-=O z$D<+DL`U723}aOqZ+@uv(l9ViD-6B^NLn&EGsUj~`Rey?+lw&3Ve9bPGIFVU*mNkk zP+(gzr2qG*aXUG=^{Sw?pU;s^XV01losB^Msj0DFQ;061KVHvnl1!oFhrh?ZJCNdh zh7`1VVL%yJt>{WR`v*SIbKhcmymiOV&Fv{Gh=M`=&)*->uqC5lJLAcdtU_49GB&n5 zG9mD|8GJjrXiO4|u&Zk2LvkJaFtD(eY_Uew`iQS-5o%OgU^h19z8?H6(w$5Xe?>1k zIGlIE>2`3n`RqLm9k;uKC0i11{-#QD0&bOcaH|ylgv70sb5jWnl`9QBE;4DPhz?X7 zIWw$U*lJmB*+1X1VSagmKty%2@s^qGY(#dpOCLu4>hAGG{W@HWjBIBTX(A@jpe{xv zS9XHX{m;oXWPDb1G@gg1TrEU+JkOmB()n1e|&#dA{JoDc%MB8I+MK}Ol7|EC22 z_I(DS`YOQgR;6&crBd78-(Ql!8S%=5f=at`WqM!OecD4+w32n`{)AeF!uaEfA_0p@ zeFx`$y>M-E_HY+YCGMXM(bD4LbiD5h{gMx$_eIn2AD^(} zOag1mw6@UBcPQL2^Y_=+rQ2Ln?SgIAgKl{^R?@e(SAD2ia?{ZXWTRkwAHqyr5qOk8 z_GhaPzZvn-5doyK>$r(L`UWS?#FB&@;ERj2BAn$iM*XKBA(fDGaqfi{ynt0ya+AY7 zc_4z4t$~N=wV)gTm*Xn*2nBvOe(O~-`nkXjtv$8XGSn-Dk;fsDXE>d8E%wzT$TCdW zChcAkg*9W1z4pX)WbY?8X6srS=sXF{_KU&0CE!{vgv8Yr==MA=GG^XsSr+3d62~PX z_?0~O9M_F)xM!e9=w2#LrU9%kM-1u3Hswe+3cJ^?BFUU2@0hU4nf;BLCipwyJrO5U z5?8{$GnWJ(#S#}(QT}jewX{D|3Lh0w`0%9tn+*$=mH7+?dzs=1)~yr8ju7TY z02tXES8tAM&cQX41)`rbNsEix-5$R@==Mw*?(|rF?s=?YQ_!xbmeB_TZi{Qgjw7H2~EBkFbS8v2w>L>6@LQ{k%# z{O_^)cU4Ch)HIIuG@LCv$+6R!k7^V(s1vegT^lD&lWR;2KoxF8Gh2o+XqD&0b-|Ig z7f?NOeAn)igECQ!LcX7H5kUd{;vpLZ@Yg7B0VJkj3RNT&@c4lpj$Jxd(Jp=MA3hWC zFimqT+^qK-mYqfvv#Vqbgf%}8Y7a+QvfZ)#3CoOelbS6$J@p5qKSI9pXZL4iV3`Y36;hB^Tl zc7giZ>MB6S&7BsR*wBXCrT={^1|XvOW5Ca#Fll%oL8-f@954$>BkpvKiYsz~Zt7v3 z)r==_)i^sqofyUWuVH_Y6q`Nfh@Cr1zs2J|TwXB~)1>8YkENQitflb$PHc)OZ(o^J zjK`lHPY&B`keG{Bu6dU3{?Hr%tL`88P{@WhQC15-zEh_2FFV+qE}34EJ^8^8yIpy$ z{dC*vgv@!nW08w1T1iyPmmi>GQ1VEn<$h6Ar*F_fKm_FiZSHD{bmc~39)=tXY-sP1 z!)rjw`u^gS`NEPQxd^DIF2OiNJfq|>IzK*22@n^4FynuK;^H~}>|YImJOQ*BHpUB8 z0TQ*tV_lo9%Jy(l$D^G=G*k6qmEX6B?!8an!|^lMXsm-Mm6zdfogS`2!hJ zxBP-2*5{xwP0&x;_9AQ%DxJ5=XlCgNi7Pa{&zbnT7lx3?ka7=3;_8=bRePg{#^1i} z?pFm^k9#(ndQW&VlCRICgPe+LO!H;)#rf30KxOvvu;h-E-ys@6{Imy!)Vm%t^cfl( z1MXYgN>e4kN&`MR>97B|gqUCD2E_Lg0P+t4JVd!xTb#uhMAUV-3f*f+M*z)G7LkJ= z$l09w+3xFc5RK~i`cvh$YYHQk_L`OsgO)(yGPjrFW(rMfPRN?0gu?H)J%nl#`Tct4 zbHoSFDe*m`!eR5~m7OBlT)Em}_|o}#Gg(70ve1S|kjEIhe{qJMBJ0RI=<0ZwkYl=C z1M0G=?VHxv5XCsD{ap-GM>S60syNFS%0S-uchb34;$*LI<*0t?Uu)3-dbrL83AdP( zojp|SdhC%8CH%0RY%+6Yz7Th2PF7qKETShkA1p43OVO2mxT72R7aelVF1Rw1ky1V5 zRRT$Px$AY8K>jW=Lu?9ODjbx*8ouq%-wnDJI1BRD^?zC&hkz|jsl5~$GvXP0-##u_ zk~gHzYxL5e7QKX07)8l^@P$zyH*kf?&dC8*Q6;)SXsXj_K_8xr_y$Cxd1JPqLy@ea zQUWR$RouoYh0Nu$RXOUKZi}AeoA&jkI2Q!chTO2p=I_rm67D=*sCZeNu~$WYO4LIt zgL~c@LT}bg4{CWTC%dUm`c)OX@1m}wUYw1>sm;vh8dS||%v-DX@`KM&5DMz`itUTA zu6X<@GGk75Z6zc4gfQc^WQhIKvC+XbSF`1wIC8$UUVPEqI5U%<+Obm@vEz5Dz+z;i z?U8fbBsW6(DG%eaKTxpE{6K26gxRpAEcgm+NE#iq!!y4B?psD33SV1Mhb=V6ev5Hc zat^_75~Zm%@O$H0mc)J8G*1D5mKp%asYv85ju;Q7m^%&;G&O^1!o07pMsWaVUh(cumJ&(%9hJGK~oz3mnyn1D`E>^<$U)Qa{_+&J_}1uf5Baz$=(OM-)mvHtmYo%)#EwM z;Lw}yAHa8HV$Uth4%v3!Mrt2a4!0z1cK6tL1c}JM9*>?VmbkDDa>MoHbP6s_Of1#S zd@#$s;1tTKs5|?~*^z|DKflT)<~`{YH5b5QE^(L%k?BQL{L^hyB`-pODl~9Vh%++T+!RDoeSp zSwdsxWAXa+YrwP)3Nk|>!RQ=c>JfuoMRwu|LdYSVaj;l*sg<&sVm8Rjn8B5&((|Vv zMR!Ftrx33=PT^q7SUkz!-!-jQw9xZVvrj|zjUxAew5d@3V z?vWjo9X-wArpIs|@c+uMP!SPWv49VXgiJRPf zdS#hZoo$YOTC(1~WS#rw|dHQFLpmVMcRJ z1R4g#{GM0*Pe}@l7n@D8)5NX*m8DkQN{lM_cvnN+gC2h7khY2yRls<|4pok@m&a@! z`w=R}_@W;pjtIaeRxf*b=8i(33s46 zuWWTn7mya1d^}!>flZn;B3m+e-#2H|p4|Fcq-|07T1EagYoV@E$w!M{4~4lv-PL_T zg^#9{aJ-b2Iqx2d<#bM8`=_Fvp9f}DAU1>~G~u>e0p~wYG|bD?K+{yFA3yHb4!F(0 z*}BfZz>c=@{_FnZT7!h=_fL=^r`bMc7mI%*>|59 zr$#x@REjWe(lB?_J{&1nVyS+gR1z=kw<*)S^M<9x)l{H@z+`Z5f0yUjB#XX%*0aS; zN=95X6$tGj?}SVv_$Ip7LL52=4FccOGXCB8`yPyp*qex%(oMHQl$b+h4vq>kiY9_) zCR#l;UGBeicxQu?CAwn#(pUB3JPBUtfqgsb9V!6>qI=+B?ohmdLW+3Mco%AuC+HMw zND~exGBOywOs8F*f6ioXb1RSd)GWy)hEmR2`y}jOQ8t0Z7)$2Xq|4I1A$AJI@G>)AR zfaeT+>$$1C70HN7^?-I~_g%2(%H`a9Ebeo4ofgLKDdKh#>^8W$1_8TJXOj9LJ-&>L zpp(PD5zY!|(^OxwstPBAvS#oM0=8#=KhZo{rP?YDR5m1SD4LnLUlsn|f$(ZvTJ}@zXvXQ5(C=aMb!G3ALU<}^3;>AMj4>}BCi^B4a+pc=#vx|L9 z2BKzQ<5b}XtDyN>eXbq&&LDt^B8AhOOPzL|aG~mE8|kXZ1=_zGdSY)-r1yb3vb*wqbA(7< zQANMiD_P?GCv7vXZu`BxEij*Y>c+-@;gFA12T&liw=bNHnU*QqP6)ZQZZ&Xb`{b>p zqJyax@oQBw`~BFdvMQmWG$SX|?z(czxO?{uAII4B8EUDxBunwUzTX>a!RGmjy81RN zghKdsmbh9W&(~5$>}vH`6z+;ThMc16n6tsuV9cw z1C!lYV4x!x%?QIxn`)8C21U))WYaVZTPmkAI0N4CH_ zXPbm|4`z7lR^!}DD+-#5Ck|!@>F`v3AgO4rEaT}VP}~YT%vRg;X$zWK%0odfQQQT}NO4j(Pj=L!=brzes*|H7L&p@NUA_fFuz*+-5L#zPslZEGQ z4}P|%$)FG1syENVAsum|nLV{O@g_}g(fHKqk@g^g#02-M9YMKN=y~c8f6$|8U+gjo z{{@mnZe{Pz(AIp74oqK?alvV>*&-7%=GPzy9Lh|bJqj%R&1A3o_Joxs~_3RREcIghI z7*g^ucGv|;Dc^aJwSxx@+eg$$P#C%pZr=NC8K01RdVbQGb9(UajjakMU^)qi&Oie8 z2-J#X4t_nk0R6HzA{k4d1sMe1dXR`L1s$vn{VVQ;goFU?$MU})G{-N~9AOud6xEA) zP%nkhzV@sS1W;#Z{#;4Ai_i^m7sK-5ile)BDz4D5t`7dhVXjv^JgYg!7HvKw}nTcGct3@CM~IVEG`j32MQA-2H4 zcbb|TgQC$>p6h4CDBs0ZH(vmXukvU-1M{pr2Gioh+{6~|uEH4J2&H>vIe)bH{y1BW zgOyk^t8$=r@FrnRr)AjN-5uCcB|2ONmRcbA1Gcs+dtj#k*FA|RkxBN2Q&YlE zp4__?2Eg&a@nU1cUdpUjY*p?v=PyN#SjQ9Rt8bwUDIclci94PTWCXFH)-hz28Jxe| zm}ZX#t;ycv_RQf1=%2GQRe8ETZdO(!ML+*$tuhePii#Wp9y`**4-xV6 zg{Cpzhlk^{214IP;=)vZyI7CQ-}*c@8l_rD(N-d>!wnrqT3wG$Y-|uAxeGCS>z8loF<3T^c?$r~0HFO0 z+KV>dl^UrjM{J3tsnNx9NM37iY62GOz)=UB%HH0h@BT9o(9@HDWLW6fWN(5JxfEKF zD}Q2oDX2>@q?C^ZWb`T(D9Rp(HP+wV9MZ+BnZ`n*AKmg>Vu<0FPJT2siWw#Ehg7As(a|IXNy44#1kqxsm?e zgrS$V@?M#IbN1tSC3D*ir1X#K@X>i)mJ;4jD3g{0H z=S_9gjk(bqe`a%}a2_KQlHN}9JW;*;yM-4xRp9>x5{)NxvH!^_P#`0{G>1SKrbv6B zKb}Sbi4YE$lKVCy)YB;$r2T-Sv;&wm?qii%1EyYCSy@09@UMK^(-LAd3Y|!<kcD z8hzv!-SfKM2$-1-SV2n5CzKB*QtZ}3LK>;5nN*8uF6rmnS<;4Ed>MOrou&6NN8!(5 z$cGm#8~#>S*WL}ik#t?|21QWd`PjE|^V$_)azh<`=k{$S84M^uD*`igpz!$^SZt-# zJYObnifRr0y4%a&`D0_1S{9CybK&_bjLtK9K}shJi@$L5^L^2dy*qoW z9l$;+_%Dh1=d3KkcxuPBKmRP`*V1f&XU&rlDe$Cr{y&_=cvTfUTooByu&OgxG=YF2&x~}_+>-%(Ynt%{IBBRk2PAzt^8%mQe;Lm}+zODYnGXbN1D=xa zySn7hN9vuttV(26ILP-e5B=1*k}0zC)*P95^39Ud%M%;;o}pL$Ny_BYR;bX+=Ft)F6{1L72r3WO$CzNQ z+w^k#J@%(+F%7@09=Sl5X?YZOXPHV$%m@W(c?mF~OJ^Ln^0Q&%TR6-%`WcIzW&dIxQTt&ngXl*;Z$2^+8}wkYX9f2dxyeSppuwy<}nl}eg( zF63Ua{)hP39#SUMi3g{0GND5}`9?$l2VXr?;fH?Jzlk{g<1QDWq2=K>XVp~U*tzf2sx`0|2A|G71AH*47lX9E2{qpmx4$F(eQ1YsLe)REV%&1V@ZC@o*M& zG!Ettql><|frNT2uInpsi-JYZ@8pc2Ne^9_A-{_(ID^r#hsQx049&aek3dMyUGu;A zrsUZWgf2kIya$vHKS^8cK#xB*SuENLnmZ5I1G{lAd@^v%5fXF1;|w4NA(RInOUN+i z%|suBAEqWK)%qbPh$fc2PCzY8$X$#po-~iBRvXXghbs$82cLewLuv?vf;(2hq^#TU zF&iVp=-9SDaknI4mX^Y#)yO!Sbij5d)j!QX`ZjSQp-44b*?`&V@O8ofH-$7;_*06^ zCp5DsuGrT(@#qh?`{*U!!AYLs3CEHoHHGe24ap&+{^HOKP{~Rfx%KreUVpYlP7{0(wPP|uNL>bMiLWH z!Sr!QN3EP$&tbc5Jq&~hs$Q)|*=rQg-8ul85p5ZUGGP!XWVIU3ytae;yadr&)yUVu zxNPV?BlNS<5B`e!zF)dW%4wD9a{pSYlN)eB`u@||x;acusF6Owt`Kt1 zQ`Gl#Ci_dy4>UD-r3@VuQ4lIZ-;GHAMJvc3F8hLtW5U6lyQ=w*(4IaQ!CPoPV9p>n$Jg{uonHNUH0Edbwrn*N5CsWUS(KSoo94Fb7Fl}THkhBUE#`|8EC!VjQ;S&*&H3KnO zavoHXk&!BkKiepz^!OdGPq`xabFQg7T@I&$4xPK@ZZ9vZnty+$G55x#Epp`5wQDZM z?n9sy3lIq5G|twE^z#WBag!=2cwg=cC~M#9e+7x!f@Kd-EgMM}w`G4zf6_v8 z=23yYHOC??<_0%jAGDjSK%ZI3$W^xhpCDodJfYxJ^Z3X?%dmJ~&k4RIN#&YqM>TJwibxUdBCzn>rO;P0e-w~xcZ-VctB zP_;u$74Max&iowNsi@j$-8fNvZb>)c?lh}D2}g@2RLyIhLPQ^@{R-?MZN7BzS@ADn z0~Jrp$c|Qi7Yfq)e=UHfSUoV#+pW^^SuM5i6geeG8ZEAZAxvER#lJLMK>4Rpo4p+r z`z89s#IZ*=L=nspy*a$}h8d0-V$ z5B~Pmk15JKKUSt3n>t0FR*~r=!rhul&i8^WIm^o3V!OJDMTnf`aGb+i81dm4QEQw7 zo*Mt24~z)uXp#a;BNK-pY%OeFWP>==u930!x=sy>a9)tJP0`=K=Fs^FNhczEn(} zaM0!7DVN{n$KLZpV64f!RYAA&K0US?Iz-S$i(Yagew01zSD$(7_+fdv#UbAL+QsC6 zo<3qHV8bCmAOm=?`8tp$BxMI=CEne+f{0%aq=uIdsfxwj>_n-KU5WH!+z^d<+^#Iw zELh%hw^5jv`j?lMe6P9IcU{|nAiTbaPw(48Z{&VT8A5(s(sz_XZIz}-vClChQ=zTf8#RQ^CUX(^sf7>Bu}@d1(y zCfq|478aQoi38EblUuIlAZNDA%3lo*1Un`96QThGzxCxxhu6u8st!!)}Z>q@4R_5lCI{|Z?Lej~>n&(b|6FEcygx!2zW8cf8S4t&`@2|u{H@PU@TNv5WR0()ddrsNUE30j8>+ah~gT;x7=CYhT*hqA+biyOV zO^YY1s{%kQaP@rLc0F zR2p>t5iN_)EY_PWe=1;x|5ydfpeUfQY(#?U(?bU#cajFnrc4-_u?;|m)Y;1VMmbYN zKzu0*5r0c-E3c5y`rKTR`-{i-?`U2Bn=Y#7`?w3=lr!t;E;vxfiSEJlX3)8g5hXag z`bs!+b+Qi}`oY;*^5OWuXxqB#Dp9(hm6{CoQZZvy70wzA${={kC}Mv1uL3-1R`~|J z_jc7!u%xAamcRW5tCjWD}H0%IKbnR6F?EQGv)z!7J zk+XijzO+OZ@Y@kkN9TV1&;`&1fERObkIJ7eKhUmu{QN$4d3BWlRaZ0av;IA`kjF)C zXw{sjC!k!sdV2DV)1QBO;stfCzyCpCo^e*R63H#wCcFK+6O=#`@PaZ4TrFnLZ86Z0 z*+GyHb>hnqH#Y-#E4n||Qapt&hTvX0cl3`mn~o|9u$M;CUwwS9{28yQ{L^*0R)_7( zdvE_tpa?cx1&C^YfgeP69s!I=_ZR#q8*S^aXZkOF`u|>fZl9-{e!;ui7iwT+{ucvk z!Uo7cv;7VWQ~vXl{Ifbh@Lvs(7Zkn&&t&bNOc|Pmmlwb;V)R;_X$g>p0917LWjP(s zYVtOBMb74mu@SN{PT*M<(~|eTM7E6CAX%A8z7+9j9K1uV3uIHbtupO3p3y6E1|!1a zG*vFE+DRcWBB(*h!{YiP6(Gn zp$j{5A*FYnw$s(j2Rl`yj`&EdW%_5`Lpo|Zblxpl1Cij=idh6WIH~FB|MG^zFsR`L zxN#8C(_6o9|9yD@aA&r%yB&gFn5&DP^$xH5k03KAZuDA^h9*zW27>DNxZm5gTl+R) zi0{$$qc3nA!)b1-4q2Yzo!^byI{iAzhpVS4E9Uy)1?q~6_ZMo7*PrH^Dg5Z@L_X(N zszjL1Ip#h0MnQ3bT72w4(LAB1Ly&Aj6_|qSgXQWd=4{$&BTXc|8m`3%cs&fB{C#-E z=E@eWb+4^X7qd`ql6LFv4@e8&r+Jg6J@C!=BNOIgZw)i}uZX4KZL?!8M&$n(m_ zfpC6IT5te+Aa)(U695U&Kb7_!uJ`0YKz%Bog>>L{)q67+dS3i@A%NQ8KKr8ogbw}( zS<=qT2UL@qXgBED=ZuDyiWDi+(EVUv@rQ*0qJy`=E);gbKre` zwAlH^uvqcoo6Nk)hJ)DOu9?v22|EV*jfTq89qjauJ?3QrX(KUiiwvXLk zK5bqco}LOeTGj)eUKM?P{WylRAUKq&%1TDSv9Y2kJNo?ev|`8Q|4&A8%MiQwSefCq zc0=`+d9Jf^I@7rA@|fA$du80YuD^2-0o?k_->{!IZglS4nY`%Sfo}rDaBQM!PE}9! z;rmrSy}BoaG@UcprzVq`4-WBl`#wr+AX5isBWY~-ptq--8XRJT%i*0NbZ@<%^_o2n z9|+p`XSy2qy{iYhq7OibaJ!wY0B#9#p7*t=D5R@oHqNH$5{66#KR^|S4xQW9rkzi# zlhMJ9#)#L(s(1wDSR$5E{URpjBTQ+65?54T4X-4fYJG2>!JaJFZ8`NEg?(?%(>J)o zj1{szq+Qg?yq-|GW~7`bPJ0{a1*r(THRo5E0;is}Y7FF`o3H_Q^}Nx= zD*3HkFKX{t!9MSzrE)dDTFZt{YERCjv3H$zR#%b-_ zWAv^7>x}gD^<(An=x5)V^UOz5DyR1fCVSq`6wRc zF_j|w*9|!aD9#)$`P2X>+Rz5TL7$$Eaqv8`Wg{b7=haV6w;jIlcX1kT8e4`Dn@qaY z9|H|shXj=+O%71K0dok{1&|c_zIOh)1}HqU2OsYN`I-LX)~C)EAcUEd==o{B^PgM} z$gOQ&ewFgOk?PC-Ei|Y{JCk>9X`py=;^1svoL=_Jb0FKNaXcpa1aj|}@-By^Y{UaQ zqrzB`jv`Yqwia0{??J11tGSdp{W3AK4&M^g2XQnk5^*%f`E1P^0&dtyWM0MwH4PpI zs#NC*T!RVG^3bf_hTDly)F)D3jk!BUtzLAEnA~B+HE1M+3r)h#;MHaL?j+@Rxe2RA z{&66%IJ&Lb3@X5XcsJhrWiWvnB15RN*j@FF!SvC(0GLnJ=O2@job1~#)dt}-7&ghJ zVyh=yf@p45+wOU@FmTTf4@p&f;ZTS$jlq4aKYBos0sg+mnbq23bI$A%hV-7f1xhg?lfNEr2jWu$kM}k^lLPJ!SkR z>zwgzWyg%|kA&c3KNzF<+rO0cwS@(e^(;O&mzi$AHOk=EgeZf=nmI43gEO~zPfOPAw9_X%>9|y^a4+#23uoF8s9*gOp_)gy`&CX zi#(F|AdJxE)rO1OJc20LqLFw1m)?9WV zoR_<1C!(C}v|5Ovv+K$HbtkaGUazQi`1-lizdNkC-&iOGF^!MD&mnBD1 z3vR8^h9>EMfUF=UU4mpe0SSy!sn9U9Le+S-@g%@J)?SW@gPiTIMFZpRqa5~b6aN6H;-Ki@Uq~!0 z2GTpYpuq{VE{6?n2b2B*Ap2$FBP#TVAN~-#Ix+^J2oXf^^}^t2iwSD|+izVq(I>;qtp9lL2eB`KdX5`ADKB zuUEs0;Eg8J&mYMR_g~bvA+V*PAY&ydehQm$V0KFu7}FU^psv303jV}V!#JjK?{CQn=+1)&Zk?_oKI zmIjHXLg5hCAgIm67NWZYRx?gd?v^aYgIE?pM3bm{oKpIl_(TEAMe1Z9 z`PLuL0M#;tMkRK0TlpXP`P5LNr=#*}S(=6*!w~}N(ju~6rP`Vcrs(I;=kw0{py&^C zm=R~##VOmM1C6Fwx@MLg_PPq4Ghl0pSSYdpFjp_SJ$Va)04|lnP_Hgb_d(db$xE$; zQI37OBFNO10W>!RsnG=qGK;%hNnDm}lfG}GgYn;Qc5Y?!+NJO2U>Bg8xp4egD9I5b z(cI4XDd0{?ohQD&8jpS|WbVr_Gq#LHnlqJnhL!cN{2LlWf!GBY zL1~1adiJVce|?o`I+AK9{_eF}pE@g+Eglb}4KOuOe{opUHpY1mwtn($B9gTPLHWaCkkPss! zw~m9YA$_>ThurXH! zX>)ePio&=3s2HI1AJKxPY+aBw;P-DKmSpULESTVi4X*vzk};v-1oav;_IbbyR{~Gp z4>0=!+f@i?gVh*~_y8_;)sA=%l#{aU+hf&O$Dp(e_TA)UnS}vi*e=5P;PMb;R)mNG zLHARB={X(mZ|KTFcW@Lb>D~lGk0dX^3N5v{J2#-JcP-$MGS!T&dSk#1FW`VGl6(lO zQFKYk+sX)WRgn&F+7;9teBDnq{yN5?TSTelYZ~R`vvS;$X*mm+mw(e{LI4UQ4-aL_ z=U+am1>2u=nJLj?(ELwsyiTCvm=4ApxOuV(6eSH6nN-*j{7rE_@R5GnzH$Ei)?SZ> z1F>MZ{zuL9sx;v5hkRT!C`3sd8Ln#1hPV+ccGq^wfZi|erGcmY6|D3X-+M<{yf#Rc zD+VwvAVe9B_L5i^cB7p9F{D9C2KX#c3YhQ;9w%#x0^y2p?uzWE;pVi%n=edP*=$@~ zQVf%a&Pz6&pl*YUxk_!U_Ezto5)tcG%GK`=ZnvPK?i1DBsmL2m&`pQwS%^9DS66D< z5Ryk^ij?K^d)gM$^+5q`}? z_#WPi!;!qJ(afk2)Pf&3ZI%pV1kz$JkLpvhm z21ufGph+SyU~4!}Xd19~i<^lXy3weR-b@hpU)pb_W1Z0P4cU1$Z~wG6<_k%LLPod6 zCXsf9EcfS_!Zm5C6%gXBJ}xPt(t#7>LKUI=h?8qe|B9G5;pcXp`PC@dF)4Pk8sLLL zz>Uo{l!#cMmD=B|Q43;5E7xh__p?G zAe5dDT7IxdFexMLRF2f|x8v35zU;{>^G*A0G-^iKz)UWW5&~$hE z<1xN(`41MI&7Fj&f)6J(Qm7Clt_^10T8521b|JGL$OpTzsfAR9r_#kK^CJvHToAP9 zzjqLxlH&~0nvESIsX<@Wa7@t_NJD#>I)^~iN1dKVpVkPpTj*KcJF8%GT*F8TxH6AS z{DL6CfS@NgT`<0XvloDSU&lXdt;EY7PtpJI3cy|H=E-7;isR@T>r0yFbB&)V?-k(_ zl&76WTAT%7Sqn}$A!1cbWh50Si>WZ}lA0SDpmQqa|0c!ze0;>DpnjdzIs7!Pi`|xT zMNCcsEy?^HZFK$|pI^}8w_5Sp_c9~&gyQd7Y$4jla|ZT>BFYVwd5MMQGs*ZcNK#xd z65_wGS;|A_m=g99<;Sfd)^@IO^?W|JKQr=FDmS@cA;h9vAREljvkN4lScB)jpOPTA zZcK?<2VV-Vz;!`>Ny^gs07sA{Fe!9$et%bOlmvq>CWmQul>|etFo;enBuknkLKSsG z#rde@i&cIZ{ZQwF+l4phUhXVaIP~{I=a=8KJz*5YPZJ$8*J+!q_{Uzo;Y^JYy|ZD} z5dh=P_M~ebz3l zxVC>OspKxMVy_fLXnfX<;NK=9YR6K;w+TPhH>AV9iv4WjZwP`C&k7SV@6YhK*y$t* zMCP8WU5mc+6S_Wc&(!bH(v0^NAN;$QYcZ7QW3P7fu|M~)ib=hd}% zO8_Gs&@*?q(D@v929AA&K<`!5K_!l~X0Wi5*2ICZ6SU-GI(`EMlquuIT7Fp>8T>v^ z&TekqKfak*Vfq6~D6@6uGDk;7%OJ*Gc};`b9^@`)3=cU+4lt{h(Ly>-l&0=&zfo;n z5q*mIPXrxva-lF-xN2qwe!BfQ-_HGkIIZ(Bw!W+_=~2DtW&O4MygKPZ>^_f ze1B~PkyY+DBYnw)5dtfOL2w8G<%4xAdo4EoFIOJ{CL&uT`cNh8mN3 zemFFpeoxpm!5azGL(xzEL^q(+CPnMYB}aWg)<}u|5uJ&?`LdBK-goQwtU`11mY5`!YIyTr$Ga~gVx;e}Ht>|D>r~$&OX3{d|L9y3Y@26!?42yu z16CnyRq#Nh>3H7$#zQv<@^H1>DUWeK|N5wcaPDjM&1drOFo(CW(c8!bID3yAJoD-- ziJM@&<7zU^fYLkvA}Nfj1w&{ zVu$W>X9?f`ok=}r)Qq|L@>YC+mzBr!%%OiU?y~O9rMOkBc@O#Qg&uv3@pbOk? z>-}-R+aBV-x5uXZ=59l*D|QXdQ2rWE$JYT*^{rrs%|E~pw!>d%+z#cBHAjvFFVb}V!Gpl-R6?Zta1z)$E8Lm&KFkb8;RT!~MUQD(ZwxpL;qhS^hcxf(D z(##1AhSwvXf|+BNOXaMnSgxCwI+L!&ttOFj>*Q=|n^^=j=t6(w9}DNk9oe6y17%cI6dDjX5M4evoyEurKBbb`fLJU_?v#O>xW z52?i?>8><|&f-!!{9*)(=GwvoA?Uaq?j?e6)!l__NhaYq){;6}-P#YbU*k!ZN++BslP?#-^Uo?naO%(EL)*o(DsY_u!j!SXm@w?SXHP|MxBiYPfWY zfiUpO=SMCONIF^8f8gz3T#Hpy@AMapHQGMeo-T7pv4!JO7heR?O?PfV< z4-T}gZghH{!^^VNjA;F#rlH|%9|;Mq#@HYUyYf!CUr*OWMuDh19?^!}BC&`f7*|k} zc*4UhS^(=|mh4hj0Wncu5oXAMm~VBNcVuR2xz*** zUNA!fBi3Fv)6mKaTA>sx$G<#IZ3aWp2B^~l*%KZZvlblRIQRQ?XT@Xl$tAf}z4EF#DG*Wzadl6t2Guy9!>4Fw^#A*u-ltFB zf})UyPi*215gG7@>3^8qd)CDno-mZd$T3(O;Sl!V;QVbVANs4SJ(qxw2z3c718o>Q z8c#moEiexrFv$+KnSZB@OS~3OpP%Dd0bA)NJ&G1A*8_j0V%|qkR63R+V2x;nN&pQn zWWie-^&4`(4%B@CUP+RwFrf_?^~U`(+9H|)UUag!Dn@FlG}osx#)P7`j4yD&GK#jO z!4?fQr~;S(7+wxM!X69Uzklmse@wDR#u>haeoOp+EkJ{R7<4i&5H4qk!z$Xvf|!8C zU~mNq*LHt!<)?yNjne7+$IA#oJM*Bzj1T0UF9^b4_}~&;bfOH}W9_uit#GU-Q=Tgg zl_ai{aNFZQv>O`T?l_G(3Q>%J^%U78Wf6ZCkTS(BV#$PuqPRPDCH{g&MiYSE*~{95)QDKsaA9#CYnu|eikSC5~%LRr|` z`vHq2>6S4iULRJ@PVA9jp&rZTj|GTg)XHt`?cKmSAT@RM{{CTW*_5b}IHji8(WI10 zLdnz{hq0#J@Ouh4$E#ceCb#@pxlMqB597gTZQC6QQvSRJJ%-I~GMgi+%@k}gSrAQl z^P;zU(&ON)0KpDICso}D3)Z#=xHa)Ti z=>&$q?;RNiim0f(BW_?F)1Tw`^rv^&CnuWc4JnC3q#BT|$8(&AZH9fct|g_pipwYj z6v_NCi>`lN+}q;?^0gY&p|st#q}t;=!g{gA3(DAw)8Jh)Odf z6@61aTfLj*>IeeDD!UOK9iRrPX?jU=rCp^MDS!b(wO!nF*x_H@bO31RFDXn(D;TE5 z^ia}dwA3)IqswZGm`;!2l`0o?@TGZYzaR8%Dp-dCTAthUMwZYuU+|>)W`nA3SM+Oy z9dEC-ss(t#ObkBtSlD&65@)S|tkMimUVz8S&Ix( zM6y`U>u2OnOxPZu!Z&4S4xiqC8Y!-2T>iRogiE|3^IBiy_B&vCKiO9l2{;h1uC_WX zEmZW<8GO*w^L|^Ke%W%?44mU;j{!^TDv|1~X=yxm&93MKZLhqPtIf1oeS*lkyxR;# z2p9%ig|trj@(}f~h(rjVSXiFf59rKdiY)F^1~?N|#)!t~qscAKwr2iI8cjnUg3QRB z?qgmGFP#4Vgsw;&eW9Y(P+FK{Uz3TyRT+ee5%PTw&ZEOnghl0mjS#wzQ`kr0H2U(T zDvJngZ&IrFn|X6X|I-Yg!JAw1)fY)3?EmgUNHS4t2Gj0Ac}q znuFgvKPbg|dfTINvP})67lC+`Z@VnS7L2+W8m8r3AM1B2HX0zW>hfZRX+$STNVY0I zQ~6)(wOh1_sjmpwCd*@-mrTdnsk^=6S-CcEYZp|v*Y;YD@$@T@mIhN$+~WDOZ#MMV zEH9nT6O~_>G>qoFH*#*H`2KL~Y>PA{5rP(AV{?3YxE#8xd~;suh&Z#`aS5xAoG4YE z`)K;@Njs^wmKEShb(qvS@EG%k;`!emw$-bDtG(C9k_6iAz&d^gT+v-|_;?mSAhG^t zOGry6=+vaVRD82VU`31B2M&#g;7)@9DfMmz^%qU17coE~0Q)z|7nSP96q;qdD;5z_ zt>xcP57mbL+N(Ma73AIo~=PcjY8o)Zxq_iJEzzV#s?T;TI{V9u`X3g5j zKMW@|Aq{U%T@)6#D<(Zxz;(WD%u-j=%}PW%5cISEjwDMzIzEKjuybnWbB!-qwz1_C z!uRVt_Gu~Jf4*v_5`vajqCV?}4o@WD2sB}%HFFp+H!&%AdyA|P{BWBa8-}DutmEFG z@uDgHamSh}34$beCJSQ5C^l#|*4fT#7%PF0I3F8$!59g2rBi1!#H!2y9V{(;GEkh? zdK&s1g?env*hFD$JD^FSgi$%&1?QIAk_TQ$(L8Vl4-VZ&Pk72vmRX55ibj zKU08m5dD%hVLAXx)i0gx@c20Ywb?qj8x^2(p+UXdckVhe-DBrL0^IH-&k)`cY1e=e zO7!9Ih;jQit(}v)&jN)fxq<=zKp)`{8E8x%8lvbT zn33CnaOZ??rfCh676oq^5szH~m3(gB3N~GH{p(3gpbgm*zSo|ZQEi(nM#&ZF=09GG zg{paaVy;v2s1D70>EY|)kd-lbviXnm3Tm4SlLy3oJQgp>hw^u+Y@hrZ!fhn72JkpE2= z5wYLZT%|0g*4_Y3mf}8t-2S7Za*Ke(bHXe#+wZCeezF6rN0W#hu?s3dK{SwOq_Mu#l5?-x9It7?9LdPpTr%~Be|d}Ns>5Y~XoJIP&=pdJ zK+O9<`pA5v*>8TCM$W?fd+7}aY|ep+=W{!I`QqrM$aPT+FIg znlzWPQNZfCG&x7VTB8+QQYTptX0z97wVbfx`cy3;uKE_W4suo1jE5u60K1e2zpJwA zYT6J{eba=$2c&maDSrqFqkgqJDmStZ#8&gWUuyTEDekJPzt(Cn2tkkjy2l(g`GX4* z{p#@cM>FCzcObM~`{)+7c$)HECw8e0F9&3h8FWSZ77eLWduu%_=H9J;HBshiKes)f#wmZrnhg{-D6gN^J>USc+kVNpRHCV3 z1;oYux?BH!I|AWtbNA2zko!$1+0*IMVQs$!%2xJ7wV4T!H>3|4Baj~WiOyHsfK73H zqzA3?>UP*i#>4Tk5>i+BfDPVBbOMsOU`-lgz~=m9%D`Zv@$kquiprjg$nj(VoKx@v ztIqVBZrE!>uZncEM37lBC(5GvwU}6FV0X@~D! zCIz~oCRLJbx*S!9p5mHN->xJunc_xJuCB-I?n+ZZiD-N&j$(NB?eE$ztIl_SyOqttiRc4(Q2LM@2%68G zWymX@dmt~mj|NtvDL(K0f}Gb2L2HFdm^dZva7M$}2-05yGGOj|G1!~!J26aQlj)1< z&<}n!p=#sm@oWgh`n}`LHPEoy_!_G`KKY?JvTeG0Whi_yyQF_e0viK6JYh(gZr;q9 zLGwk!O=eIAm$Tj~p9^)wk;wNi3v2?G9P3Gl1gBy^GkL0tZ>8fW@NRGf#ul z4iN6FzRE@OKyiDy*0wDpbnOn^?r)Vbgh_6fIYDZ}P)>ZdB~|+w-#32SeSXiA$<(}z zabd4%#whi@VbV*S_J@hhqD@n*H)nk8`i5@7zz?kkt2q6x_NFG$`j5ZRc*V6SchuR2 z2e${9flC=fw|-1y`|zgwNfg&qps*dXdYh860C+gq`Qd8AY zMKivtidgU==ZoB?S~9Q`=mp)PQRzFv8AW_KVfoNmB!!V1fmT8(=Z?7%1{^z%tQ<#9 z3F>5CACr_b?p$^Tq#0<-gI_jNAh{yCs00^ipl<_+Y{S6>v`6E7I6a1-zrwp#ZFt}C~rIc>cpNFd0S3?TpFL!7in}8Q}c&*J+FWWHc;28q!%BEIY%0S zRq#)F{aad1Vk!;kB2w zH9L`gqpz>L)MuCB1}@cl^5n)KUJYFujsWo1^@9ykKJr}T*SIP|K&`!K2Jt)w)OqqR zNw@4+$++W1-%zkiB9LiSq;)u+wLTL0s0>zqM(H-lfict6q^ADfq_%kVL)>Ctk{^|8 zVcRo=ymf2#zPsDwQb9r%ob7I8W+XaED<@G}0o__8oh(cp`;$CcS2fvWRIl>}BOXZt z;A+^U3272ZwB%RW8V}57whj68|MNK2Au*Q(|=i7;wXt9FoVz3A9FceOS zU8gL|5d_^4F%=nZV6-^dbJP2svD+sG?;$jenf^I`GYjK2R}38rOq+lnf9ArhGWYF1 zDap&1EJe?((4jo=eT&z971`r*BM}5)1MIcfgm_j&y^U_wqJoUrX}Kh3t!bYk?LYuI zW#bJAC;3bsmI%L(6gd^kw1YE3ETHX&2LvFhlXiFCZW= zM*-Cg6kG*q#Bb50um)IspU0;a73R&?9ZA3|CJIyn%jJ=tOLY_+sH>1W2si@K60thY zR;gWbJ(G<7{{)Z(O7|pAXutGkTdQ@;gBaJVry;m=W+4wYxldfz-Xi`eUTRhDX8qAN zJDf>=LW}4-aW(hFawb9j+Vt)!f7sz**ib$0b&<29Sg(rP<+H5_g$gx{Y}6t^srSbW z*1tLZFY}|L*hI+KQtfl}57I5pe`t=(rDLpg#+=#$hpM(1p!E zn)B3fr+ci#{u96#f>+oqTNpnDYB^gA$K+Fle`1-(#EZ5=stc7qA750DLSp-rJNTkW z)|xaXtmKES%Cnq6%P2!qGYhFY(UZc2=0|k<2znv@)ajA>WyHJQQ56@Ti_wVfNEVc(;!yje){Pv{D!wFO%!et8Godim8fvVVNFneU%^DAPejec2n6^ALPEkJo7QLz zYU=D6tG=?^bvHjO;d7F_QglU%CWcbrcC+StebA3%`W8e-0&*NZ(Q{Q-69%4D0yju3}*DjcP zK8!Ut@NtBH3pNjmieSWBX#+E+OJ+`Hz6H;)>UY0g#eeDP#)_wxV@h-d(ml+xwVWHtn(g?Tf5BWW``BJ#`(Q z7Y_#qq(<;hr^Bhj49qk5A421!qqI$8jN$Hezgv&&lNG1q8=8Q3>L!#OClU~zb*i2< z;^avrAfj>14)WeH317a{H>&FcLb6s<3#K?V#rx``(l9+W+;2<~>su|PZG=&DDsPqR zqJG^k`;Fb*MPWUh_;$@kN{zh1IZ~36MMXtc%|3Brt^V%IIRTQ|hd*xGxWqN7=8Lcc zz7{Uqe5Cl2L)+-@{laArdGXd@D$}dE{>;PPX=*_h!8CPg{a*2#Y{J2rK6i|CDqdu7 zPQzc7$DB}fh%&8CLxHoZZ$91JEc~JVX|fw3W9)Y^ZASL=b2PzY!8dITu?bmQmeAQ* z)vW#eNei~L-m@}b0A&~s|CCfl`pl8Jeg4RvoqRg}v|245?*c>2y#+50q|Ka9s&!= zDjoUC%gaH4%j#?e3}xWyiRTZAeT@)(ymDS?016|2VlSi{Oa~AS;o7vkjrNutKr?ECLPw9JngW* z(Rgd2Fe9HS`3P+57i~HDbaUn9?@r1AuJP1NztI7(vV-3$e#EjB!fY6~|JBe%vL+1c zGXe&#LubIq+lXaXk|iHa2PEABdS*0LSYZ|53I-+7lrw!;$1J+5Y|_ehSm)nvnz|>t zkSWqv4Dv~)%eT?4LYbbvV=b(1zIZWX!6}(!*#sm|04NhddT{crVn7Ev znqnxy-a;CzgY414?bCX*O;aX?C=vsZqc75l#74vMWk9q;_%a))+=coYTwkBCf`fzO z{Jo@dSww#=X(3lgOhp_siV#T$mY~JNg^(9~vo4reBt3B8X0RL<%#OMFnE$8Z zLjh6}0Jgg@aW(Ef2}4aXN-{FEN<*+8$X2i}-ZqpCO%oXz^%uHzSc+R^F2!Q%3nq&-Wl(+IdN_y{(=8SjO_5^&v#k$RaTFs=+}u1x zvWF7=@}GQwBApOu-g-MKY~@xz8+a{Y{EAe9kPt*pB5R0lY1b`Fi-46bpdaoA%T)_s z3k_Hd8h=5gj+zxuYreuts8VrB+q1+41O%w-`!qhO#|5K6gx{f9V;2>j@Z5h5v19F3 zLNlv>e0z?$^yWqV;J@VfMfeH<5jKDMG?#jJR3bN8AFu`M?30a_fMPW`3KuO;5ZS}! zhAK^x3CQGQY&d7KXG2Wq{@7nH>DeBJ()A7wzuRR_l?h4zM%N+X!fq}$pRX2Ac<0By zZc5)^gT$^VlIjOrAv6!d`SBBN1Hgpum8;B8()=1mE-qXU&UYBhYCww6kR|9-4Xvf@ zy9H_KlPOJt-LtfG&>P%K6#wKp!9yS~9k)<7-fY2w0~9E%N;T0FEGcuH(hNt0REbrx zu%{fjX3Bwcd^k@N{P1s;55V9<|M|J>bIzL%g};>Wa(KamC?Js15sr0cYv?p#fAd>=i~foh0$CG*M0aLQC?p%TX*Nq~!NB*TM#=ljqT)1ZvslfHL?%G^mDb5bYFUyG=PxW0VzH8v;lUQ50QyBOL zE>4JM0c^#*ScYgo9$P*H`s*ocQ;>N{WcI`Uq=Yb70|`^p8-t+9;NRnd?akGy+(C!i{QW+GOW^| zg$3~E6kZ70LzNqHLIZE(n&kpO+6h6$YCT(z7W2o zKYLWMpEM-XnixK+9{l|_&7{9Wo?-UVL~*k*7b*%f(wJ^2MxM-wD9{p~&>{K}$?{P| zq801KW5|eS@xbS6Y}T>>z7*Hdo#SR)&DP^G=#AaOgWJ8tQ}u5ZqqbZP4n&tttnUJ! z7w0GAey6cMrCYD?x+}S&qwfS#|I^Y{2Q>A)eE}r}q(u>skPc}WAsxc#?(T-sL#08G z9Nj40-YEGKlaDAd(XM&IZr?5`FN!^RxAhABkzQ>;-QY< z06%^$O(|FXvb71JY!a|$d?wD_;izucY$EuugxHc`XO$|ypkd0&myd{4Y3|KMToc=f zY~IP=j)x))mZaRKr7`oJEX%|W0o3XFM+vtKlUoUMeUq z=JU2D0nXaiG8wX0j|TqmYAFXH9P$MaLKb<{cbJJbXcKsbH!1@CrirBuYw3`0x?0iu zVA1faw);D|%$V~_WWu9vrRh+eq=s}q5OkKxZK|G>R28M~xfEd1QXDyBoHiEY3YIqh6u< zBAcWc^ZTIFv=CYO-CtEvlxxG%s(?|Gi|%y3g>9p?;NLR8A@-f%*W*z{(IssW0(1(x{jQ- zsIaex;WD%Ic_xOzZF&+69VTZ=laFak8fffnB;jG@oK!e5kpraB+->0oUa0i176;#B z8T5TADP;zuS?-PLV5InP$3&Fi)?{U%fjcVm{#=VsPTzlxKiEyLJEU`)iuk7$>NL$K zo+3((-Cxj{r8~7@6jCj`fKD_Be6>t`1)ADa!OpP+37NtA@g8E;Ij_M?HwtI#_IV(^ z@q26754k(EJ+pCEHAzrn?=>MSs(b*zn$(;qh`;*QCLB0~Z*wh-*NDfv`;c+C*-PzHn(g44hOrV7+#MhZ?!?*=VU2@) zsgm+2^AQ-3n#fV%GS2}Tas^8RcG9w!Hc3G)jp{hSGErs$C{{+h=FpCNDJ>4EA(2*N z<4i2e-{v%`zf@)5mvU?YIw?eY_eJA}3_AV$NbXzqv|y zBfmQ@SAB-5THp*FcLJBEnE5H1!ZUX(M+313Tgn?-D zO-?$qCcZASq!)ajFeBg#PraeVIQh*Mmt@#7-_J?ePW1Y@_|9yWs59D+y#rQ0dS8mF z_&Hz*y>hH)AE;XMi5qG$+Q5-y+CYM^;GMGf<8Y_f82QUbB`IFGJ;p<^EOgv2F^rL9>Rp{nC+?Q>G^Lzsbom4&nZbo-&HQm+<)3HSU~m*c=Ze2) zTWHoejVpIeg$w1GRhSPJn-ihxV#t3h?-VC++t0y(4{sY&Brz}DYsW%_IKzE=5`3yM zxr|OcYMEn|73UCcs@D|ZZs(W77#p4;074*g>Sm{=#ZduF6rgV2Q?T@;waeSXG@VxG zU&6^10xv_B!U0S1i@+zFBxXj(bl+FcB=;BOVFLudlZ!mklO(C4s@EDGuqJTY;%_2#D>f)XWdms>ak<5T<{A zqGcQ5v{e-G*n~gng-udGg26GOgRmVl3Ism10!p~DN)ztR6_I3sELWF%W-Kph_QMTljl*)uo4~&eb#_SDv_=QPN>S+ic&AZM1eN z+U|G{@FalZf01kiMpuy;s7alB9U)8PdaJYLBSI=9p1AjPdZ!>_mgPno_@e1%!9~QV z6=oHtB`iGin%D6Ajq^B=ZP?#$<6fb5sPd~%P${ptCq8-1Hc>b02jq^j07p5i?$H!1 zsE{M=+H7?1#EL(fn)BXjTs(0%;N|}C1Exgq$jd16LdQ7?=%_b50g|(Xi*j5drvsNi zJH(ZHiXq0GjCV|Cm|GW_4_CPVo6M34qlI;`U_KA@Vi}?RE1+d_XR}JErWF$o91S91 zta2mCAlhAMDPjiedsXqF2JN1sh~(*X9X0fIB4a*uEO8p-|9q*-;1~X%eXJ+e9}T24 zh~1_x(XeCI(z zflxk;3&(_N>2u7o`DNb`m$u92%02<^u)A&X0Ho?OeV{hY4aKJpK;h!!6&4;~v8F@f zKsmW#yt9qgvQ!ESfnRtn6FxuJSiqkeVn3e|10O)oXhEjx3jSMvfgO65Wnl3gn+xg= zc^X{E(;g>T%p4!O*f(vg=E-~T&X;ZW?UpX3#C zG%#iNfrwP7^C7AC0x-OVdpw_jpJi(SRbOo2-4}?`<@Z52!F0P1=TcTb*N=)#v_O(S zysWmY@8#ZV(tO6@3W+nGB`h2Ld_X!?fW7VQzI63sl{}QViD$lNGs1q?g$|U2eCaBl zV2Sa|I{z6Jp=L-1$I@(c8292CuOz;MV?_UU$bQtr7!*XcLQHGtV}@9WpZX|^y?E_F z#y2s68`W>~!TG~&xEukEvM%*Ymyeul&Os5p0Mh#NcLQjgaI)pj*5E+;tM z>p9=iW|13Aasa>}8#&8Ld(<0=lQT8+1|3ym62Fb z`hWacTLYSHQLYSGu_|ABwj!^tu*G07*tk~wQ?+?8=(2XRweiiwsbcS!u8|Kwg)ZDynX=iCj(FO+V+~;&yQ?=2g5Ba ztgO;uiZ28@o&ET2p6SQf^C^>neuM0Ug}1s`oa<6zv$UY-=vHS$&`j^Ara7o9_)8Ps5-iSjN98AeLvuhvLE!({`v2X@i39;OiZv8r#tI_-G z^R5?DD|(p&5q?Ugw+8fQq={2Q6c&Sa_Hlnq7;deuT*JdVn*jLL_$#*6AQXL;{xYxk zrt#2uk9RvG{i;6*mIkZ+{bhGW`{Z`Xlg|}vwsCQTN5R!a6q+Y@i=C{U)tDwm1qIC{ zl8GyjCf1vOr^pkbNePbdpiY)p9&~aCL9gjc67>NW`-Q#ywT0!)6OpRY$Hon0T@o>**(~d2L>~Uq^n(BE{FC3Kh)G z5F*}FPlGS}WmzWG{evUrXfmA^z5ZRVgm3AY{fZXqQ#gs%C&9;0tKRF5-g+^n0PPY=vjvRz8ZqBu!11yg; zP?vd=&I7iC((d^+=Y!uH5k3i|2?vnNs4 z%8gFpa8p(M8z}U5`T0tO_g1zcV-`l1k12s+Ig*)$MGkSbuw|?ahCaG)1wEZjX>6`( zD%kHt1>H*C8t2E4CdI@f^%C@suw6A`f3-48Jauk~^faos1B4hLoAmHfb$xtK#^;W+(1OhJ0Rv_=P)2cO+|0Sr>c}LiisQPMw{?Q_ z6cdO#s}s93UdhCG&5t1;dyg?bR}4)v$+=>hyEWJV!Af4HZB#9q_^tRcluE90ikzGn zIe6l<#{aU*WIzAj<{y_pZE59}ws5o0#;F>|b^COBVq*(hDYugnU;x_*ZK{1Yie_N= zb}f*IT(%tL5D>|P^%%SjPq)8ty?cGg-i}!%Y^B7{v9M5ADh!8_1jbU|D z$c%Otj7OOFHKEXNAt&;ec=oFe+ie8_p`U8Q(9z$jEYdnvO(IxZme#!&iedWWPFv2- zlb@Cw{ORz~9w5;2qPd$)_t6R43ts_ol=zW5AWeP$!1fA!X_yxubojO*3z#s!Yyjc% zh^+kY9#7bDD^z^+`tgyEFXs@YjioM@;P9$s{$h*!TP{pIA}o5AmR{F%4~(kjl&5Rr z<4e--{C4qX^v%4WFGnF?gM0$&wIq!sw?o%Op1AVn6{jizkL)G4tkNRvs;gdCU+DxA zZp{GwlM!V}ASrHWDEojya*y_QvJ0L|bAuRH!ALXk@otId7phLhQwHG6P5W`P1Rzfe zsotwKLvj3Rkh)Jve$Srw0JBytyvIW}O}er57PS4=egAZ=o$Y1bpEO<3N=($HH92K3 z4MiG&=iAM|I;?Sr@<&ncXXJ(Km*37YD|YMooU_`k}G~nJ#tiISm~fmny-K3DnBGfp?fmO^!OG za*sUMvAD`2CO_YsjI2f@$FLHkb$cj5hX9{s++g1mj*?}wDVN;D^h~@2b0)tJwjlt1*&-^uE3sgD7llW!qf)6=Fx7a%3 zCO6z@!~46T$xVslL2&)j+X@UCJ;&k~6yzHQqDw;{q$XyL(h1mIrv22FTz**{a6U4H z%02p#VtxAcFe1W@vva~W@yja{|0N3L%{<+dGjz~VE4wcI6wV)u8x zVm)xU+2xqk_vxek{o23piew9ukRwE4%rqMoz|<{bu;F41Dxb52Y(D+9F%KV||C1z+ z@NM&F#g@&<-M^CXT@D=aJK1(UJZbyl@gq(V$SAm?vWAY%P0C$O4>tBgzIC1#dneo! z^V$do!@u`hD%(oZ{}XTI_*<2gw>*PRiXQy(8L-{b%sep1fUVNG6Vj>mcu3(i^cp{( z;^S+t0!I8ouy~>h3iIZ2^UiE;*?On+VGw$d{M!cfAtCvKC4}>!3o$&mJ~g5?_@>I@ z_3RF2&9=6zP1B$unO`3LwdbXomkvXz%t|snXnR!A(>cHS)dKhr(9fzib$f0(_$BTn zaw(kwKeN0L@@pCeG4S={&xXD8UI__33Sd$8N(XEAenKEp9=9HUb;WLO3Y_<{^Pv?w z55Hecu<)T{2}$ceJ+q<}1VuxMQ%w>uA*zQ2Oxq> zxj^D+-K!W3#Hiq>uv2v3o+$BD{7}?dhV~7(x;Bm+6cp6ib&jVbx%iYHvqc?916#ex zdplZf{V`nsFJY~=-1Q6=i_k8<>Ac}{T?lMlz%DGb7HU6GM?Ga$V1?Dm@Y8CDL9WV} zKnxYX`s$bO0HzQlP!`ipHZkd};H}WdsfwPgz6$5?o6)hAEOzdkylK6*9=chW`n9Tx zNZj>74Go3Y`)0Q>@J8^p+1f)y)F$ujL!5!kOB_?3Q7sDf64(F7Nx@gIpo?pqppliN zFioLHnm?BpW2m19j!udBbIIMVoiafeFRRzQYi9hU-DxDnHE8EZC`|dYvsI1#95%P! z5YUo{K9q(YZIsWtDVq&@Kp2%-SpsxA_W!n)9j0#`)3?e4`Q&3MkgJ!IC(jSTuu1^MIcY8-T z0AIth#H5(7IjrQ;yWjrGlpK$RN@Nkt#Tjw%u=_G_1NVK;A>0w#sy6wSvr*=DDC6U`IjP z{&~5|BFy$+rm`?lYMJwqZg7C|YZia-2+oVd*xxooWL6tnJ0z&GnWNWl%@f)RoZuF; zpwGl&sA8D-_?B>rfC44OqUA^*gZ4cPo#5_2X){%eOQ8$k1O8NQ(xiq6={Za!P$!FK zmgk@TXq}XGzi^%(XuP>G7OlW^#$YUUC27Q8Fmh(V^q+G#eL(L|x9>*3y*DNhQ0So~GTGPp4(P)yi6B&$ zQqZoXVyrV^7ZsfA*eWjK`L#Io=txh0hgaw@N%ytZpQ;C+)$W~$nicG>GWOEHOM%oq z6P=E=oVP1Ix#it6Py`;D2{-^EXRPX6qJ`l2ugPflFO4-r$q12pX08wqiSxm%8ZY)% zUk`~mt-2I3ul=WaJu*JLa*d|w)t7qcrgt@oqfM(F%MsjKsA`jY$yyrKNlB6vpTIPH zHex)ozcb941_@uzHp2l*;^F0+Jh9ZfV&IeHVB@&^<*48JLDlDX?h->Min4k%=npHp@^yVECg zq|^hbKRa7;&|~{2S;`yYs=ZT8CRwn_iK6b{KV93!pCiw{!TjxLL^=Rnvk!@DwpA(w zg1fJpUmJwld;~S^ZUv-;2-Lk&=Dw!2@u*7>_HU61+iLq^0)^o2ey{mqVFn%wDVmSX zF~~6_$*Jw*m!FSqPV4VwIkr-pnpau%ZbR?eRHn3_?ctKMj7)B(6{C(>`Mjsi`Dcn= z87Tdd*5e?UbB-;P_HBA*v1;JfXKj8b6YS$*Pv!EzP#9eviSkvJ-A%qf z#+Wa3(LB^RV#^Ur(2;-;R6z9HwE{NQ!0WXBL%)FLv%NwFwA>gGgKgIMre%*C;IqaK z#M8K|iT*Qjs#cl)29csBTknnGH~Ug|BxuW$ti7%M#`+$W9u?vDU;QP;{n%#H zpq&JLr!9gb;ToD?U?;rewd-~C5YBz8nDli;z(y?I|5aN5+%lX?=&^jF3jXzdur_mL zrOehRnz0V&L#5k}kf+|Q3*ex(#S}_j=lESD0160-!gF^I~)WFpH5OXDk`9HPz7iZZVzVpNISJV>} z`e!~~sU=Q-Y955)t1G|EJ6C`s^6G!q+;m2W|J#{7BI&rL+aJv-^}LFUctj zZE6`gL`4&%jl3 zF&3Z#ax$x_t4+V4%R`b=*k>tlROMWt;{dUg1`k%dJ#cfXVXz$d-<#VS z@&mf^Ev3j&RmcDwVExZ;ZJ$SMmu5c((Q8jp=XNV4sxS@#61o#uU}%F<6gIbQkG7Vv zz0WC2#RYP8M1;O%CAa)N#YC5+LY<2T&;=YaI;B-8;Yf>xbDuKUDx00%VRzYZ5Tpat zyZ4D7Yh`yVvGQgryZc*BUSyM`L4nD17u}J+VGW^jg`~#;qN$dSbvk|3rUKA? zT+(T-3!vlT=`Km)81hA;3U_6;$RVa_o2d^t0U&-`GNOi(-aTCjmwime%CCU57oz`f zpRy5*b=({4vHFxc;{jPYB%hcl^36}W@0$jn*=ArWE+{HW`4UOvCEXNLq)AIMyB&~g z^}{;s47{;b!<$V~7_FZMORLBPu)*CE2m~Y1l-+a<#u&<6Qpa zmO)M$Rrk_sbP&5R;;XH|{fCD!GezB0*oCg(;BAEYw2_;u+E|<&^rgcVX{pZ;u|` z7R_ak)9L|VFx1eh%vck&8)N2(P(|6(z4M%4bG+a_WiAELVF4d3ARvsn50Dq7H(kZu zQq7WcbeBTGeFD73JtEYR#cP6PW%1ojTfO3%<7^;ZAJjHemJNLZ` zlB+>^_nIWe)}9F_LZ=A5p>?Gf`}i^EJTwA7f476rrom1qA>VHyk_@|jX%-5%Z2k`B z(%S26JU35Se=vX_l3K>66b4aWJM*4v;t-p3(Y>$yl+M$<&jQ`>TI9C;P6VB@KxfP& zE}<<|kT~(zxoQYv99eDs10W5J2GncgZuJI*(F^EX1`db2n;-bqlw7C>kj;ZVs+yuH z)XBI3Aqi>vc#Ew(Kb>#I3g>g0wAVg816w-yv5mA$AK1GYL8myO)Md zd(sz}7vZ`Oqbl!!3)~X-Po3S}Q7>jW<&8bjxK8AZ=0Fw~FhXKfaK2F{|E@z8WB_>D zkhl?o%d)hm954}dU&EEA8NOZJ8!>)S!Aq&m2c^0XlgvnbBG_BGLi&~ z2m1@)s+!1rut#6~e;(0O3t{4akp@j67M8OAxh{izvWZg}+DL6%PND2l0Bn7mkj-|3 zDTgK~VD~9oQ3?yVz%MEH!at`SQvfjh|8z7iS^3>3ByKpZ@}G#ch$tMRg(*Dqor5nk zl|cL&M|HI0=}tj3BY-IH_93Rwm|o);s}%Ibs1lB0zQ!aSNi!-_=ejE>C@7OUC?JV7 z%%P=XL2mcd*$p+U8yQOhsI)7dOAqtDF%SJTXe3}tcSSyk(%3FiBu<@dfamlRI zZ!5%0IP-WX12lt%%9}M%Z$Sri5cK^f$#nv$xMYWxt9KCxs?jW%b3s-ENGC}n6Y z*V()Go+K{_$2v*{@Kxr&qO{T$$RV;RuakD@UlvHv2p#!9k@!sfVZNh4mOcl)-w{7J z%eb~V>$S$98eKEHI`z+=s&L8C8#1v|7Zw5jZQ+54DSRX#-}-8SLkajnac?cY`o%9g zv&gZ%==4!-tpbS{EyQG_Ldb;%d1mE5D-pm>>44|2sJ+wrw{K4`@x1hOr#&D&CRJ;K zl>WIK?MM$pJhGaSe>9(GRdUIsVuHNa{*)fhn#ewmRDt3%2p3Os%M4{d>3t(qJUQmX zc=zRhVL51|&St`WWhWQ+<~Sl^$H4)0xy&@aZ0W_;G^%TKAfTUf9DQg+NOGnC_85p4 zeJ1GG7^`k1WY0mXJWvuWk%07oOeE-YzPj*#LQ9ghR8G!Wtw|ILFj)hAzaAh}saQlb zr=D_@V__`9XD$9pIN0qW;IKxdV6gN<>7K4G2V253Ag@oBe2Be!yRnK*c+#6nVCeFY z?%Yb(A(n!=Oc$R6+)uqT8K|?f-@yHfD|EpP;;CxV_jV~$kB-j&-{W&b1cR@-4+{&6 zZ&%&g?PwL$c@u|wOs3y*rxxRIaJ`429DC)yl4Q-{3SZB2zF2*0^a=Dt8fNCe`0jRb zJ3w1IEIYfA-dIp!(g1kNR=m_is!AQvBY!R6#zJSZe)~T$YrlW{NJ!^{dueF}2Oe0W z6Ur!dwNh1!vc@#37xoG=rfpIg)u_uT)(5%1C0yN~_qt56c564oSllDsP>NQG{p>8H z{mfpb;bV0SSDU}Rzj~{V+KW@+wZ9xgk7c z8UXXM3NPveGFk}EZdJhRPtbQ^z`~y~*v5FKa|ohtKu{nGY1%}*?(&h;)NL{;E&to! wjfb(c_w@kR=;UBYA^L1;Gu=tb^lQm2!G89xD`~_*z#ZUHR|YFJDA>IDKMZF*3;+NC literal 0 HcmV?d00001 diff --git a/aloha_ws/src/moveit_visual_tools/src/imarker_end_effector.cpp b/aloha_ws/src/moveit_visual_tools/src/imarker_end_effector.cpp new file mode 100644 index 00000000..ee6210ce --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/src/imarker_end_effector.cpp @@ -0,0 +1,329 @@ +// Copyright 2016 University of Colorado, Boulder +// +// 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 the copyright holder 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 HOLDER 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 + Desc: Class to encapsule a visualized robot state that can be controlled using an interactive marker +*/ + +// Boost +#include + +// C++ +#include + +// Conversions +#if __has_include() +#include +#else +#include +#endif + +// this package +#include +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("imarker_end_effector"); +static rclcpp::Clock steady_clock(RCL_STEADY_TIME); +namespace +{ +bool isStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, moveit::core::RobotState* robot_state, + const moveit::core::JointModelGroup* group, const double* ik_solution) +{ + // Apply IK solution to robot state + robot_state->setJointGroupPositions(group, ik_solution); + robot_state->update(); + +#if 0 // Ensure there are objects in the planning scene + const std::size_t num_collision_objects = planning_scene->getCollisionEnv()->getWorld()->size(); + if (num_collision_objects == 0) + { + RCLCPP_ERROR(LOGGER, "No collision objects exist in world, you need at least a table " + "modeled for the controller to work"); + RCLCPP_ERROR(LOGGER, "To fix this, relaunch the teleop/head tracking/whatever MoveIt " + "node to publish the collision objects"); + return false; + } +#endif + + if (!planning_scene) + { + RCLCPP_ERROR(LOGGER, "No planning scene provided"); + return false; + } + if (only_check_self_collision) + { + // No easy API exists for only checking self-collision, so we do it here. + // TODO(davetcoleman): move this into planning_scene.cpp + collision_detection::CollisionRequest req; + req.verbose = verbose; + req.group_name = group->getName(); + collision_detection::CollisionResult res; + planning_scene->checkSelfCollision(req, res, *robot_state); + if (!res.collision) + return true; // not in collision + } + else if (!planning_scene->isStateColliding(*robot_state, group->getName())) + return true; // not in collision + + // Display more info about the collision + if (verbose) + { + visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED); + planning_scene->isStateColliding(*robot_state, group->getName(), true); + visual_tools->publishContactPoints(*robot_state, planning_scene); + } + RCLCPP_WARN_THROTTLE(LOGGER, steady_clock, 2000, "Collision"); + return false; +} + +} // namespace + +namespace moveit_visual_tools +{ +IMarkerEndEffector::IMarkerEndEffector(IMarkerRobotState* imarker_parent, const std::string& imarker_name, + ArmData arm_data, rviz_visual_tools::Colors color) + : name_(imarker_name) + , imarker_parent_(imarker_parent) + , imarker_state_(imarker_parent_->imarker_state_) + , psm_(imarker_parent_->psm_) + , visual_tools_(imarker_parent_->visual_tools_) + , arm_data_(arm_data) + , color_(color) + , imarker_server_(imarker_parent_->imarker_server_) + , clock_(RCL_ROS_TIME) +{ + // Get pose from robot state + imarker_pose_ = imarker_state_->getGlobalLinkTransform(arm_data_.ee_link_); + + // Create imarker + initializeInteractiveMarkers(); + + RCLCPP_INFO_STREAM(LOGGER, "IMarkerEndEffector " << name_ << " tracking ee link '" << arm_data_.ee_link_->getName() + << "' ready."); +} + +void IMarkerEndEffector::getPose(Eigen::Isometry3d& pose) +{ + pose = imarker_pose_; +} + +bool IMarkerEndEffector::setPoseFromRobotState() +{ + imarker_pose_ = imarker_state_->getGlobalLinkTransform(arm_data_.ee_link_); + + sendUpdatedIMarkerPose(); + + return true; +} + +void IMarkerEndEffector::iMarkerCallback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback) +{ + if (feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP) + { + // Save pose to file if its been long enough + double save_every_sec = 0.1; + if (time_since_last_save_ < clock_.now() - rclcpp::Duration::from_seconds(save_every_sec)) + { + imarker_parent_->saveToFile(); + time_since_last_save_ = clock_.now(); + } + return; + } + + // Ignore if not pose update + if (feedback->event_type != visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE) + return; + + // Only allow one feedback to be processed at a time + { + // boost::unique_lock scoped_lock(imarker_mutex_); + if (!imarker_ready_to_process_) + { + return; + } + imarker_ready_to_process_ = false; + } + + // Convert + Eigen::Isometry3d robot_ee_pose; + tf2::fromMsg(feedback->pose, robot_ee_pose); + + // Update robot + solveIK(robot_ee_pose); + + // Redirect to base class + if (imarker_callback_) + imarker_callback_(feedback, robot_ee_pose); + + // Allow next feedback to be processed + { + // boost::unique_lock scoped_lock(imarker_mutex_); + imarker_ready_to_process_ = true; + } +} + +void IMarkerEndEffector::solveIK(Eigen::Isometry3d& pose) +{ + // Cartesian settings + const double timeout = 1.0 / 30.0; // 30 fps + + // Optionally collision check + moveit::core::GroupStateValidityCallbackFn constraint_fn; + if (use_collision_checking_) + { + // TODO(davetcoleman): this is currently not working, the locking seems to cause segfaults + boost::scoped_ptr ls; + ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(psm_)); + constraint_fn = boost::bind(&isStateValid, static_cast(*ls).get(), + collision_checking_verbose_, only_check_self_collision_, visual_tools_, + boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); + } + + // Attempt to set robot to new pose + if (imarker_state_->setFromIK(arm_data_.jmg_, pose, arm_data_.ee_link_->getName(), timeout, constraint_fn)) + { + imarker_state_->update(); + // if (psm_->getPlanningScene()->isStateValid(*imarker_state_)) + //{ + // ROS_INFO_STREAM_NAMED(name_, "Solved IK"); + imarker_parent_->publishRobotState(); + //} + // else + // { + // visual_tools_->publishRobotState(imarker_state_, rviz_visual_tools::RED); + // exit(0); + // } + } +} + +void IMarkerEndEffector::initializeInteractiveMarkers() +{ + // Convert + const geometry_msgs::msg::Pose pose_msg = tf2::toMsg(imarker_pose_); + + // marker + make6DofMarker(pose_msg); +} + +void IMarkerEndEffector::updateIMarkerPose(const Eigen::Isometry3d& /*pose*/) +{ + // Move marker to tip of fingers + // imarker_pose_ = pose * imarker_offset_.inverse(); + sendUpdatedIMarkerPose(); +} + +void IMarkerEndEffector::sendUpdatedIMarkerPose() +{ + // Convert + const geometry_msgs::msg::Pose pose_msg = tf2::toMsg(imarker_pose_); + + imarker_server_->setPose(int_marker_.name, pose_msg); + imarker_server_->applyChanges(); +} + +void IMarkerEndEffector::make6DofMarker(const geometry_msgs::msg::Pose& pose) +{ + RCLCPP_DEBUG_STREAM(LOGGER, "Making 6dof interactive marker named " << name_); + + int_marker_.header.frame_id = "world"; + int_marker_.pose = pose; + int_marker_.scale = 0.2; + + int_marker_.name = name_; + // int_marker_.description = "imarker_" + name_; // TODO: unsure, but I think this causes a caption in Rviz that I + // don't want + + // insert a box + // makeBoxControl(int_marker_); + + // int_marker_.controls[0].interaction_mode = InteractiveMarkerControl::MENU; + + InteractiveMarkerControl control; + control.orientation.w = 1; + control.orientation.x = 1; + control.orientation.y = 0; + control.orientation.z = 0; + control.name = "rotate_x"; + control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker_.controls.push_back(control); + control.name = "move_x"; + control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker_.controls.push_back(control); + + control.orientation.w = 1; + control.orientation.x = 0; + control.orientation.y = 1; + control.orientation.z = 0; + control.name = "rotate_z"; + control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker_.controls.push_back(control); + control.name = "move_z"; + control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker_.controls.push_back(control); + + control.orientation.w = 1; + control.orientation.x = 0; + control.orientation.y = 0; + control.orientation.z = 1; + control.name = "rotate_y"; + control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker_.controls.push_back(control); + control.name = "move_y"; + control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker_.controls.push_back(control); + + imarker_server_->insert(int_marker_, std::bind(&IMarkerEndEffector::iMarkerCallback, this, std::placeholders::_1)); + + // menu_handler_.apply(*imarker_server_, int_marker_.name); +} + +visualization_msgs::msg::InteractiveMarkerControl& +IMarkerEndEffector::makeBoxControl(visualization_msgs::msg::InteractiveMarker& msg) +{ + visualization_msgs::msg::InteractiveMarkerControl control; + control.always_visible = true; + + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = msg.scale * 0.3; // x direction + marker.scale.y = msg.scale * 0.10; // y direction + marker.scale.z = msg.scale * 0.10; // height + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 0.5; + marker.color.a = 1.0; + + control.markers.push_back(marker); + msg.controls.push_back(control); + + return msg.controls.back(); +} + +} // namespace moveit_visual_tools diff --git a/aloha_ws/src/moveit_visual_tools/src/imarker_robot_state.cpp b/aloha_ws/src/moveit_visual_tools/src/imarker_robot_state.cpp new file mode 100644 index 00000000..f947229a --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/src/imarker_robot_state.cpp @@ -0,0 +1,391 @@ +// Copyright 2016 University of Colorado, Boulder +// +// 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 the copyright holder 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 HOLDER 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 + Desc: Class to encapsule a visualized robot state that can be controlled using an interactive marker +*/ + +// MoveIt +#include +#include + +// this package +#include +#include + +// C++ +#include +#include +#include + +// Boost +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("imarker_end_effector"); +static rclcpp::Clock steady_clock(RCL_STEADY_TIME); + +namespace +{ +bool isIKStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, + moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* group, + const double* ik_solution) +{ + // Apply IK solution to robot state + robot_state->setJointGroupPositions(group, ik_solution); + robot_state->update(); + +#if 0 // Ensure there are objects in the planning scene + const std::size_t num_collision_objects = planning_scene->getCollisionEnv()->getWorld()->size(); + if (num_collision_objects == 0) + { + ROS_ERROR_STREAM_NAMED("imarker_robot_state", "No collision objects exist in world, you need at least a table " + "modeled for the controller to work"); + ROS_ERROR_STREAM_NAMED("imarker_robot_state", "To fix this, relaunch the teleop/head tracking/whatever MoveIt " + "node to publish the collision objects"); + return false; + } +#endif + + if (!planning_scene) + { + RCLCPP_ERROR_STREAM(LOGGER, "No planning scene provided"); + return false; + } + if (only_check_self_collision) + { + // No easy API exists for only checking self-collision, so we do it here. + // TODO(davetcoleman): move this into planning_scene.cpp + collision_detection::CollisionRequest req; + req.verbose = verbose; + req.group_name = group->getName(); + collision_detection::CollisionResult res; + planning_scene->checkSelfCollision(req, res, *robot_state); + if (!res.collision) + return true; // not in collision + } + else if (!planning_scene->isStateColliding(*robot_state, group->getName())) + return true; // not in collision + + // Display more info about the collision + if (verbose) + { + visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED); + planning_scene->isStateColliding(*robot_state, group->getName(), true); + visual_tools->publishContactPoints(*robot_state, planning_scene); + RCLCPP_WARN_THROTTLE(LOGGER, steady_clock, 2000, "Collision in IK CC callback"); + } + + return false; +} + +} // namespace + +namespace moveit_visual_tools +{ +IMarkerRobotState::IMarkerRobotState(rclcpp::Node::SharedPtr node, planning_scene_monitor::PlanningSceneMonitorPtr psm, + const std::string& imarker_name, std::vector arm_datas, + rviz_visual_tools::Colors color, const std::string& package_path) + : name_(imarker_name) + , arm_datas_(std::move(arm_datas)) + , psm_(std::move(psm)) + , color_(color) + , package_path_(package_path) +{ + const std::string node_namespace(node->get_namespace()); + // Load Visual tools with respect to Eigen memory alignment + visual_tools_ = std::allocate_shared( + Eigen::aligned_allocator(), node, psm_->getRobotModel()->getModelFrame(), + imarker_name, psm_); + + // visual_tools_->setPlanningSceneMonitor(psm_); + visual_tools_->loadRobotStatePub(node_namespace + "imarker_" + imarker_name + "_state"); + + // Load robot state + imarker_state_ = std::make_shared(psm_->getRobotModel()); + imarker_state_->setToDefaultValues(); + + // Create Marker Server + imarker_server_ = std::make_shared(node_namespace, node); + + // Get file name + if (!getFilePath(file_path_, "imarker_" + name_ + ".csv", "config/imarkers")) + exit(-1); + + // Load previous pose from file + if (!loadFromFile(file_path_)) + RCLCPP_INFO_STREAM(LOGGER, "Unable to find state from file, setting to default"); + + // Show initial robot state loaded from file + publishRobotState(); + + // Create each end effector + end_effectors_.resize(arm_datas_.size()); + for (std::size_t i = 0; i < arm_datas_.size(); ++i) + { + std::string eef_name; + if (i == 0) + eef_name = imarker_name + "_right"; + else + eef_name = imarker_name + "_left"; + + // respect Eigen alignment + end_effectors_[i] = std::allocate_shared(Eigen::aligned_allocator(), this, + eef_name, arm_datas_[i], color); + + // Create map from eef name to object + name_to_eef_[eef_name] = end_effectors_[i]; + } + + // After both end effectors have been added, apply on server + imarker_server_->applyChanges(); + + RCLCPP_DEBUG_STREAM(LOGGER, "IMarkerRobotState '" << name_ << "' Ready."); +} + +bool IMarkerRobotState::loadFromFile(const std::string& file_name) +{ + if (!boost::filesystem::exists(file_name)) + { + RCLCPP_WARN_STREAM(LOGGER, "File not found: " << file_name); + return false; + } + std::ifstream input_file(file_name); + + std::string line; + + if (!std::getline(input_file, line)) + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to read line"); + return false; + } + + // Get robot state from file + moveit::core::streamToRobotState(*imarker_state_, line); + + return true; +} + +bool IMarkerRobotState::saveToFile() +{ + output_file_.open(file_path_); + moveit::core::robotStateToStream(*imarker_state_, output_file_, false); + output_file_.close(); + + return true; +} + +void IMarkerRobotState::setIMarkerCallback(const IMarkerCallback& callback) +{ + for (const IMarkerEndEffectorPtr& ee : end_effectors_) + ee->setIMarkerCallback(callback); +} + +void IMarkerRobotState::setRobotState(const moveit::core::RobotStatePtr& state) +{ + // Do a copy + *imarker_state_ = *state; + + // Update the imarkers + for (const IMarkerEndEffectorPtr& ee : end_effectors_) + ee->setPoseFromRobotState(); +} + +void IMarkerRobotState::setToCurrentState() +{ + // Get the real current state + planning_scene_monitor::LockedPlanningSceneRO scene(psm_); // Lock planning scene + (*imarker_state_) = scene->getCurrentState(); + + // Set updated pose from robot state + for (std::size_t i = 0; i < arm_datas_.size(); ++i) + end_effectors_[i]->setPoseFromRobotState(); + + // Show new state + visual_tools_->publishRobotState(imarker_state_, color_); +} + +bool IMarkerRobotState::setToRandomState(double clearance) +{ + static const std::size_t MAX_ATTEMPTS = 1000; + for (std::size_t attempt = 0; attempt < MAX_ATTEMPTS; ++attempt) + { + // Set each planning group to random + for (std::size_t i = 0; i < arm_datas_.size(); ++i) + { + imarker_state_->setToRandomPositions(arm_datas_[i].jmg_); + } + + // Update transforms + imarker_state_->update(); + planning_scene_monitor::LockedPlanningSceneRO planning_scene(psm_); // Read only lock + + // Collision check + // which planning group to collision check, "" is everything + static const bool verbose = false; + if (planning_scene->isStateValid(*imarker_state_, "", verbose)) + { + // Check clearance + if (clearance > 0) + { + // which planning group to collision check, "" is everything + if (planning_scene->distanceToCollision(*imarker_state_) < clearance) + { + continue; // clearance is not enough + } + } + + RCLCPP_INFO_STREAM(LOGGER, "Found valid random robot state after " << attempt << " attempts"); + + // Set updated pose from robot state + for (std::size_t i = 0; i < arm_datas_.size(); ++i) + end_effectors_[i]->setPoseFromRobotState(); + + // Send to imarker + for (std::size_t i = 0; i < arm_datas_.size(); ++i) + end_effectors_[i]->sendUpdatedIMarkerPose(); + + return true; + } + + if (attempt == 100) + RCLCPP_WARN_STREAM(LOGGER, "Taking long time to find valid random state"); + } + + RCLCPP_ERROR_STREAM(LOGGER, + "Unable to find valid random robot state for imarker after " << MAX_ATTEMPTS << " attempts"); + + return false; +} + +bool IMarkerRobotState::isStateValid(bool verbose) +{ + // Update transforms + imarker_state_->update(); + + planning_scene_monitor::LockedPlanningSceneRO planning_scene(psm_); // Read only lock + + // which planning group to collision check, "" is everything + return planning_scene->isStateValid(*imarker_state_, "", verbose); +} + +void IMarkerRobotState::publishRobotState() +{ + visual_tools_->publishRobotState(imarker_state_, color_); +} + +moveit_visual_tools::MoveItVisualToolsPtr IMarkerRobotState::getVisualTools() +{ + return visual_tools_; +} + +bool IMarkerRobotState::getFilePath(std::string& file_path, const std::string& file_name, + const std::string& subdirectory) const + +{ + namespace fs = boost::filesystem; + + // Check that the directory exists, if not, create it + fs::path rootPath = fs::path(package_path_); + rootPath = rootPath / fs::path(subdirectory); + + boost::system::error_code returnedError; + fs::create_directories(rootPath, returnedError); + + if (returnedError) + { + // did not successfully create directories + RCLCPP_ERROR(LOGGER, "Unable to create directory %s", subdirectory.c_str()); + return false; + } + + // directories successfully created, append the group name as the file name + rootPath = rootPath / fs::path(file_name); + file_path = rootPath.string(); + // ROS_DEBUG_STREAM_NAMED(name_, "Config file: " << file_path); + + return true; +} + +bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d& poses, + const moveit::core::JointModelGroup* group) +{ + std::vector tips; + for (std::size_t i = 0; i < arm_datas_.size(); ++i) + tips.push_back(arm_datas_[i].ee_link_->getName()); + + // ROS_DEBUG_STREAM_NAMED(name_, "First pose should be for joint model group: " << arm_datas_[0].ee_link_->getName()); + + const double timeout = 1.0 / 30.0; // 30 fps + + // Optionally collision check + moveit::core::GroupStateValidityCallbackFn constraint_fn; +#if 1 + bool collision_checking_verbose_ = false; + bool only_check_self_collision_ = false; + + // TODO(davetcoleman): this is currently not working, the locking seems to cause segfaults + // TODO(davetcoleman): change to std shared_ptr + boost::scoped_ptr ls; + ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(psm_)); + constraint_fn = boost::bind(&isIKStateValid, static_cast(*ls).get(), + collision_checking_verbose_, only_check_self_collision_, visual_tools_, + boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); +#endif + + // Solve + std::size_t outer_attempts = 20; + for (std::size_t i = 0; i < outer_attempts; ++i) + { + if (!imarker_state_->setFromIK(group, poses, tips, timeout, constraint_fn)) + { + RCLCPP_DEBUG_STREAM(LOGGER, "Failed to find dual arm pose, trying again"); + + // Re-seed + imarker_state_->setToRandomPositions(group); + } + else + { + RCLCPP_DEBUG_STREAM(LOGGER, "Found IK solution"); + + // Visualize robot + publishRobotState(); + + // Update the imarkers + for (const IMarkerEndEffectorPtr& ee : end_effectors_) + ee->setPoseFromRobotState(); + + return true; + } + } + + RCLCPP_ERROR_STREAM(LOGGER, "Failed to find dual arm pose"); + return false; +} + +} // namespace moveit_visual_tools diff --git a/aloha_ws/src/moveit_visual_tools/src/moveit_visual_tools.cpp b/aloha_ws/src/moveit_visual_tools/src/moveit_visual_tools.cpp new file mode 100644 index 00000000..4ae6d137 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/src/moveit_visual_tools.cpp @@ -0,0 +1,1702 @@ +// Copyright 2015 University of Colorado, Boulder +// +// 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 the copyright holder 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 HOLDER 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 +// Desc: Simple tools for showing parts of a robot in Rviz, such as the gripper or arm + +#include + +// MoveIt Messages +#include + +// MoveIt +#include +#include +#include + +// Conversions +#if __has_include() +#include +#else +#include +#endif + +// Transforms +#include + +// Shape tools +#include +#include + +// C++ +#include +#include +#include +#include +#include +#include +#include + +using namespace std::literals::chrono_literals; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_visual_tools"); +namespace moveit_visual_tools +{ +MoveItVisualTools::MoveItVisualTools(const rclcpp::Node::SharedPtr& node) + : RvizVisualTools("", rviz_visual_tools::RVIZ_MARKER_TOPIC, node) + , robot_state_topic_(DISPLAY_ROBOT_STATE_TOPIC) + , planning_scene_topic_(PLANNING_SCENE_TOPIC) + , node_(node) +{ + loadSharedRobotState(); + setBaseFrame(robot_model_->getModelFrame()); +} + +MoveItVisualTools::MoveItVisualTools(const rclcpp::Node::SharedPtr& node, const std::string& base_frame, + const std::string& marker_topic, + planning_scene_monitor::PlanningSceneMonitorPtr psm) + : RvizVisualTools::RvizVisualTools(base_frame, marker_topic, node) + , psm_(std::move(psm)) + , robot_state_topic_(DISPLAY_ROBOT_STATE_TOPIC) + , planning_scene_topic_(PLANNING_SCENE_TOPIC) + , node_(node) +{ +} + +MoveItVisualTools::MoveItVisualTools(const rclcpp::Node::SharedPtr& node, const std::string& base_frame, + const std::string& marker_topic, moveit::core::RobotModelConstPtr robot_model) + : RvizVisualTools::RvizVisualTools(base_frame, marker_topic, node) + , robot_model_(std::move(robot_model)) + , planning_scene_topic_(PLANNING_SCENE_TOPIC) + , node_(node) +{ +} + +bool MoveItVisualTools::loadPlanningSceneMonitor() +{ + // Check if we already have one + if (psm_) + { + RCLCPP_WARN_STREAM(LOGGER, "Will not load a new planning scene monitor when one has " + "already been set for Visual Tools"); + return false; + } + RCLCPP_INFO_STREAM(LOGGER, "Loading planning scene monitor"); + + // Regular version b/c the other one causes problems with recognizing end effectors + psm_.reset(new planning_scene_monitor::PlanningSceneMonitor(node_, ROBOT_DESCRIPTION, "visual_tools_scene")); + + if (psm_->getPlanningScene()) + { + // Optional monitors to start: + // psm_->startWorldGeometryMonitor(); + // psm_->startSceneMonitor("/move_group/monitored_planning_scene"); + // psm_->startStateMonitor("/joint_states", "/attached_collision_object"); + psm_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + planning_scene_topic_); + RCLCPP_INFO_STREAM(LOGGER, "Publishing planning scene on " << planning_scene_topic_); + + planning_scene_monitor::LockedPlanningSceneRW planning_scene(psm_); + planning_scene->setName("visual_tools_scene"); + } + else + { + RCLCPP_ERROR_STREAM(LOGGER, "Planning scene not configured"); + return false; + } + + return true; +} + +bool MoveItVisualTools::processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject& msg, + const rviz_visual_tools::Colors& color) +{ + // Apply command directly to planning scene to avoid a ROS msg call + { + planning_scene_monitor::LockedPlanningSceneRW scene(getPlanningSceneMonitor()); + scene->getCurrentStateNonConst().update(); // TODO: remove hack to prevent bad transforms + scene->processCollisionObjectMsg(msg); + scene->setObjectColor(msg.id, getColor(color)); + } + // Trigger an update + + if (!manual_trigger_update_) + { + triggerPlanningSceneUpdate(); + } + + return true; +} + +bool MoveItVisualTools::processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& msg) +{ + // Apply command directly to planning scene to avoid a ROS msg call + { + planning_scene_monitor::LockedPlanningSceneRW scene(getPlanningSceneMonitor()); + // scene->getCurrentStateNonConst().update(); // hack to prevent bad transforms + scene->processAttachedCollisionObjectMsg(msg); + } + + // Trigger an update + if (!manual_trigger_update_) + { + triggerPlanningSceneUpdate(); + } + + return true; +} + +bool MoveItVisualTools::moveCollisionObject(const Eigen::Isometry3d& pose, const std::string& name, + const rviz_visual_tools::Colors& color) +{ + return moveCollisionObject(convertPose(pose), name, color); +} + +bool MoveItVisualTools::moveCollisionObject(const geometry_msgs::msg::Pose& pose, const std::string& name, + const rviz_visual_tools::Colors& color) +{ + moveit_msgs::msg::CollisionObject collision_obj; + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.id = name; + collision_obj.operation = moveit_msgs::msg::CollisionObject::MOVE; + + collision_obj.primitive_poses.resize(1); + collision_obj.primitive_poses[0] = pose; + + return processCollisionObjectMsg(collision_obj, color); +} + +bool MoveItVisualTools::triggerPlanningSceneUpdate() +{ + // TODO(davetcoleman): perhaps switch to using the service call? + getPlanningSceneMonitor()->triggerSceneUpdateEvent(planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY); + + return true; +} + +bool MoveItVisualTools::loadSharedRobotState() +{ + // Get robot state + if (!shared_robot_state_) + { + // Check if a robot model was passed in + if (!robot_model_) + { + // Fall back on using planning scene monitor. + robot_model_ = getPlanningSceneMonitor()->getRobotModel(); + } + shared_robot_state_.reset(new moveit::core::RobotState(robot_model_)); + + // TODO(davetcoleman): this seems to be a work around for a weird NaN bug + shared_robot_state_->setToDefaultValues(); + shared_robot_state_->update(true); + + // hidden_robot_state_.reset(new moveit::core::RobotState(robot_model_)); + // hidden_robot_state_->setToDefaultValues(); + // hidden_robot_state_->update(true); + + hidden_robot_state_.reset(new moveit::core::RobotState(*shared_robot_state_)); + root_robot_state_.reset(new moveit::core::RobotState(*shared_robot_state_)); + } + + return !(shared_robot_state_ == nullptr); +} + +moveit::core::RobotStatePtr& MoveItVisualTools::getSharedRobotState() +{ + // Always load the robot state before using + loadSharedRobotState(); + return shared_robot_state_; +} + +moveit::core::RobotModelConstPtr MoveItVisualTools::getRobotModel() +{ + // Always load the robot state before using + loadSharedRobotState(); + return shared_robot_state_->getRobotModel(); +} + +bool MoveItVisualTools::loadEEMarker(const moveit::core::JointModelGroup* ee_jmg, + const std::vector& ee_joint_pos) +{ + // Get joint state group + if (ee_jmg == nullptr) // make sure EE_GROUP exists + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to find joint model group with address" << ee_jmg); + return false; + } + + // Always load the robot state before using + loadSharedRobotState(); + shared_robot_state_->setToDefaultValues(); + shared_robot_state_->update(); + + if (!ee_joint_pos.empty()) + { + if (ee_joint_pos.size() != ee_jmg->getActiveJointModels().size()) + { + RCLCPP_ERROR_STREAM(LOGGER, "The number of joint positions given (" + << ee_joint_pos.size() << ") does not match the number of active joints in " + << ee_jmg->getName() << "(" << ee_jmg->getActiveJointModels().size() << ")"); + return false; + } + shared_robot_state_->setJointGroupPositions(ee_jmg, ee_joint_pos); + shared_robot_state_->update(true); + } + + // Clear old EE markers and EE poses + ee_markers_map_[ee_jmg].markers.clear(); + ee_poses_map_[ee_jmg].clear(); + + // Remember joint state + ee_joint_pos_map_[ee_jmg] = ee_joint_pos; + + // Keep track of how many unique markers we have between different EEs + static std::size_t marker_id_offset = 0; + + // Get end effector group + + // Create color to use for EE markers + std_msgs::msg::ColorRGBA marker_color = getColor(rviz_visual_tools::GREY); + + // Get link names that are in end effector + const std::vector& ee_link_names = ee_jmg->getLinkModelNames(); + + // Get EE link markers for Rviz + shared_robot_state_->getRobotMarkers(ee_markers_map_[ee_jmg], ee_link_names, marker_color, ee_jmg->getName(), + rclcpp::Duration::from_seconds(0)); + RCLCPP_DEBUG_STREAM(LOGGER, "Number of rviz markers in end effector: " << ee_markers_map_[ee_jmg].markers.size()); + + // Error check + if (ee_markers_map_[ee_jmg].markers.empty()) + { + RCLCPP_ERROR_STREAM(LOGGER, + "No links found to visualize in end effector for joint model group: " << ee_jmg->getName()); + return false; + } + + const std::string& ee_parent_link_name = ee_jmg->getEndEffectorParentGroup().second; + const moveit::core::LinkModel* ee_parent_link = robot_model_->getLinkModel(ee_parent_link_name); + + Eigen::Isometry3d ee_marker_global_transform = shared_robot_state_->getGlobalLinkTransform(ee_parent_link); + Eigen::Isometry3d ee_marker_pose; + + // Process each link of the end effector + for (std::size_t i = 0; i < ee_markers_map_[ee_jmg].markers.size(); ++i) + { + // Header + ee_markers_map_[ee_jmg].markers[i].header.frame_id = base_frame_; + + // Options for meshes + if (ee_markers_map_[ee_jmg].markers[i].type == visualization_msgs::msg::Marker::MESH_RESOURCE) + { + ee_markers_map_[ee_jmg].markers[i].mesh_use_embedded_materials = true; + } + + // Unique id + ee_markers_map_[ee_jmg].markers[i].id += marker_id_offset; + + // Copy original marker poses to a vector + ee_marker_pose = ee_marker_global_transform.inverse() * convertPose(ee_markers_map_[ee_jmg].markers[i].pose); + ee_poses_map_[ee_jmg].push_back(ee_marker_pose); + } + + marker_id_offset += ee_markers_map_[ee_jmg].markers.size(); + + return true; +} + +void MoveItVisualTools::loadTrajectoryPub(const std::string& display_planned_path_topic, bool blocking) +{ + if (pub_display_path_) + return; + + // Trajectory paths + pub_display_path_ = node_->create_publisher(display_planned_path_topic, 10); + RCLCPP_DEBUG_STREAM(LOGGER, "Publishing MoveIt trajectory on topic " << pub_display_path_->get_topic_name()); + + // Wait for topic to be ready + if (blocking) + { + const bool subscribed = waitForSubscriber(pub_display_path_, 5.0 /* seconds */); + if (subscribed) + { + RCLCPP_DEBUG_STREAM(LOGGER, "Subscribed to display trajectory topic: " << display_planned_path_topic); + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Cannot subscribe to display trajectory topic: " << display_planned_path_topic); + } + } +} + +void MoveItVisualTools::loadRobotStatePub(const std::string& robot_state_topic, bool blocking) +{ + if (pub_robot_state_) + return; + + // Update global var if new topic was passed in + if (!robot_state_topic.empty()) + robot_state_topic_ = robot_state_topic; + + // RobotState Message + pub_robot_state_ = node_->create_publisher(robot_state_topic, 1); + RCLCPP_DEBUG_STREAM(LOGGER, "Publishing MoveIt robot state on topic " << pub_robot_state_->get_topic_name()); + + // Wait for topic to be ready + if (blocking) + { + const bool subscribed = waitForSubscriber(pub_robot_state_, 5.0 /* seconds */); + if (subscribed) + { + RCLCPP_DEBUG_STREAM(LOGGER, "Subscribed to robot state topic: " << robot_state_topic); + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Cannot subscribe to robot state topic: " << robot_state_topic); + } + } +} + +bool MoveItVisualTools::publishEEMarkers(const geometry_msgs::msg::Pose& pose, + const moveit::core::JointModelGroup* ee_jmg, + const std::vector& ee_joint_pos, + const rviz_visual_tools::Colors& color, const std::string& ns) +{ + // Check if we have not loaded the EE markers + if (ee_markers_map_[ee_jmg].markers.empty() || ee_poses_map_[ee_jmg].empty() || + ee_joint_pos_map_[ee_jmg] != ee_joint_pos) + { + if (!loadEEMarker(ee_jmg, ee_joint_pos)) + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to publish EE marker, unable to load EE markers"); + return false; + } + } + + Eigen::Isometry3d eigen_goal_ee_pose = convertPose(pose); + Eigen::Isometry3d eigen_this_marker; + + // Process each link of the end effector + for (std::size_t i = 0; i < ee_markers_map_[ee_jmg].markers.size(); ++i) + { + // Make sure ROS is still spinning + if (!rclcpp::ok()) + break; + + // Header + ee_markers_map_[ee_jmg].markers[i].header.stamp = node_->now(); + + // Namespace + ee_markers_map_[ee_jmg].markers[i].ns = ns; + + // Lifetime + ee_markers_map_[ee_jmg].markers[i].lifetime = marker_lifetime_; + + // Color + if (color != rviz_visual_tools::DEFAULT) + ee_markers_map_[ee_jmg].markers[i].color = getColor(color); + + // Convert pose + eigen_this_marker = eigen_goal_ee_pose * ee_poses_map_[ee_jmg][i]; + ee_markers_map_[ee_jmg].markers[i].pose = convertPose(eigen_this_marker); + } + + // Helper for publishing rviz markers + // Does not require trigger() because publishing array auto-triggers + if (!publishMarkers(ee_markers_map_[ee_jmg])) + { + RCLCPP_WARN_STREAM(LOGGER, "Unable to publish EE markers"); + return false; + } + + return true; +} + +bool MoveItVisualTools::publishGrasps(const std::vector& possible_grasps, + const moveit::core::JointModelGroup* ee_jmg, double animate_speed) +{ + RCLCPP_DEBUG_STREAM(LOGGER, "Visualizing " << possible_grasps.size() << " grasps with EE joint model group " + << ee_jmg->getName()); + + // Loop through all grasps + for (std::size_t i = 0; i < possible_grasps.size(); ++i) + { + if (!rclcpp::ok()) // Check that ROS is still ok and that user isn't trying to quit + break; + + publishEEMarkers(possible_grasps[i].grasp_pose.pose, ee_jmg); + + rclcpp::sleep_for(std::chrono::milliseconds(int(animate_speed * 1000))); + } + + return true; +} + +bool MoveItVisualTools::publishAnimatedGrasps(const std::vector& possible_grasps, + const moveit::core::JointModelGroup* ee_jmg, double animate_speed) +{ + RCLCPP_DEBUG_STREAM(LOGGER, "Visualizing " << possible_grasps.size() << " grasps with joint model group " + << ee_jmg->getName() << " at speed " << animate_speed); + + // Loop through all grasps + for (std::size_t i = 0; i < possible_grasps.size(); ++i) + { + if (!rclcpp::ok()) // Check that ROS is still ok and that user isn't trying to quit + break; + + publishAnimatedGrasp(possible_grasps[i], ee_jmg, animate_speed); + rclcpp::sleep_for(std::chrono::milliseconds(int(animate_speed * 1000))); + } + + return true; +} + +bool MoveItVisualTools::publishAnimatedGrasp(const moveit_msgs::msg::Grasp& grasp, + const moveit::core::JointModelGroup* ee_jmg, double animate_speed) +{ + // Grasp Pose Variables + geometry_msgs::msg::Pose grasp_pose = grasp.grasp_pose.pose; + +#if 0 // Debug + publishArrow(grasp_pose, rviz_visual_tools::GREEN); + publishEEMarkers(grasp_pose, ee_jmg); + ros::Duration(0.5).sleep(); +#endif + + Eigen::Isometry3d grasp_pose_eigen; + tf2::fromMsg(grasp_pose, grasp_pose_eigen); + + // Pre-grasp pose variables + geometry_msgs::msg::Pose pre_grasp_pose; + Eigen::Isometry3d pre_grasp_pose_eigen; + + // Approach direction variables + Eigen::Vector3d pre_grasp_approach_direction_local; + + // Display Grasp Score + // std::string text = "Grasp Quality: " + + // boost::lexical_cast(int(grasp.grasp_quality*100)) + "%"; + // publishText(grasp_pose, text); + + // Convert the grasp pose into the frame of reference of the approach/retreat frame_id + + // Animate the movement - for ee approach direction + double animation_resulution = 0.1; // the lower the better the resolution + for (double percent = 0; percent < 1; percent += animation_resulution) + { + if (!rclcpp::ok()) // Check that ROS is still ok and that user isn't trying to quit + break; + + // Copy original grasp pose to pre-grasp pose + pre_grasp_pose_eigen = grasp_pose_eigen; + + // The direction of the pre-grasp + // Calculate the current animation position based on the percent + Eigen::Vector3d pre_grasp_approach_direction = Eigen::Vector3d( + -1 * grasp.pre_grasp_approach.direction.vector.x * grasp.pre_grasp_approach.min_distance * (1 - percent), + -1 * grasp.pre_grasp_approach.direction.vector.y * grasp.pre_grasp_approach.min_distance * (1 - percent), + -1 * grasp.pre_grasp_approach.direction.vector.z * grasp.pre_grasp_approach.min_distance * (1 - percent)); + + // Decide if we need to change the approach_direction to the local frame of the end effector + // orientation + const std::string& ee_parent_link_name = ee_jmg->getEndEffectorParentGroup().second; + + if (grasp.pre_grasp_approach.direction.header.frame_id == ee_parent_link_name) + { + // Apply/compute the approach_direction vector in the local frame of the grasp_pose + // orientation + pre_grasp_approach_direction_local = grasp_pose_eigen.rotation() * pre_grasp_approach_direction; + } + else + { + pre_grasp_approach_direction_local = pre_grasp_approach_direction; // grasp_pose_eigen.rotation() * + // pre_grasp_approach_direction; + } + + // Update the grasp pose usign the new locally-framed approach_direction + pre_grasp_pose_eigen.translation() += pre_grasp_approach_direction_local; + + // Convert eigen pre-grasp position back to regular message + pre_grasp_pose = tf2::toMsg(pre_grasp_pose_eigen); + + publishEEMarkers(pre_grasp_pose, ee_jmg); + + rclcpp::sleep_for(std::chrono::milliseconds(int(animate_speed * 1000))); + + // Pause more at initial pose for debugging purposes + if (percent == 0) + rclcpp::sleep_for(std::chrono::milliseconds(int(animate_speed * 2000))); + } + return true; +} + +bool MoveItVisualTools::publishIKSolutions(const std::vector& ik_solutions, + const moveit::core::JointModelGroup* arm_jmg, double display_time) +{ + if (ik_solutions.empty()) + { + RCLCPP_WARN_STREAM(LOGGER, "Empty ik_solutions vector passed into publishIKSolutions()"); + return false; + } + + loadSharedRobotState(); + + RCLCPP_DEBUG_STREAM(LOGGER, "Visualizing " << ik_solutions.size() << " inverse kinematic solutions"); + + // Apply the time to the trajectory + trajectory_msgs::msg::JointTrajectoryPoint trajectory_pt_timed; + + // Create a trajectory with one point + moveit_msgs::msg::RobotTrajectory trajectory_msg; + trajectory_msg.joint_trajectory.header.frame_id = base_frame_; + trajectory_msg.joint_trajectory.joint_names = arm_jmg->getActiveJointModelNames(); + + // Overall length of trajectory + double running_time = 0; + + // Loop through all inverse kinematic solutions + for (std::size_t i = 0; i < ik_solutions.size(); ++i) + { + trajectory_pt_timed = ik_solutions[i]; + trajectory_pt_timed.time_from_start = rclcpp::Duration::from_seconds(running_time); + trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed); + + running_time += display_time; + } + + // Re-add final position so the last point is displayed fully + trajectory_pt_timed = trajectory_msg.joint_trajectory.points.back(); + trajectory_pt_timed.time_from_start = rclcpp::Duration::from_seconds(running_time); + trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed); + + return publishTrajectoryPath(trajectory_msg, shared_robot_state_, true); +} + +bool MoveItVisualTools::removeAllCollisionObjects() +{ + // Apply command directly to planning scene to avoid a ROS msg call + { + planning_scene_monitor::LockedPlanningSceneRW scene(getPlanningSceneMonitor()); + scene->removeAllCollisionObjects(); + } + + return true; +} + +bool MoveItVisualTools::cleanupCO(const std::string& name) +{ + // Clean up old collision objects + moveit_msgs::msg::CollisionObject co; + co.header.stamp = node_->now(); + co.header.frame_id = base_frame_; + co.id = name; + co.operation = moveit_msgs::msg::CollisionObject::REMOVE; + + return processCollisionObjectMsg(co); +} + +bool MoveItVisualTools::cleanupACO(const std::string& /*name*/) +{ + // Clean up old attached collision object + moveit_msgs::msg::AttachedCollisionObject aco; + aco.object.header.stamp = node_->now(); + aco.object.header.frame_id = base_frame_; + + // aco.object.id = name; + aco.object.operation = moveit_msgs::msg::CollisionObject::REMOVE; + + return processAttachedCollisionObjectMsg(aco); +} + +bool MoveItVisualTools::attachCO(const std::string& name, const std::string& ee_parent_link) +{ + // Attach a collision object + moveit_msgs::msg::AttachedCollisionObject aco; + aco.object.header.stamp = node_->now(); + aco.object.header.frame_id = base_frame_; + + aco.object.id = name; + aco.object.operation = moveit_msgs::msg::CollisionObject::ADD; + + // Link to attach the object to + aco.link_name = ee_parent_link; + + return processAttachedCollisionObjectMsg(aco); +} + +bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::msg::Pose& block_pose, const std::string& name, + double block_size, const rviz_visual_tools::Colors& color) +{ + moveit_msgs::msg::CollisionObject collision_obj; + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.id = name; + collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD; + + collision_obj.primitives.resize(1); + collision_obj.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + collision_obj.primitives[0].dimensions.resize( + geometric_shapes::solidPrimitiveDimCount()); + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = block_size; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = block_size; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = block_size; + collision_obj.primitive_poses.resize(1); + collision_obj.primitive_poses[0] = block_pose; + + // RCLCPP_INFO_STREAM(LOGGER,"CollisionObject: \n " << collision_obj); + // RCLCPP_DEBUG_STREAM(LOGGER,"Published collision object " << name); + return processCollisionObjectMsg(collision_obj, color); +} + +bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, + const std::string& name, const rviz_visual_tools::Colors& color) +{ + return publishCollisionCuboid(convertPoint(point1), convertPoint(point2), name, color); +} + +bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::msg::Point& point1, + const geometry_msgs::msg::Point& point2, const std::string& name, + const rviz_visual_tools::Colors& color) +{ + moveit_msgs::msg::CollisionObject collision_obj; + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.id = name; + collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD; + + // Calculate center pose + collision_obj.primitive_poses.resize(1); + collision_obj.primitive_poses[0].position.x = (point1.x - point2.x) / 2.0 + point2.x; + collision_obj.primitive_poses[0].position.y = (point1.y - point2.y) / 2.0 + point2.y; + collision_obj.primitive_poses[0].position.z = (point1.z - point2.z) / 2.0 + point2.z; + + // Calculate scale + collision_obj.primitives.resize(1); + collision_obj.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + collision_obj.primitives[0].dimensions.resize( + geometric_shapes::solidPrimitiveDimCount()); + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = fabs(point1.x - point2.x); + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = fabs(point1.y - point2.y); + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = fabs(point1.z - point2.z); + + // Prevent scale from being zero + if (!collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X]) + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = rviz_visual_tools::SMALL_SCALE; + if (!collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y]) + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = rviz_visual_tools::SMALL_SCALE; + if (!collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z]) + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE; + + // RCLCPP_INFO_STREAM(LOGGER,"CollisionObject: \n " << collision_obj); + return processCollisionObjectMsg(collision_obj, color); +} + +bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height, + const std::string& name, const rviz_visual_tools::Colors& color) +{ + geometry_msgs::msg::Pose pose_msg = tf2::toMsg(pose); + return publishCollisionCuboid(pose_msg, width, depth, height, name, color); +} + +bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::msg::Pose& pose, double width, double depth, + double height, const std::string& name, + const rviz_visual_tools::Colors& color) +{ + moveit_msgs::msg::CollisionObject collision_obj; + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.id = name; + collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD; + + // Calculate center pose + collision_obj.primitive_poses.resize(1); + collision_obj.primitive_poses[0] = pose; + + // Calculate scale + collision_obj.primitives.resize(1); + collision_obj.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + collision_obj.primitives[0].dimensions.resize( + geometric_shapes::solidPrimitiveDimCount()); + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = width; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = depth; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = height; + + // Prevent scale from being zero + if (!collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X]) + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = rviz_visual_tools::SMALL_SCALE; + if (!collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y]) + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = rviz_visual_tools::SMALL_SCALE; + if (!collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z]) + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE; + + return processCollisionObjectMsg(collision_obj, color); +} + +bool MoveItVisualTools::publishCollisionFloor(double z, const std::string& plane_name, + const rviz_visual_tools::Colors& color) +{ + // Instead just generate a rectangle + geometry_msgs::msg::Point point1; + geometry_msgs::msg::Point point2; + + point1.x = rviz_visual_tools::LARGE_SCALE; + point1.y = rviz_visual_tools::LARGE_SCALE; + point1.z = z; + + point2.x = -rviz_visual_tools::LARGE_SCALE; + point2.y = -rviz_visual_tools::LARGE_SCALE; + point2.z = z - rviz_visual_tools::SMALL_SCALE; + + return publishCollisionCuboid(point1, point2, plane_name, color); +} + +bool MoveItVisualTools::publishCollisionCylinder(const geometry_msgs::msg::Point& a, const geometry_msgs::msg::Point& b, + const std::string& object_name, double radius, + const rviz_visual_tools::Colors& color) +{ + return publishCollisionCylinder(convertPoint(a), convertPoint(b), object_name, radius, color); +} + +bool MoveItVisualTools::publishCollisionCylinder(const Eigen::Vector3d& a, const Eigen::Vector3d& b, + const std::string& object_name, double radius, + const rviz_visual_tools::Colors& color) +{ + // Distance between two points + double height = (a - b).lpNorm<2>(); + + // Find center point + Eigen::Vector3d pt_center = getCenterPoint(a, b); + + // Create vector + Eigen::Isometry3d pose; + pose = getVectorBetweenPoints(pt_center, b); + + // Convert pose to be normal to cylindar axis + Eigen::Isometry3d rotation; + rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()); + pose = pose * rotation; + + return publishCollisionCylinder(pose, object_name, radius, height, color); +} + +bool MoveItVisualTools::publishCollisionCylinder(const Eigen::Isometry3d& object_pose, const std::string& object_name, + double radius, double height, const rviz_visual_tools::Colors& color) +{ + return publishCollisionCylinder(convertPose(object_pose), object_name, radius, height, color); +} + +bool MoveItVisualTools::publishCollisionCylinder(const geometry_msgs::msg::Pose& object_pose, + const std::string& object_name, double radius, double height, + const rviz_visual_tools::Colors& color) +{ + moveit_msgs::msg::CollisionObject collision_obj; + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.id = object_name; + collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD; + collision_obj.primitives.resize(1); + collision_obj.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; + collision_obj.primitives[0].dimensions.resize( + geometric_shapes::solidPrimitiveDimCount()); + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::CYLINDER_HEIGHT] = height; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::CYLINDER_RADIUS] = radius; + collision_obj.primitive_poses.resize(1); + collision_obj.primitive_poses[0] = object_pose; + + // RCLCPP_INFO_STREAM(LOGGER,"CollisionObject: \n " << collision_obj); + return processCollisionObjectMsg(collision_obj, color); +} + +bool MoveItVisualTools::publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name, + const std::string& mesh_path, const rviz_visual_tools::Colors& color) +{ + return publishCollisionMesh(convertPose(object_pose), object_name, mesh_path, color); +} + +bool MoveItVisualTools::publishCollisionMesh(const geometry_msgs::msg::Pose& object_pose, + const std::string& object_name, const std::string& mesh_path, + const rviz_visual_tools::Colors& color) +{ + shapes::Shape* mesh = shapes::createMeshFromResource(mesh_path); // make sure its prepended by file:// + shapes::ShapeMsg shape_msg; // this is a boost::variant type from shape_messages.h + if (!mesh || !shapes::constructMsgFromShape(mesh, shape_msg)) + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to create mesh shape message from resource " << mesh_path); + return false; + } + + if (!publishCollisionMesh(object_pose, object_name, boost::get(shape_msg), color)) + return false; + + RCLCPP_DEBUG(LOGGER, "Loaded mesh from '%s'", mesh_path.c_str()); + return true; +} + +bool MoveItVisualTools::publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name, + const shape_msgs::msg::Mesh& mesh_msg, + const rviz_visual_tools::Colors& color) +{ + return publishCollisionMesh(convertPose(object_pose), object_name, mesh_msg, color); +} + +bool MoveItVisualTools::publishCollisionMesh(const geometry_msgs::msg::Pose& object_pose, + const std::string& object_name, const shape_msgs::msg::Mesh& mesh_msg, + const rviz_visual_tools::Colors& color) +{ + // Create collision message + moveit_msgs::msg::CollisionObject collision_obj; + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.id = object_name; + collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD; + collision_obj.mesh_poses.resize(1); + collision_obj.mesh_poses[0] = object_pose; + collision_obj.meshes.resize(1); + collision_obj.meshes[0] = mesh_msg; + + return processCollisionObjectMsg(collision_obj, color); +} + +bool MoveItVisualTools::publishCollisionGraph(const graph_msgs::msg::GeometryGraph& graph, + const std::string& object_name, double radius, + const rviz_visual_tools::Colors& color) +{ + RCLCPP_INFO_STREAM(LOGGER, "Preparing to create collision graph"); + + // The graph is one collision object with many primitives + moveit_msgs::msg::CollisionObject collision_obj; + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.id = object_name; + collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD; + + // Track which pairs of nodes we've already connected since graph is bi-directional + typedef std::pair node_ids; + std::set added_edges; + std::pair::iterator, bool> return_value; + + Eigen::Vector3d a, b; + for (std::size_t i = 0; i < graph.nodes.size(); ++i) + { + for (std::size_t j = 0; j < graph.edges[i].node_ids.size(); ++j) + { + // Check if we've already added this pair of nodes (edge) + return_value = added_edges.insert(node_ids(i, j)); + if (!return_value.second) + { + // Element already existed in set, so don't add a new collision object + } + else + { + // Create a cylinder from two points + a = convertPoint(graph.nodes[i]); + b = convertPoint(graph.nodes[graph.edges[i].node_ids[j]]); + + // add other direction of edge + added_edges.insert(node_ids(j, i)); + + // Distance between two points + double height = (a - b).lpNorm<2>(); + + // Find center point + Eigen::Vector3d pt_center = getCenterPoint(a, b); + + // Create vector + Eigen::Isometry3d pose; + pose = getVectorBetweenPoints(pt_center, b); + + // Convert pose to be normal to cylindar axis + Eigen::Isometry3d rotation; + rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()); + pose = pose * rotation; + + // Create the solid primitive + shape_msgs::msg::SolidPrimitive cylinder; + cylinder.type = shape_msgs::msg::SolidPrimitive::CYLINDER; + cylinder.dimensions.resize( + geometric_shapes::solidPrimitiveDimCount()); + cylinder.dimensions[shape_msgs::msg::SolidPrimitive::CYLINDER_HEIGHT] = height; + cylinder.dimensions[shape_msgs::msg::SolidPrimitive::CYLINDER_RADIUS] = radius; + + // Add to the collision object + collision_obj.primitives.push_back(cylinder); + + // Add the pose + collision_obj.primitive_poses.push_back(convertPose(pose)); + } + } + } + + return processCollisionObjectMsg(collision_obj, color); +} + +void MoveItVisualTools::getCollisionWallMsg(double x, double y, double z, double angle, double width, double height, + const std::string& name, moveit_msgs::msg::CollisionObject& collision_obj) +{ + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD; + collision_obj.primitives.resize(1); + collision_obj.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + collision_obj.primitives[0].dimensions.resize( + geometric_shapes::solidPrimitiveDimCount()); + + geometry_msgs::msg::Pose rec_pose; + + // ---------------------------------------------------------------------------------- + // Name + collision_obj.id = name; + + double depth = 0.1; + + // Position + rec_pose.position.x = x; + rec_pose.position.y = y; + rec_pose.position.z = height / 2 + z; + + // Size + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = depth; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = width; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = height; + // ---------------------------------------------------------------------------------- + + Eigen::Quaterniond quat(Eigen::AngleAxis(static_cast(angle), Eigen::Vector3d::UnitZ())); + rec_pose.orientation.x = quat.x(); + rec_pose.orientation.y = quat.y(); + rec_pose.orientation.z = quat.z(); + rec_pose.orientation.w = quat.w(); + + collision_obj.primitive_poses.resize(1); + collision_obj.primitive_poses[0] = rec_pose; +} + +bool MoveItVisualTools::publishCollisionWall(double x, double y, double angle, double width, double height, + const std::string& name, const rviz_visual_tools::Colors& color) +{ + return publishCollisionWall(x, y, 0.0, angle, width, height, name, color); +} + +bool MoveItVisualTools::publishCollisionWall(double x, double y, double z, double angle, double width, double height, + const std::string& name, const rviz_visual_tools::Colors& color) +{ + moveit_msgs::msg::CollisionObject collision_obj; + getCollisionWallMsg(x, y, z, angle, width, height, name, collision_obj); + + return processCollisionObjectMsg(collision_obj, color); +} + +bool MoveItVisualTools::publishCollisionTable(double x, double y, double z, double angle, double width, double height, + double depth, const std::string& name, + const rviz_visual_tools::Colors& color) +{ + geometry_msgs::msg::Pose table_pose; + + // Center of table + table_pose.position.x = x; + table_pose.position.y = y; + table_pose.position.z = z + height / 2.0; + + // Orientation + Eigen::Quaterniond quat(Eigen::AngleAxis(static_cast(angle), Eigen::Vector3d::UnitZ())); + table_pose.orientation.x = quat.x(); + table_pose.orientation.y = quat.y(); + table_pose.orientation.z = quat.z(); + table_pose.orientation.w = quat.w(); + + moveit_msgs::msg::CollisionObject collision_obj; + collision_obj.header.stamp = node_->now(); + collision_obj.header.frame_id = base_frame_; + collision_obj.id = name; + collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD; + collision_obj.primitives.resize(1); + collision_obj.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + collision_obj.primitives[0].dimensions.resize( + geometric_shapes::solidPrimitiveDimCount()); + + // Size + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = depth; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = width; + collision_obj.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = height; + + collision_obj.primitive_poses.resize(1); + collision_obj.primitive_poses[0] = table_pose; + return processCollisionObjectMsg(collision_obj, color); +} + +bool MoveItVisualTools::loadCollisionSceneFromFile(const std::string& path) +{ + return loadCollisionSceneFromFile(path, Eigen::Isometry3d::Identity()); +} + +bool MoveItVisualTools::loadCollisionSceneFromFile(const std::string& path, const Eigen::Isometry3d& offset) +{ + // Open file + std::ifstream fin(path.c_str()); + if (fin.good()) + { + // Load directly to the planning scene + planning_scene_monitor::LockedPlanningSceneRW scene(getPlanningSceneMonitor()); + { + if (scene) + { + scene->loadGeometryFromStream(fin, offset); + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Unable to get locked planning scene RW"); + return false; + } + } + RCLCPP_INFO(LOGGER, "Loaded scene geometry from '%s'", path.c_str()); + } + else + RCLCPP_WARN(LOGGER, "Unable to load scene geometry from '%s'", path.c_str()); + + fin.close(); + + return triggerPlanningSceneUpdate(); +} + +bool MoveItVisualTools::publishWorkspaceParameters(const moveit_msgs::msg::WorkspaceParameters& params) +{ + return publishCuboid(convertPoint(params.min_corner), convertPoint(params.max_corner), rviz_visual_tools::TRANSLUCENT, + "Planning_Workspace", 1); +} + +bool MoveItVisualTools::checkAndPublishCollision(const moveit::core::RobotState& robot_state, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::Colors& highlight_link_color, + const rviz_visual_tools::Colors& contact_point_color) +{ + // Compute the contacts if any + collision_detection::CollisionRequest c_req; + collision_detection::CollisionResult c_res; + c_req.contacts = true; + c_req.max_contacts = 10; + c_req.max_contacts_per_pair = 3; + c_req.verbose = true; + + // Check for collisions + planning_scene->checkCollision(c_req, c_res, robot_state); + std::vector highlight_links; + for (const auto& contact : c_res.contacts) + { + highlight_links.push_back(contact.first.first); + highlight_links.push_back(contact.first.second); + } + + publishRobotState(robot_state, highlight_link_color, highlight_links); + publishContactPoints(c_res.contacts, planning_scene, contact_point_color); + return c_res.collision; +} + +bool MoveItVisualTools::publishContactPoints(const moveit::core::RobotState& robot_state, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::Colors& color) +{ + // Compute the contacts if any + collision_detection::CollisionRequest c_req; + collision_detection::CollisionResult c_res; + c_req.contacts = true; + c_req.max_contacts = 10; + c_req.max_contacts_per_pair = 3; + c_req.verbose = true; + + // Check for collisions + planning_scene->checkCollision(c_req, c_res, robot_state); + return publishContactPoints(c_res.contacts, planning_scene, color); +} + +bool MoveItVisualTools::publishContactPoints(const collision_detection::CollisionResult::ContactMap& contacts, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::Colors& color) +{ + // Display + if (!contacts.empty()) + { + visualization_msgs::msg::MarkerArray arr; + collision_detection::getCollisionMarkersFromContacts(arr, planning_scene->getPlanningFrame(), contacts); + RCLCPP_INFO_STREAM(LOGGER, "Completed listing of explanations for invalid states."); + + // Check for markers + if (arr.markers.empty()) + return true; + + // Convert markers to same namespace and other custom stuff + for (std::size_t i = 0; i < arr.markers.size(); ++i) + { + arr.markers[i].ns = "Collision"; + arr.markers[i].id = i; + arr.markers[i].scale.x = 0.04; + arr.markers[i].scale.y = 0.04; + arr.markers[i].scale.z = 0.04; + arr.markers[i].color = getColor(color); + } + + return publishMarkers(arr); + } + return true; +} + +bool MoveItVisualTools::publishTrajectoryPoint(const trajectory_msgs::msg::JointTrajectoryPoint& trajectory_pt, + const std::string& planning_group, double display_time) +{ + // Get joint state group + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group); + + if (jmg == nullptr) // not found + { + RCLCPP_ERROR_STREAM(LOGGER, "Could not find joint model group " << planning_group); + return false; + } + + // Apply the time to the trajectory + trajectory_msgs::msg::JointTrajectoryPoint trajectory_pt_timed = trajectory_pt; + trajectory_pt_timed.time_from_start = rclcpp::Duration::from_seconds(display_time); + + // Create a trajectory with one point + moveit_msgs::msg::RobotTrajectory trajectory_msg; + trajectory_msg.joint_trajectory.header.frame_id = base_frame_; + trajectory_msg.joint_trajectory.joint_names = jmg->getJointModelNames(); + trajectory_msg.joint_trajectory.points.push_back(trajectory_pt); + trajectory_msg.joint_trajectory.points.push_back(trajectory_pt_timed); + + return publishTrajectoryPath(trajectory_msg, shared_robot_state_, true); +} + +bool MoveItVisualTools::publishTrajectoryPath(const std::vector& trajectory, + const moveit::core::JointModelGroup* jmg, double speed, bool blocking) +{ + // Copy the vector of RobotStates to a RobotTrajectory + robot_trajectory::RobotTrajectoryPtr robot_trajectory( + new robot_trajectory::RobotTrajectory(robot_model_, jmg->getName())); + + double duration_from_previous = 0; + for (std::size_t k = 0; k < trajectory.size(); ++k) + { + duration_from_previous += speed; + robot_trajectory->addSuffixWayPoint(trajectory[k], duration_from_previous); + } + + // Convert trajectory to a message + moveit_msgs::msg::RobotTrajectory trajectory_msg; + robot_trajectory->getRobotTrajectoryMsg(trajectory_msg); + + // Use first trajectory point as reference state + moveit_msgs::msg::RobotState robot_state_msg; + if (!trajectory.empty()) + moveit::core::robotStateToRobotStateMsg(*trajectory[0], robot_state_msg); + + return publishTrajectoryPath(trajectory_msg, robot_state_msg, blocking); +} + +bool MoveItVisualTools::publishTrajectoryPath(const robot_trajectory::RobotTrajectoryPtr& trajectory, bool blocking) +{ + return publishTrajectoryPath(*trajectory, blocking); +} + +bool MoveItVisualTools::publishTrajectoryPath(const robot_trajectory::RobotTrajectory& trajectory, bool blocking) +{ + moveit_msgs::msg::RobotTrajectory trajectory_msg; + trajectory.getRobotTrajectoryMsg(trajectory_msg); + + // Add time from start if none specified + if (trajectory_msg.joint_trajectory.points.size() > 1) + { + if (trajectory_msg.joint_trajectory.points[1].time_from_start == + rclcpp::Duration::from_seconds(0)) // assume no timestamps exist + { + for (std::size_t i = 0; i < trajectory_msg.joint_trajectory.points.size(); ++i) + { + trajectory_msg.joint_trajectory.points[i].time_from_start = rclcpp::Duration::from_seconds(i * 2); // 1 hz + } + } + } + + // Use first trajectory point as reference state + moveit_msgs::msg::RobotState robot_state_msg; + if (!trajectory.empty()) + moveit::core::robotStateToRobotStateMsg(trajectory.getFirstWayPoint(), robot_state_msg); + + return publishTrajectoryPath(trajectory_msg, robot_state_msg, blocking); +} + +bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit::core::RobotStateConstPtr& robot_state, bool blocking) +{ + return publishTrajectoryPath(trajectory_msg, *robot_state, blocking); +} + +bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit::core::RobotState& robot_state, bool blocking) +{ + // Convert the robot state to a ROS message + moveit_msgs::msg::RobotState robot_state_msg; + moveit::core::robotStateToRobotStateMsg(robot_state, robot_state_msg); + return publishTrajectoryPath(trajectory_msg, robot_state_msg, blocking); +} + +bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit_msgs::msg::RobotState& robot_state, bool blocking) +{ + // Check if we have enough points + if (trajectory_msg.joint_trajectory.points.empty()) + { + RCLCPP_WARN_STREAM(LOGGER, "Unable to publish trajectory path because trajectory has zero points"); + return false; + } + + // Create the message + moveit_msgs::msg::DisplayTrajectory display_trajectory_msg; + display_trajectory_msg.model_id = robot_model_->getName(); + display_trajectory_msg.trajectory.resize(1); + display_trajectory_msg.trajectory[0] = trajectory_msg; + display_trajectory_msg.trajectory_start = robot_state; + + publishTrajectoryPath(display_trajectory_msg); + + // Wait the duration of the trajectory + if (blocking) + { + auto duration = std::chrono::milliseconds(1000 * trajectory_msg.joint_trajectory.points.back().time_from_start.sec); + // If trajectory has not been parameterized, assume each waypoint takes 50 milliseconds (based on Rviz) + if (duration.count() < std::numeric_limits::epsilon() * 1000 /* ms */) + { + duration = std::chrono::milliseconds(50 * trajectory_msg.joint_trajectory.points.size()); + } + RCLCPP_DEBUG_STREAM(LOGGER, "Waiting for trajectory animation " << duration.count() * 1000 << " seconds"); + + // Check if ROS is ok in intervals + auto counter = 0ms; + const auto CHECK_TIME_INTERVAL = 250ms; // check every fourth second + while (rclcpp::ok() && counter <= duration) + { + counter += CHECK_TIME_INTERVAL; + rclcpp::sleep_for(CHECK_TIME_INTERVAL); + } + } + + return true; +} + +void MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::msg::DisplayTrajectory& display_trajectory_msg) +{ + // Publish message + loadTrajectoryPub(); // always call this before publishing + pub_display_path_->publish(display_trajectory_msg); +} + +bool MoveItVisualTools::publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit::core::LinkModel* ee_parent_link, + const moveit::core::JointModelGroup* arm_jmg, + const rviz_visual_tools::Colors& color) +{ + // Error check + if (!arm_jmg) + { + RCLCPP_FATAL_STREAM(LOGGER, "arm_jmg is NULL"); + return false; + } + + // Always load the robot state before using + loadSharedRobotState(); + + // Convert trajectory into a series of RobotStates + robot_trajectory::RobotTrajectoryPtr robot_trajectory( + new robot_trajectory::RobotTrajectory(robot_model_, arm_jmg->getName())); + robot_trajectory->setRobotTrajectoryMsg(*shared_robot_state_, trajectory_msg); + + return publishTrajectoryLine(robot_trajectory, ee_parent_link, color); +} + +bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + const moveit::core::LinkModel* ee_parent_link, + const rviz_visual_tools::Colors& color) +{ + return publishTrajectoryLine(*robot_trajectory, ee_parent_link, color); +} + +bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory, + const moveit::core::LinkModel* ee_parent_link, + const rviz_visual_tools::Colors& color) +{ + // Error check + if (!ee_parent_link) + { + RCLCPP_FATAL_STREAM(LOGGER, "ee_parent_link is NULL"); + return false; + } + + // Point location datastructure + EigenSTL::vector_Vector3d path; + + // Visualize end effector position of cartesian path + for (std::size_t i = 0; i < robot_trajectory.getWayPointCount(); ++i) + { + const Eigen::Isometry3d& tip_pose = robot_trajectory.getWayPoint(i).getGlobalLinkTransform(ee_parent_link); + + // Error Check + if (tip_pose.translation().x() != tip_pose.translation().x()) + { + RCLCPP_ERROR_STREAM(LOGGER, "NAN DETECTED AT TRAJECTORY POINT i=" << i); + return false; + } + + path.push_back(tip_pose.translation()); + publishSphere(tip_pose, color, rviz_visual_tools::MEDIUM); + } + + const double radius = 0.005; + publishPath(path, color, radius); + + return true; +} + +bool MoveItVisualTools::publishTrajectoryLine(const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const moveit::core::JointModelGroup* arm_jmg, + const rviz_visual_tools::Colors& color) +{ + if (!arm_jmg) + { + RCLCPP_FATAL_STREAM(LOGGER, "arm_jmg is NULL"); + return false; + } + + std::vector tips; + if (!arm_jmg->getEndEffectorTips(tips) || tips.empty()) + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to get end effector tips from jmg"); + return false; + } + + // For each end effector + for (const moveit::core::LinkModel* ee_parent_link : tips) + { + if (!publishTrajectoryLine(trajectory_msg, ee_parent_link, arm_jmg, color)) + return false; + } + + return true; +} + +bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + const moveit::core::JointModelGroup* arm_jmg, + const rviz_visual_tools::Colors& color) +{ + return publishTrajectoryLine(*robot_trajectory, arm_jmg, color); +} + +bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTrajectory& robot_trajectory, + const moveit::core::JointModelGroup* arm_jmg, + const rviz_visual_tools::Colors& color) +{ + if (!arm_jmg) + { + RCLCPP_FATAL_STREAM(LOGGER, "arm_jmg is NULL"); + return false; + } + + std::vector tips; + if (!arm_jmg->getEndEffectorTips(tips) || tips.empty()) + { + RCLCPP_ERROR_STREAM(LOGGER, "Unable to get end effector tips from jmg"); + return false; + } + + // For each end effector + for (const moveit::core::LinkModel* ee_parent_link : tips) + { + if (!publishTrajectoryLine(robot_trajectory, ee_parent_link, color)) + return false; + } + + return true; +} + +bool MoveItVisualTools::publishTrajectoryPoints(const std::vector& robot_state_trajectory, + const moveit::core::LinkModel* ee_parent_link, + const rviz_visual_tools::Colors& color) +{ + // Visualize end effector position of cartesian path + for (std::size_t i = 0; i < robot_state_trajectory.size(); ++i) + { + const Eigen::Isometry3d& tip_pose = robot_state_trajectory[i]->getGlobalLinkTransform(ee_parent_link); + + publishSphere(tip_pose, color); + } + return true; +} + +void MoveItVisualTools::enableRobotStateRootOffet(const Eigen::Isometry3d& offset) +{ + robot_state_root_offset_enabled_ = true; + robot_state_root_offset_ = offset; +} + +void MoveItVisualTools::disableRobotStateRootOffet() +{ + robot_state_root_offset_enabled_ = false; +} + +bool MoveItVisualTools::publishRobotState(const trajectory_msgs::msg::JointTrajectoryPoint& trajectory_pt, + const moveit::core::JointModelGroup* jmg, + const rviz_visual_tools::Colors& color) +{ + return publishRobotState(trajectory_pt.positions, jmg, color); +} + +bool MoveItVisualTools::publishRobotState(const std::vector& joint_positions, + const moveit::core::JointModelGroup* jmg, + const rviz_visual_tools::Colors& color) +{ + // Always load the robot state before using + loadSharedRobotState(); + + // Set robot state + shared_robot_state_->setToDefaultValues(); // reset the state just in case + shared_robot_state_->setJointGroupPositions(jmg, joint_positions); + + // Publish robot state + return publishRobotState(*shared_robot_state_, color); +} + +bool MoveItVisualTools::publishRobotState(const moveit::core::RobotStatePtr& robot_state, + const rviz_visual_tools::Colors& color, + const std::vector& highlight_links) +{ + return publishRobotState(*robot_state.get(), color, highlight_links); +} + +bool MoveItVisualTools::publishRobotState(const moveit::core::RobotState& robot_state, + const rviz_visual_tools::Colors& color, + const std::vector& highlight_links) +{ + // when only a subset of links should be colored, the default message is used rather than the cached solid robot + // messages + rviz_visual_tools::Colors base_color = color; + if (!highlight_links.empty()) + base_color = rviz_visual_tools::DEFAULT; + + // Reference to the correctly colored version of message (they are cached) + // May not exist yet but this will create it + moveit_msgs::msg::DisplayRobotState& display_robot_msg = display_robot_msgs_[base_color]; + + // Check if a robot state message already exists for this color + if (display_robot_msg.highlight_links.empty()) // has not been colored yet, lets create that + { + if (color != rviz_visual_tools::DEFAULT) // ignore color highlights when set to default + { + // Get links names + const std::vector& link_names = + highlight_links.empty() ? robot_state.getRobotModel()->getLinkModelNamesWithCollisionGeometry() : + highlight_links; + display_robot_msg.highlight_links.resize(link_names.size()); + + // Get color + const std_msgs::msg::ColorRGBA& color_rgba = getColor(color); + + // Color every link + for (std::size_t i = 0; i < link_names.size(); ++i) + { + display_robot_msg.highlight_links[i].id = link_names[i]; + display_robot_msg.highlight_links[i].color = color_rgba; + } + } + } + + // Apply the offset + if (robot_state_root_offset_enabled_) + { + loadSharedRobotState(); + + // Copy robot state + *shared_robot_state_ = robot_state; + + applyVirtualJointTransform(*shared_robot_state_, robot_state_root_offset_); + + // Convert state to message + moveit::core::robotStateToRobotStateMsg(*shared_robot_state_, display_robot_msg.state); + } + else + { + // Convert state to message + moveit::core::robotStateToRobotStateMsg(robot_state, display_robot_msg.state); + } + + // Publish + publishRobotState(display_robot_msg); + + // remove highlight links from default message + if (!highlight_links.empty()) + display_robot_msg.highlight_links.clear(); + + return true; +} + +void MoveItVisualTools::publishRobotState(const moveit_msgs::msg::DisplayRobotState& robot_state_msg) +{ + loadRobotStatePub(); + pub_robot_state_->publish(robot_state_msg); +} + +bool MoveItVisualTools::hideRobot() +{ + moveit_msgs::msg::DisplayRobotState display_robot_state_msg; + // Hide the robot state + display_robot_state_msg.hide = true; + + // Publish + publishRobotState(display_robot_state_msg); + return true; +} + +void MoveItVisualTools::showJointLimits(const moveit::core::RobotStatePtr& robot_state) +{ + const std::vector& joints = robot_model_->getActiveJointModels(); + + // Loop through joints + for (std::size_t i = 0; i < joints.size(); ++i) + { + // Assume all joints have only one variable + if (joints[i]->getVariableCount() > 1) + { + // RCLCPP_WARN_STREAM(LOGGER, "Unable to handle joints with more than one var, skipping '" + //<< joints[i]->getName() << "'"); + continue; + } + + double current_value = robot_state->getVariablePosition(joints[i]->getName()); + + // check if bad position + bool out_of_bounds = !robot_state->satisfiesBounds(joints[i]); + + const moveit::core::VariableBounds& bound = joints[i]->getVariableBounds()[0]; + + if (out_of_bounds) + std::cout << MOVEIT_CONSOLE_COLOR_RED; + + std::cout << " " << std::fixed << std::setprecision(5) << bound.min_position_ << "\t"; + double delta = bound.max_position_ - bound.min_position_; + // std::cout << "delta: " << delta << " "; + double step = delta / 20.0; + + bool marker_shown = false; + for (double value = bound.min_position_; value < bound.max_position_; value += step) + { + // show marker of current value + if (!marker_shown && current_value < value) + { + std::cout << "|"; + marker_shown = true; + } + else + std::cout << "-"; + } + // show max position + std::cout << " \t" << std::fixed << std::setprecision(5) << bound.max_position_ << " \t" << joints[i]->getName() + << " current: " << std::fixed << std::setprecision(5) << current_value << std::endl; + + if (out_of_bounds) + std::cout << MOVEIT_CONSOLE_COLOR_RESET; + } +} + +/** + * -------------------------------------------------------------------------------------- + * Private Functions + * -------------------------------------------------------------------------------------- + */ + +planning_scene_monitor::PlanningSceneMonitorPtr MoveItVisualTools::getPlanningSceneMonitor() +{ + if (!psm_) + { + RCLCPP_INFO_STREAM(LOGGER, "No planning scene passed into moveit_visual_tools, creating one."); + loadPlanningSceneMonitor(); + } + return psm_; +} + +bool MoveItVisualTools::checkForVirtualJoint(const moveit::core::RobotState& robot_state) +{ + static const std::string VJOINT_NAME = "virtual_joint"; + + // Check if joint exists + if (!robot_state.getRobotModel()->hasJointModel(VJOINT_NAME)) + { + RCLCPP_WARN_STREAM(LOGGER, "Joint '" << VJOINT_NAME << "' does not exist."); + return false; + } + + // Check if variables exist + if (!robot_state.getRobotModel()->getJointModel(VJOINT_NAME)->hasVariable(VJOINT_NAME + "/trans_x")) + { + // Debug + RCLCPP_WARN_STREAM(LOGGER, "Variables for joint '" << VJOINT_NAME + << "' do not exist. Try making this vjoint " + "floating"); + RCLCPP_WARN_STREAM(LOGGER, "The only available joint variables are:"); + const std::vector& var_names = + robot_state.getRobotModel()->getJointModel(VJOINT_NAME)->getVariableNames(); + std::copy(var_names.begin(), var_names.end(), std::ostream_iterator(std::cout, "\n")); + return false; + } + + return true; +} + +bool MoveItVisualTools::applyVirtualJointTransform(moveit::core::RobotState& robot_state, + const Eigen::Isometry3d& offset) +{ + static const std::string VJOINT_NAME = "virtual_joint"; + + // Error check + if (!checkForVirtualJoint(robot_state)) + { + RCLCPP_WARN_STREAM(LOGGER, "Unable to apply virtual joint transform, hideRobot() functionality is disabled"); + return false; + } + + // Apply translation + robot_state.setVariablePosition(VJOINT_NAME + "/trans_x", offset.translation().x()); + robot_state.setVariablePosition(VJOINT_NAME + "/trans_y", offset.translation().y()); + robot_state.setVariablePosition(VJOINT_NAME + "/trans_z", offset.translation().z()); + + // Apply rotation + Eigen::Quaterniond q(offset.rotation()); + robot_state.setVariablePosition(VJOINT_NAME + "/rot_x", q.x()); + robot_state.setVariablePosition(VJOINT_NAME + "/rot_y", q.y()); + robot_state.setVariablePosition(VJOINT_NAME + "/rot_z", q.z()); + robot_state.setVariablePosition(VJOINT_NAME + "/rot_w", q.w()); + + return true; +} + +} // namespace moveit_visual_tools diff --git a/aloha_ws/src/moveit_visual_tools/src/moveit_visual_tools_demo.cpp b/aloha_ws/src/moveit_visual_tools/src/moveit_visual_tools_demo.cpp new file mode 100644 index 00000000..d34030c6 --- /dev/null +++ b/aloha_ws/src/moveit_visual_tools/src/moveit_visual_tools_demo.cpp @@ -0,0 +1,330 @@ +// Copyright 2015 University of Colorado, Boulder +// +// 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 the copyright holder 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 HOLDER 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 + Desc: Demo implementation of moveit_visual_tools + To use, add a Rviz Marker Display subscribed to topic /moveit_visual_tools +*/ + +// ROS +#include +#include + +// For visualizing things in rviz +#include + +// MoveIt +#include + +// C++ +#include + +// Boost +#include + +using namespace std::literals; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_visual_tools_demo"); + +namespace rvt = rviz_visual_tools; + +namespace moveit_visual_tools +{ +static const std::string PLANNING_GROUP_NAME = "arm"; // RRBot Specific +static const std::string THIS_PACKAGE = "moveit_visual_tools"; + +class VisualToolsDemo +{ +public: + /** + * \brief Constructor + */ + VisualToolsDemo(const rclcpp::Node::SharedPtr& node) : node_(node) + { + visual_tools_ = std::make_shared(node_, "world", "/moveit_visual_tools"); + visual_tools_->loadPlanningSceneMonitor(); + visual_tools_->loadMarkerPub(true); + visual_tools_->loadRobotStatePub("display_robot_state"); + visual_tools_->setManualSceneUpdating(); + + robot_state_ = visual_tools_->getSharedRobotState(); + jmg_ = robot_state_->getJointModelGroup(PLANNING_GROUP_NAME); + // Allow time to publish messages + rclcpp::spin_some(node); + rclcpp::sleep_for(1000ms); + + // Clear collision objects and markers + visual_tools_->deleteAllMarkers(); + visual_tools_->removeAllCollisionObjects(); + visual_tools_->triggerPlanningSceneUpdate(); + rclcpp::sleep_for(1000ms); + + // Show message + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 4; + visual_tools_->publishText(text_pose, "MoveIt Visual Tools", rvt::WHITE, rvt::XLARGE, /*static_id*/ false); + + runRobotStateTests(); + + runDeskTest(); + + runCollisionObjectTests(); + } + + void publishLabelHelper(const Eigen::Isometry3d& pose, const std::string& label) + { + Eigen::Isometry3d pose_copy = pose; + pose_copy.translation().x() -= 0.2; + visual_tools_->publishText(pose_copy, label, rvt::WHITE, rvt::XLARGE, false); + visual_tools_->trigger(); + } + + void runRobotStateTests() + { + // Show 5 random robot states + RCLCPP_INFO(LOGGER, "Showing 5 random robot states"); + for (std::size_t i = 0; i < 5; ++i) + { + robot_state_->setToRandomPositions(jmg_); + visual_tools_->publishRobotState(robot_state_, rvt::DEFAULT); + rclcpp::sleep_for(1000ms); + } + + // Show 5 robot state in different colors + RCLCPP_INFO(LOGGER, "Showing 5 random robot states in different colors"); + for (std::size_t i = 0; i < 5; ++i) + { + robot_state_->setToRandomPositions(jmg_); + visual_tools_->publishRobotState(robot_state_, visual_tools_->getRandColor()); + rclcpp::sleep_for(1000ms); + } + + // Hide the robot + RCLCPP_INFO(LOGGER, "Hiding the robot"); + visual_tools_->hideRobot(); + rclcpp::sleep_for(1000ms); + + // Show the robot + RCLCPP_INFO(LOGGER, "Showing the robot"); + visual_tools_->publishRobotState(robot_state_, rvt::DEFAULT); + rclcpp::sleep_for(1000ms); + } + + void runDeskTest() + { + double common_angle = M_PI * 1.1; + double x_offset = -3.0; + + // -------------------------------------------------------------------- + RCLCPP_INFO_STREAM(LOGGER, "Moving the robot"); + Eigen::Isometry3d robot_pose = Eigen::Isometry3d::Identity(); + robot_pose = robot_pose * Eigen::AngleAxisd(common_angle, Eigen::Vector3d::UnitZ()); + robot_pose.translation().x() = x_offset; + visual_tools_->applyVirtualJointTransform(*robot_state_, robot_pose); + visual_tools_->publishRobotState(robot_state_, rvt::DEFAULT); + rclcpp::sleep_for(1000ms); + + // -------------------------------------------------------------------- + RCLCPP_INFO_STREAM(LOGGER, "Publishing Collision Floor"); + visual_tools_->publishCollisionFloor(-0.5, "Floor", rvt::GREY); + rclcpp::sleep_for(1000ms); + + // -------------------------------------------------------------------- + RCLCPP_INFO_STREAM(LOGGER, "Publishing Collision Wall"); + double wall_x = x_offset - 1.0; + double wall_y = -1.0; + double wall_width = 6.0; + double wall_height = 4; + visual_tools_->publishCollisionWall(wall_x, wall_y, common_angle, wall_width, wall_height, "Wall", rvt::GREEN); + rclcpp::sleep_for(1000ms); + + // -------------------------------------------------------------------- + RCLCPP_INFO_STREAM(LOGGER, "Publishing Collision Table"); + double table_x = x_offset + 1.0; + double table_y = 0; + double table_z = 0; + double table_width = 3; + double table_height = 1; + double table_depth = 1; + visual_tools_->publishCollisionTable(table_x, table_y, table_z, common_angle, table_width, table_height, + table_depth, "Table", rvt::BLUE); + rclcpp::sleep_for(1000ms); + + // Send ROS messages + visual_tools_->triggerPlanningSceneUpdate(); + + // Show 5 random robot states + RCLCPP_INFO(LOGGER, "Showing 5 random robot states"); + for (std::size_t i = 0; i < 5; ++i) + { + robot_state_->setToRandomPositions(jmg_); + visual_tools_->publishRobotState(robot_state_, rvt::DEFAULT); + rclcpp::sleep_for(1000ms); + } + } + + void runCollisionObjectTests() + { + // Create pose + Eigen::Isometry3d pose1 = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity(); + + double space_between_rows = 0.2; + double y = 0; + double step; + + // -------------------------------------------------------------------- + RCLCPP_INFO_STREAM(LOGGER, "Publishing Collision Mesh"); + // TODO: Catch exception + std::string file_path = "file://" + ament_index_cpp::get_package_share_directory(THIS_PACKAGE); + if (file_path == "file://") + RCLCPP_FATAL_STREAM(LOGGER, "Unable to get " << THIS_PACKAGE << " package path "); + file_path.append("/resources/demo_mesh.stl"); + step = 0.4; + for (double i = 0; i <= 1.0; i += 0.1) + { + visual_tools_->publishCollisionMesh(visual_tools_->convertPose(pose1), + "Mesh_" + boost::lexical_cast(i), file_path, + visual_tools_->getRandColor()); + if (!i) + publishLabelHelper(pose1, "Mesh"); + pose1.translation().x() += step; + } + rclcpp::sleep_for(1000ms); + + // -------------------------------------------------------------------- + RCLCPP_INFO_STREAM(LOGGER, "Publishing Collision Block"); + pose1 = Eigen::Isometry3d::Identity(); + y += space_between_rows * 2.0; + pose1.translation().y() = y; + for (double i = 0; i <= 1.0; i += 0.1) + { + double size = 0.1 * (i + 0.5); + visual_tools_->publishCollisionBlock(visual_tools_->convertPose(pose1), + "Block_" + boost::lexical_cast(i), size, + visual_tools_->getRandColor()); + + if (!i) + publishLabelHelper(pose1, "Block"); + pose1.translation().x() += step; + } + rclcpp::sleep_for(1000ms); + + // -------------------------------------------------------------------- + RCLCPP_INFO_STREAM(LOGGER, "Publishing Collision Rectanglular Cuboid"); + double cuboid_min_size = 0.05; + double cuboid_max_size = 0.2; + pose1 = Eigen::Isometry3d::Identity(); + pose2 = Eigen::Isometry3d::Identity(); + y += space_between_rows * 2.0; + pose1.translation().y() = y; + pose2.translation().y() = y; + for (double i = 0; i <= 1.0; i += 0.1) + { + pose2 = pose1; + pose2.translation().x() += i * cuboid_max_size + cuboid_min_size; + pose2.translation().y() += (i * cuboid_max_size + cuboid_min_size) / 2.0; + pose2.translation().z() += i * cuboid_max_size + cuboid_min_size; + visual_tools_->publishCollisionCuboid(pose1.translation(), pose2.translation(), + "Rectangle_" + boost::lexical_cast(i), + visual_tools_->getRandColor()); + if (!i) + publishLabelHelper(pose1, "Cuboid"); + pose1.translation().x() += step; + } + rclcpp::sleep_for(1000ms); + + // -------------------------------------------------------------------- + RCLCPP_INFO_STREAM(LOGGER, "Publishing Collision Cylinder"); + double cylinder_min_size = 0.01; + double cylinder_max_size = 0.3; + pose1 = Eigen::Isometry3d::Identity(); + pose2 = Eigen::Isometry3d::Identity(); + y += space_between_rows * 2.0; + pose1.translation().y() = y; + pose2.translation().y() = y; + for (double i = 0; i <= 1.0; i += 0.1) + { + double radius = 0.1; + pose2 = pose1; + pose2.translation().x() += i * cylinder_max_size + cylinder_min_size; + // pose2.translation().y() += i * cylinder_max_size + cylinder_min_size; + // pose2.translation().z() += i * cylinder_max_size + cylinder_min_size; + visual_tools_->publishCollisionCylinder(pose1.translation(), pose2.translation(), + "Cylinder_" + boost::lexical_cast(i), radius, + visual_tools_->getRandColor()); + + if (!i) + publishLabelHelper(pose1, "Cylinder"); + pose1.translation().x() += step; + + pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ()); + } + + // TODO(davetcoleman): publishCollisionGraph + // TODO(davetcoleman): publishContactPoint + // TODO(davetcoleman): trajectory stuff + // TODO(davetcoleman): gripper stuff + + // Send ROS messages + visual_tools_->triggerPlanningSceneUpdate(); + rclcpp::sleep_for(1000ms); + } + +private: + // A shared node handle + rclcpp::Node::SharedPtr node_; + + // For visualizing things in rviz + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + + // MoveIt Components + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + const moveit::core::JointModelGroup* jmg_; + + moveit::core::RobotStatePtr robot_state_; +}; // end class + +} // namespace moveit_visual_tools + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("moveit_visual_tools_demo", node_options); + RCLCPP_INFO_STREAM(LOGGER, "Visual Tools Demo"); + + moveit_visual_tools::VisualToolsDemo demo(node); + + RCLCPP_INFO_STREAM(LOGGER, "Shutting down."); + rclcpp::shutdown(); + + return 0; +}