├── .clang-format ├── .github └── workflows │ └── main.yml ├── .github_actions.yml ├── .run-clang-format ├── README.md ├── ROS1_BRIDGE_BUILD_INSTRUCTIONS.md ├── changes.diff ├── crs.rosinstall ├── crs_application ├── CMakeLists.txt ├── README.md ├── config │ ├── main │ │ ├── crs.yaml │ │ ├── motion_planning │ │ │ └── MP_config.yaml │ │ ├── part_localization.yaml │ │ └── rework │ │ │ ├── region_crop.yaml │ │ │ └── region_detection.yaml │ └── swri │ │ ├── crs.yaml │ │ ├── motion_planning │ │ └── MP_config.yaml │ │ ├── part_localization.yaml │ │ └── rework │ │ ├── region_crop.yaml │ │ └── region_detection.yaml ├── include │ └── crs_application │ │ ├── common │ │ ├── common.h │ │ ├── config.h │ │ ├── datatypes.h │ │ └── simulation_object_spawner.h │ │ ├── crs_executive.h │ │ └── task_managers │ │ ├── motion_planning_manager.h │ │ ├── part_registration_manager.h │ │ ├── part_rework_manager.h │ │ ├── process_execution_manager.h │ │ └── scan_acquisition_manager.h ├── launch │ ├── crs.launch.py │ ├── crs.launch.xml │ ├── load_preview_robot_model.launch.xml │ ├── load_robot_model.launch.xml │ ├── registration_startup.launch.py │ └── rework_startup.launch.py ├── package.xml ├── resources │ ├── crs.rviz │ └── crs_process.scxml └── src │ ├── common │ ├── config.cpp │ └── simulation_object_spawner.cpp │ ├── crs_application.cpp │ ├── crs_executive.cpp │ └── task_managers │ ├── motion_planning_manager.cpp │ ├── part_registration_manager.cpp │ ├── part_rework_manager.cpp │ ├── process_execution_manager.cpp │ └── scan_acquisition_manager.cpp ├── crs_area_selection ├── CMakeLists.txt ├── include │ └── crs_area_selection │ │ ├── area_selector.h │ │ ├── area_selector_parameters.h │ │ ├── filter.h │ │ ├── filter_impl.h │ │ └── selection_artist.h ├── package.xml └── src │ ├── area_selection_node.cpp │ ├── area_selector.cpp │ └── selection_artist.cpp ├── crs_gazebo_plugins ├── CMakeLists.txt ├── include │ └── crs_gazebo_plugins │ │ └── joint_pose_trajectory.hpp ├── package.xml └── src │ └── joint_pose_trajectory.cpp ├── crs_gui ├── CMakeLists.txt ├── README.md ├── config │ └── crs_application_demo.rviz ├── include │ └── crs_gui │ │ ├── application_panel.h │ │ ├── register_ros_msgs_for_qt.h │ │ └── widgets │ │ ├── crs_application_widget.h │ │ ├── part_selection_widget.h │ │ ├── polygon_area_selection_widget.h │ │ └── state_machine_interface_widget.h ├── launch │ ├── area_selection_demo.launch.xml │ ├── crs_application_demo.launch.xml │ └── state_machine_interface_demo.launch.xml ├── package.xml ├── plugin_description.xml ├── src │ ├── application_panel.cpp │ ├── nodes │ │ ├── area_selection_widget_demo.cpp │ │ ├── crs_application_demo.cpp │ │ ├── part_selection_widget_demo.cpp │ │ └── state_machine_interface_widget_demo.cpp │ └── widgets │ │ ├── crs_application_widget.cpp │ │ ├── part_selection_widget.cpp │ │ ├── polygon_area_selection_widget.cpp │ │ └── state_machine_interface_widget.cpp └── ui │ ├── crs_application.ui │ ├── part_selection.ui │ ├── polygon_area_selection_widget.ui │ └── state_machine_interface.ui ├── crs_hardware.rosinstall ├── crs_motion_planning ├── CMakeLists.txt ├── README.md ├── include │ └── crs_motion_planning │ │ ├── path_planning_utils.h │ │ └── path_processing_utils.h ├── package.xml └── src │ ├── motion_planning_server.cpp │ ├── path_planning_utils.cpp │ ├── path_processing_utils.cpp │ └── process_planner_test.cpp ├── crs_msgs ├── CMakeLists.txt ├── msg │ ├── ProcessConfiguration.msg │ ├── ProcessMotionPlan.msg │ └── ToolProcessPath.msg ├── package.xml └── srv │ ├── CallFreespaceMotion.srv │ ├── CropToolpaths.srv │ ├── ExecuteAction.srv │ ├── GetAvailableActions.srv │ ├── GetConfiguration.srv │ ├── GetROISelection.srv │ ├── LoadPart.srv │ ├── LocalizeToPart.srv │ ├── PlanProcessMotions.srv │ ├── RobotPositioner.srv │ └── RunRobotScript.srv ├── crs_perception ├── CMakeLists.txt ├── README.md ├── include │ └── crs_perception │ │ └── model_to_point_cloud.hpp ├── package.xml └── src │ ├── model_to_point_cloud.cpp │ └── nodes │ ├── localize_to_part.cpp │ └── test_localize_to_part.cpp ├── crs_process_data ├── CMakeLists.txt ├── data │ ├── main │ │ └── toolpaths │ │ │ ├── part_1 │ │ │ ├── crs.yaml │ │ │ ├── job_toolpath_ls10cm_ps15cm_r0d.yaml │ │ │ ├── job_toolpath_ls10cm_ps20cm_r0d.yaml │ │ │ ├── job_toolpath_ls20cm_ps20cm_r90d.yaml │ │ │ └── part_1.stl │ │ │ ├── part_2 │ │ │ ├── configs │ │ │ │ └── job_0_degrees_20200520T192241.997282_config.yaml │ │ │ ├── crs.yaml │ │ │ ├── job_0_degrees_20200520T192241.997282.yaml │ │ │ ├── job_45_degrees_20200520T192321.124980.yaml │ │ │ ├── job_90_degrees_20200520T192342.971284.yaml │ │ │ ├── job_neg_45_degrees_20200520T192431.034149.yaml │ │ │ ├── job_neg_90_degrees_20200520T192403.508846.yaml │ │ │ └── part_2.stl │ │ │ ├── part_3 │ │ │ ├── crs.yaml │ │ │ ├── job_0_degrees_20200520T192928.608964.yaml │ │ │ ├── job_45_degrees_20200520T193117.992144.yaml │ │ │ ├── job_90_degrees_20200520T193139.616155.yaml │ │ │ ├── job_neg_45_degrees_20200520T193244.746333.yaml │ │ │ ├── job_neg_90_degrees_20200520T193201.590978.yaml │ │ │ └── part_3.stl │ │ │ └── part_5 │ │ │ ├── crs.yaml │ │ │ ├── job_toolpath_ls10cm_ps10cm_r0d.yaml │ │ │ ├── job_toolpath_ls20cm_ps10cm_r0d.yaml │ │ │ ├── job_toolpath_ls20cm_ps15cm_r-20d.yaml │ │ │ ├── job_toolpath_ls20cm_ps15cm_r0d.yaml │ │ │ ├── job_toolpath_ls20cm_ps15cm_r60d.yaml │ │ │ └── part_5.stl │ └── surrogate1 │ │ └── toolpaths │ │ ├── fake_part │ │ ├── configs │ │ │ └── fake_toolpath_config.yaml │ │ ├── crs.yaml │ │ ├── fake_part.stl │ │ └── fake_toolpath.yaml │ │ ├── part_2 │ │ ├── crs.yaml │ │ ├── job_0_degrees_20200520T192241.997282.yaml │ │ ├── job_45_degrees_20200520T192321.124980.yaml │ │ ├── job_90_degrees_20200520T192342.971284.yaml │ │ ├── job_90_mid.yaml │ │ ├── job_90_red.yaml │ │ ├── job_90_red_red.yaml │ │ ├── job_neg_45_degrees_20200520T192431.034149.yaml │ │ ├── job_neg_90_degrees_20200520T192403.508846.yaml │ │ ├── part_2.stl │ │ └── part_2_original.stl │ │ └── swri_part │ │ ├── crs.yaml │ │ ├── job_job_ls10cm_ps10cm_r0d_20200804T170757.524898.yaml │ │ ├── job_reduced.yaml │ │ ├── job_toolpath_ls10cm_ps10cm_r90d_20200804T194320.202533.yaml │ │ └── swri_part.stl └── package.xml ├── crs_robot_comms ├── CMakeLists.txt ├── README.md ├── package.xml └── src │ ├── ur_comms.cpp │ └── ur_comms_sim.cpp ├── crs_support ├── CMakeLists.txt ├── launch │ ├── environment_launch.launch.py │ ├── environment_launch_swri.launch.py │ ├── gazebo_swri_demo.launch.py │ ├── gazebo_test.launch.py │ ├── spawn_cart.launch.py │ └── spawn_part.launch.py ├── meshes │ ├── Mockup │ │ ├── collision │ │ │ ├── Mockup3_6_Cart_collision.stl │ │ │ ├── empty_cart_collision.stl │ │ │ └── mockup_cart_collision_v3.stl │ │ └── visual │ │ │ ├── Mockup3_6_Frame_noTools_small.stl │ │ │ ├── empty_cart.stl │ │ │ ├── mock_up_frame.ply │ │ │ ├── mock_up_frame.stl │ │ │ ├── mockup_cart_v3.stl │ │ │ ├── mockup_cart_v4.stl │ │ │ └── mockup_frame_v3.stl │ ├── Parts │ │ ├── collision │ │ │ └── part1_ch.stl │ │ └── visual │ │ │ ├── part1_ch.stl │ │ │ └── part_from_mockup.stl │ ├── Swri_demo │ │ └── mold_transformed.stl │ ├── end_effector │ │ ├── collision │ │ │ ├── sander_collision.stl │ │ │ └── tool_collision.stl │ │ └── visual │ │ │ ├── ee1.stl │ │ │ ├── ee2.stl │ │ │ ├── ee3.stl │ │ │ ├── sander.stl │ │ │ └── tool.stl │ └── ur10e │ │ ├── collision │ │ ├── base.stl │ │ ├── forearm.stl │ │ ├── forearm_multibody.dae │ │ ├── shoulder.stl │ │ ├── upper_arm_multibody.dae │ │ ├── upperarm.stl │ │ ├── wrist1.stl │ │ ├── wrist2.stl │ │ └── wrist3.stl │ │ └── visual │ │ ├── base.dae │ │ ├── forearm.dae │ │ ├── shoulder.dae │ │ ├── upperarm.dae │ │ ├── wrist1.dae │ │ ├── wrist2.dae │ │ └── wrist3.dae ├── package.xml ├── sdf │ ├── cart.sdf │ └── part1.sdf ├── toolpaths │ └── scanned_part1 │ │ ├── job_45degrees.yaml │ │ ├── job_90degrees.yaml │ │ └── job_long_strips.yaml ├── urdf │ ├── crs.urdf.xacro │ ├── swri_demo.urdf.xacro │ ├── swri_demo_sander.urdf.xacro │ └── ur10e_robot.srdf └── worlds │ └── crs.world ├── crs_toolpath_gen ├── CMakeLists.txt └── package.xml ├── crs_utils_py ├── __init__.py ├── crs_utils_py │ ├── .gitignore │ ├── __init__.py │ └── joint_state_publisher.py ├── package.xml ├── resource │ └── crs_utils_py ├── setup.cfg └── setup.py └── skip.mixin /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | AlignEscapedNewlinesLeft: false 5 | AlignTrailingComments: true 6 | AlignAfterOpenBracket: Align 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortFunctionsOnASingleLine: true 9 | AllowShortIfStatementsOnASingleLine: false 10 | AllowShortLoopsOnASingleLine: false 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakBeforeMultilineStrings: false 13 | AlwaysBreakTemplateDeclarations: true 14 | BinPackArguments: false 15 | BinPackParameters: false 16 | BreakBeforeBinaryOperators: false 17 | BreakBeforeTernaryOperators: false 18 | BreakConstructorInitializersBeforeComma: true 19 | ColumnLimit: 120 20 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 21 | ConstructorInitializerIndentWidth: 2 22 | ContinuationIndentWidth: 4 23 | Cpp11BracedListStyle: false 24 | DerivePointerBinding: false 25 | ExperimentalAutoDetectBinPacking: false 26 | IndentCaseLabels: true 27 | IndentFunctionDeclarationAfterType: false 28 | IndentWidth: 2 29 | MaxEmptyLinesToKeep: 1 30 | NamespaceIndentation: None 31 | ObjCSpaceBeforeProtocolList: true 32 | PenaltyBreakBeforeFirstCallParameter: 19 33 | PenaltyBreakComment: 60 34 | PenaltyBreakFirstLessLess: 1000 35 | PenaltyBreakString: 1 36 | PenaltyExcessCharacter: 1000 37 | PenaltyReturnTypeOnItsOwnLine: 90 38 | PointerBindsToType: true 39 | SortIncludes: false 40 | SpaceAfterControlStatementKeyword: true 41 | SpaceAfterCStyleCast: false 42 | SpaceBeforeAssignmentOperators: true 43 | SpaceInEmptyParentheses: false 44 | SpacesBeforeTrailingComments: 2 45 | SpacesInAngles: false 46 | SpacesInCStyleCastParentheses: false 47 | SpacesInParentheses: false 48 | Standard: Auto 49 | TabWidth: 2 50 | UseTab: Never 51 | 52 | # Configure each individual brace in BraceWrapping 53 | BreakBeforeBraces: Custom 54 | 55 | # Control of individual brace wrapping cases 56 | BraceWrapping: { 57 | AfterClass: 'true' 58 | AfterControlStatement: 'true' 59 | AfterEnum : 'true' 60 | AfterFunction : 'true' 61 | AfterNamespace : 'true' 62 | AfterStruct : 'true' 63 | AfterUnion : 'true' 64 | BeforeCatch : 'true' 65 | BeforeElse : 'true' 66 | IndentBraces : 'false' 67 | } 68 | ... 69 | 70 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | pull_request: 6 | schedule: 7 | - cron: '0 5 * * *' 8 | 9 | jobs: 10 | industrial_ci: 11 | strategy: 12 | fail-fast: false 13 | matrix: 14 | env: 15 | - {OS_NAME: ubuntu, 16 | OS_CODE_NAME: bionic, 17 | ROS_DISTRO: eloquent, 18 | ROS_REPO: main, 19 | ROSDEP_SKIP_KEYS: "catkin ompl message_runtime message_generation dynamic_reconfigure roscpp roslib eigen_conversions scxml_core tesseract tesseract_kinematics tesseract_visualization trajopt trajopt_utils trajopt_sco descartes_light descartes_ikfast descartes_samplers ur_ikfast_kinematics iterative_spline_parameterization realsense2 pcl_ros", 20 | CLANG_FORMAT_CHECK: file, 21 | CLANG_FORMAT_VERSION: 8, 22 | BADGE: clang-format} 23 | - {OS_NAME: ubuntu, 24 | OS_CODE_NAME: bionic, 25 | UPSTREAM_WORKSPACE: 'crs.rosinstall', 26 | ROSDEP_SKIP_KEYS: "catkin ompl message_runtime message_generation dynamic_reconfigure roscpp roslib eigen_conversions scxml_core tesseract tesseract_kinematics tesseract_visualization trajopt trajopt_utils trajopt_sco descartes_light descartes_ikfast descartes_samplers ur_ikfast_kinematics iterative_spline_parameterization realsense2 pcl_ros", 27 | DOCKER_IMAGE: "mpowelson/bionic-qt-ros:eloquent", 28 | BADGE: eloquent, 29 | UPSTREAM_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug", 30 | TARGET_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug", 31 | DOWNSTREAM_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug", 32 | ADDITIONAL_DEBS: "libompl-dev libyaml-cpp-dev", 33 | NOT_TEST_BUILD: true, 34 | BUILDER: "colcon"} 35 | 36 | runs-on: ubuntu-latest 37 | steps: 38 | - uses: actions/checkout@v1 39 | - name: Free Disk Space 40 | run: | 41 | sudo swapoff -a 42 | sudo rm -f /swapfile 43 | sudo apt clean 44 | docker rmi $(docker image ls -aq) 45 | df -h 46 | - uses: 'ros-industrial/industrial_ci@master' 47 | env: ${{matrix.env}} 48 | -------------------------------------------------------------------------------- /.github_actions.yml: -------------------------------------------------------------------------------- 1 | .github/workflows/main.yml -------------------------------------------------------------------------------- /.run-clang-format: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-8 -style=file -i {} \; 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Collaborative-Robotic-Sanding 2 | The CRS (Collaborative Robotic Sanding) Application buit in ROS2 eloquent 3 | 4 | [![Build Status](https://github.com/swri-robotics/collaborative-robotic-sanding/workflows/CI/badge.svg)](https://github.com/swri-robotics/collaborative-robotic-sanding/actions?query=branch%3Amaster+) 5 | [![Github Issues](https://img.shields.io/github/issues/swri-robotics/collaborative-robotic-sanding.svg)](http://github.com/swri-robotics/collaborative-robotic-sanding/issues) 6 | 7 | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) 8 | [![license - bsd 2 clause](https://img.shields.io/:license-BSD%202--Clause-blue.svg)](https://opensource.org/licenses/BSD-2-Clause) 9 | 10 | 11 | --- 12 | ### Workspace Setup 13 | #### Create the workspace directory 14 | ``` 15 | mkdir -p ~/crs_ws/src # Make a new workspace and source directory 16 | cd ~/crs_ws/src # Navigate to the workspace source directory 17 | git clone https://github.com/swri-robotics/collaborative-robotic-sanding.git # clone the repository 18 | ``` 19 | 20 | #### Download source dependencies 21 | - Install [wstool](http://wiki.ros.org/wstool) 22 | - cd into your colcon workspace root directory 23 | - Run wstool as follows 24 | ``` 25 | cd ~/crs_ws 26 | wstool init src src/collaborative-robotic-sanding/crs.rosinstall 27 | wstool update -t src 28 | ``` 29 | 30 | #### Download debian dependences 31 | - Install [rosdep](http://wiki.ros.org/rosdep) 32 | - From the root directory of your workspace run the following: 33 | ``` 34 | rosdep install --from-path src --ignore-src -ry 35 | ``` 36 | #### Download additional REQUIRED resources 37 | - ros-eloquent-launch-xml 38 | ``` 39 | sudo apt install ros-eloquent-launch-xml 40 | ``` 41 | This allows using xml formatted launch files 42 | 43 | - **QT5**, which is a dependency of [ros_scxml](https://github.com/swri-robotics/ros_scxml) therefore follow the instructions provided [here](https://github.com/swri-robotics/ros_scxml) 44 | 45 | --- 46 | #### Issues 47 | - To run on hardware it is necessary to build the ros1 robot driver and the bridge in separate workspace, see instructions on that below. 48 | --- 49 | ### Build 50 | ##### ROS2 Application 51 | This will build the main application, from the workspace run the following 52 | ``` 53 | colcon build --symlink-install 54 | ``` 55 | **This application is all that is necessary to run the system in simulation mode with gazebo** 56 | 57 | --- 58 | 59 | ### Setup 60 | #### Data Directory 61 | Create a soft link (shortcut) in the home directory as follows 62 | 1. cd into the `crs_process_data` package 63 | ``` 64 | cd crs_process_data/data/main/toolpaths 65 | ``` 66 | > The **main** directory holds the toolpaths for the main workcell, choose **surrogate1** if that workcell is to be used instead. 67 | 68 | 2. Create soft link 69 | ``` 70 | ln -s $(pwd) $HOME/crs_data 71 | 72 | --- 73 | ### Running Application 74 | - See the instructions [here](crs_application/README.md) 75 | 76 | --- 77 | ### Hardware dependencies 78 | The following instructions are necessary when running on real hardware: 79 | #### Camera Driver 80 | - Go to [Framos Depth Camera](https://www.framos.com/en/industrial-depth-cameras#downloads). Download and Extract the [tar file](https://www.framos.com/framos3d/D400e/Software/FRAMOS_D400e_Software_Package_v1-8-0_Linux64_x64.tar.gz) and then install the FRAMOS-librealsense2-2.29.8-Linux64_x64.deb debian 81 | - Download source and debian dependencies 82 | ``` 83 | cd ~/crs_ws 84 | wstool merge -t src src/collaborative-robotic-sanding/crs_hardware.rosinstall 85 | wstool update -t src 86 | rosdep install --from-path src --ignore-src -ry 87 | colcon build --symlink-install 88 | ``` 89 | 90 | #### Build ROS1 Robot Driver and Bridge 91 | - Follow instructions [here](ROS1_BRIDGE_BUILD_INSTRUCTIONS.md) 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /ROS1_BRIDGE_BUILD_INSTRUCTIONS.md: -------------------------------------------------------------------------------- 1 | # Hardware Setup 2 | 3 | This application works on the Universal UR10 robot, as of now the ROS2 driver is not available therefore it is necessary to use the ROS1 bridge to allow the ROS1 robot driver to communicate with the ROS2 application 4 | 5 | --- 6 | ## Requirements 7 | ### ROS1 8 | - Install [ROS1 melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) 9 | ### Catkin 10 | - Install [catkin tools](https://catkin-tools.readthedocs.io/en/latest/installing.html) 11 | 12 | ### Wstool 13 | - Install [wstool](http://wiki.ros.org/wstool#Installation) 14 | --- 15 | ## Setting up Wokrspaces 16 | ### ROS1 Workspace 17 | This workspace contains the robot driver node and a few other essential componets 18 | - Create a **crs_ros1_ws** ROS 1 workspace directory 19 | ``` 20 | source /opt/ros/melodic/setup.bash # Source ROS1 21 | mkdir -p ~/crs_ros1_ws/src # Make a new workspace and source directory 22 | cd ~/crs_ros1_ws/ # Navigate to the workspace root 23 | catkin init # Initialize it 24 | ``` 25 | > More info on catkin workspaces [here](https://catkin-tools.readthedocs.io/en/latest/quick_start.html) 26 | - Clone the **collaborative-robotic-sanding-ros1** repository 27 | ``` 28 | cd ~/crs_ros1_ws/src 29 | git clone https://github.com/swri-robotics/collaborative-robotic-sanding-ros1.git 30 | ``` 31 | - Download source dependencies with wstool 32 | ``` 33 | cd ~/crs_ros1_ws/ 34 | wstool init src src/collaborative-robotic-sanding-ros1/crs.rosinstall 35 | ``` 36 | - Downlad debian dependences 37 | ``` 38 | rosdep install --from-path src --ignore-src -ry 39 | ``` 40 | > You'll need admin priviledges to install 41 | - Build with catkin 42 | ``` 43 | catkin build 44 | ``` 45 | ### ROS1 Bridge Workspace 46 | This workspace contains the bridge implementation, it allows the ROS1 and ROS2 application to communicate. 47 | #### WARNING!!! : **It is requried that the ROS1 amd ROS2 workspaces have been built prior to building the bridge* 48 | - Create another workspace for the bridge 49 | ``` 50 | source /opt/ros/melodic/setup.bash # Source ROS1 51 | source /opt/ros/eloquent/setup.bash # Source ROS2 52 | mkdir -p ~/crs_ros_bridge_ws/src # Make a new workspace and source space 53 | cd ~/crs_ros_bridge_ws/ # Navigate to the workspace root 54 | ``` 55 | - Clone the bridge repository 56 | ``` 57 | cd ~/crs_ros_bridge_ws/src 58 | git clone https://github.com/swri-robotics/crs_ros_bridge_ws.git 59 | ``` 60 | - Download source dependencies 61 | ``` 62 | cd ~/crs_ros_bridge_ws/ 63 | wstool init src src/crs_ros_bridge_ws/crs.rosinstall 64 | ``` 65 | - Downlad debian dependences 66 | ``` 67 | rosdep install --from-path src --ignore-src -ry 68 | ``` 69 | > You'll need admin priviledges to install 70 | - Overlay CRS ROS1 and ROS2 workspaces 71 | **!!!It is assumed that your ROS2 workspace is located in the `~/crs_ws/` directory** 72 | - Source the workspaces in the following order 73 | ``` 74 | source ~/crs_ros1_ws/devel/setup.bash 75 | source ~/crs_ws/install/local_setup.bash 76 | colcon build --symlink-install 77 | ``` 78 | -------------------------------------------------------------------------------- /crs.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: descartes_light, uri: 'https://github.com/swri-robotics/descartes_light.git', version: 6addf6b} 2 | - git: {local-name: octomap_msgs, uri: 'https://github.com/AcutronicRobotics/octomap_msgs.git', version: d0c420f} 3 | - git: {local-name: opw_kinematics, uri: 'https://github.com/Jmeyer1292/opw_kinematics.git', version: 8ca5a07b} 4 | - git: {local-name: tesseract, uri: 'https://github.com/ros-industrial-consortium/tesseract.git', version: b508c89} 5 | - git: {local-name: tesseract_ext, uri: 'https://github.com/ros-industrial-consortium/tesseract_ext.git', version: 6b61b68} 6 | - git: {local-name: tesseract_ros2, uri: 'https://github.com/ros-industrial-consortium/tesseract_ros2.git', version: 458535b} 7 | - git: {local-name: trajopt_ros, uri: 'https://github.com/ros-industrial-consortium/trajopt_ros.git', version: e6bb280} 8 | - git: {local-name: ifopt, uri: 'https://github.com/ethz-adrl/ifopt.git', version: 15fe9b3} 9 | - git: {local-name: ros_scxml, uri: 'https://github.com/swri-robotics/ros_scxml.git', version: master} 10 | - git: {local-name: ur_ikfast_kinematics, uri: 'https://github.com/schornakj/ur_ikfast_kinematics.git', version: 533d183} 11 | - git: {local-name: iterative_spline_parameterization, uri: 'https://github.com/schornakj/iterative_spline_parameterization.git', version: 81df45a} 12 | - git: {local-name: Universal_Robots_ROS_Driver, uri: 'https://github.com/marrts/Universal_Robots_ROS_Driver.git', version: ros2} 13 | - git: {local-name: cartesian_trajectory_msgs, uri: 'https://github.com/marrts/cartesian_trajectory_msgs.git', version: ros2} 14 | - git: {local-name: region_detection, uri: 'https://github.com/swri-robotics/Region-Detection.git', version: master} 15 | -------------------------------------------------------------------------------- /crs_application/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crs_application) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | #add_compile_options(-Wall -Wextra -Wno-pedantic -Wno-missing-field-initializers) 16 | add_compile_options("-w") 17 | endif() 18 | 19 | # find dependencies 20 | find_package(Boost COMPONENTS system filesystem REQUIRED) 21 | find_package(ament_cmake REQUIRED) 22 | find_package(rclcpp REQUIRED) 23 | find_package(rclcpp_components REQUIRED) 24 | find_package(crs_msgs REQUIRED) 25 | find_package(control_msgs REQUIRED) 26 | find_package(tf2_eigen REQUIRED) 27 | find_package(rclcpp_action REQUIRED) 28 | find_package(scxml_core REQUIRED) 29 | find_package(Qt5 REQUIRED COMPONENTS Widgets) 30 | find_package(crs_motion_planning REQUIRED) 31 | find_package(Eigen3 REQUIRED NO_MODULE) 32 | find_package(yaml_cpp_vendor REQUIRED) 33 | find_package(gazebo_msgs REQUIRED) 34 | find_package(region_detection_msgs REQUIRED) 35 | 36 | ################################## 37 | # build 38 | ################################## 39 | add_library(${PROJECT_NAME} SHARED 40 | src/crs_executive.cpp 41 | src/common/config.cpp 42 | src/common/simulation_object_spawner.cpp 43 | src/task_managers/scan_acquisition_manager.cpp 44 | src/task_managers/part_registration_manager.cpp 45 | src/task_managers/part_rework_manager.cpp 46 | src/task_managers/motion_planning_manager.cpp 47 | src/task_managers/process_execution_manager.cpp) 48 | target_link_libraries(${PROJECT_NAME} 49 | scxml_core::scxml_core 50 | Eigen3::Eigen 51 | crs_motion_planning::crs_motion_planning_path_processing_utils 52 | ) 53 | target_include_directories(${PROJECT_NAME} PUBLIC 54 | "$" 55 | "$") 56 | target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC) 57 | ament_target_dependencies(${PROJECT_NAME} 58 | rclcpp 59 | rclcpp_components 60 | crs_msgs 61 | crs_motion_planning 62 | control_msgs 63 | tf2_eigen 64 | rclcpp_action 65 | yaml_cpp_vendor 66 | gazebo_msgs 67 | region_detection_msgs) 68 | 69 | add_executable(${PROJECT_NAME}_node src/crs_application.cpp) 70 | target_link_libraries(${PROJECT_NAME}_node 71 | ${PROJECT_NAME} 72 | ) 73 | 74 | ################################## 75 | # install 76 | ################################## 77 | list (APPEND PACKAGE_LIBRARIES ${PROJECT_NAME}) 78 | install(TARGETS ${PACKAGE_LIBRARIES} 79 | EXPORT ${PROJECT_NAME}-targets 80 | DESTINATION lib) 81 | 82 | install(TARGETS ${PROJECT_NAME}_node 83 | DESTINATION lib/${PROJECT_NAME}) 84 | 85 | install(DIRECTORY resources launch config 86 | DESTINATION share/${PROJECT_NAME}/ 87 | PATTERN ".svn" EXCLUDE 88 | ) 89 | 90 | if(BUILD_TESTING) 91 | find_package(ament_lint_auto REQUIRED) 92 | # the following line skips the linter which checks for copyrights 93 | # uncomment the line when a copyright and license is not present in all source files 94 | #set(ament_cmake_copyright_FOUND TRUE) 95 | # the following line skips cpplint (only works in a git repo) 96 | # uncomment the line when this package is not in a git repo 97 | #set(ament_cmake_cpplint_FOUND TRUE) 98 | ament_lint_auto_find_test_dependencies() 99 | endif() 100 | 101 | ament_package() 102 | -------------------------------------------------------------------------------- /crs_application/README.md: -------------------------------------------------------------------------------- 1 | # Run Application 2 | **WARNING: This application uses xml launch files which is only supported in ROS Eloquent, make sure to have installed ** 3 | `apt install ros-eloquent-launch-xml` 4 | --- 5 | ## System Startup 6 | ### On Hardware 7 | Prior to running you must've build the ROS1 driver and ROS bridge as described in ../ROS1_BRIDGE_BUILD_INSTRUCTIONS.md 8 | 9 | 1. #### Run ROS1 driver 10 | ``` 11 | source /opt/ros/melodic/setup.bash 12 | cd ~/crs_ros1_ws/ 13 | source devel/setup.bash 14 | roslaunch crs_support_ros1 crs_startup.launch 15 | ``` 16 | 17 | **You should see the UR teach pendant showing "Running" under the program tab** 18 | 19 | 2. #### Run ROS Bridge workspace 20 | - Open another terminal window and navigate to ROS Bridge workspace 21 | and run the following lines in the terminal 22 | ``` 23 | source /opt/ros/melodic/setup.bash 24 | source /opt/ros/eloquent/setup.bash 25 | cd ~/crs_ros_bridge_ws/ 26 | source install/local_setup.bash 27 | ros2 launch crs_bridge_support combined_bridge.launch.py 28 | ``` 29 | 3. #### Run ROS2 Application 30 | - Open a third terminal window and navigate to ROS2 workspace and run the following lines in the terminal 31 | ``` 32 | source /opt/ros/eloquent/setup.bash 33 | cd ~/crs_ws 34 | source install/local_setup.bash 35 | ros2 launch crs_application crs.launch.xml sim:=false 36 | ``` 37 | This last command will launch the RVIZ application where you should be able to see the robot in the mockup cell along with the user interface to control the application. 38 | 39 | ### In Simulation 40 | - Source the workspace 41 | ``` 42 | source /opt/ros/eloquent/setup.bash 43 | cd ~/crs_ws 44 | source install/local_setup.bash 45 | ``` 46 | - Start 47 | Run the *crs.launch.xml* launch file 48 | ``` 49 | ros2 launch crs_application crs.launch.xml 50 | ``` 51 | 52 | This last command will launch the RVIZ application where you should be able to see the robot in the mockup cell along with the user interface to control the application. 53 | It will start the SM and all of the required nodes. When initialization is complete the application will go into the `Ready::Wait_User_Cmd` 54 | 55 | ## System Usage 56 | ### Using GUI 57 | - Select the part to be processed in the "Load Part" Screen 58 | - On the right panel of the "Load Part" Screen select the toolpath configuration file 59 | - Press "Load Selected Part" button and wait for the part to load in RVIZ 60 | - Once loaded hit "Approve" button to move to next state load part->registration->planning->execution 61 | 62 | ### Using Command line 63 | - Query the current state 64 | Open a new terminal and run the following: 65 | ``` 66 | ros2 topic echo /crs/current_state 67 | ``` 68 | You should see the current state id printed on the terminal repeateadly 69 | 70 | - Query available actions in the current state 71 | From another terminal run the following 72 | ``` 73 | ros2 service call /crs/get_available_actions crs_msgs/srv/GetAvailableActions 'search_pattern: "user"' 74 | ``` 75 | The response may look as follows: 76 | ``` 77 | response: crs_msgs.srv.GetAvailableActions_Response(action_ids=['user_done', 'user_approves'], succeeded=False, err_msg='') 78 | ``` 79 | This means the the actions "user_done" and "user_approves" are valid and can be executed. 80 | - Execute an action 81 | In a new terminal run the following: 82 | ``` 83 | ros2 service call /crs/execute_action crs_msgs/srv/ExecuteAction 'action_id: user_approves' 84 | ``` 85 | If the current state allows that action then you should see the SM transition to another state 86 | -------------------------------------------------------------------------------- /crs_application/config/main/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: true 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | # joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994] 8 | joint_position: [1.03515, 0.14504, -2.359510, -1.8396, -1.00758, 0.785747] 9 | process_path: 10 | tool_speed: 0.1 11 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [px, py, pz, rx, ry, rz] relative to default tool 12 | retreat_dist: 0.03 13 | approach_dist: 0.03 14 | tool_frame: "sander_center_link" 15 | target_force: 50 16 | media_change: 17 | change_time: 300.0 # seconds 18 | change_pose: [-0.381, -0.202, 1.5, 0.0, -0.786, 1.57] # [px, py, pz, rx, ry, rz] in world coordinates 19 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 20 | joint_position: [] 21 | preview: 22 | time_scaling: 1.0 23 | scan_acquisition: 24 | scan_poses: 25 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 26 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 27 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 28 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 29 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 30 | - pose: [0.016, 0.604, 1.700, 2.5848052, -0.6232238, 3.0450751] 31 | tool_frame: "camera_link_optical" 32 | skip_on_failure: true 33 | process_execution: 34 | time_tolerance: 5.0 # seconds 35 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] # radians 36 | cartesian_path_tolerance: [0.015, 0.015, 0.025, 0.15, 0.15, 0.15] 37 | cartesian_goal_tolerance: [0.01, 0.01, 0.01, 0.05, 0.05, 0.05] 38 | force_tolerance: 5 39 | force_controlled_trajectories: true 40 | ur_tool_change_script: 'simpleMove.urp' 41 | execute_tool_change: false 42 | part_registration: 43 | simulation_pose: [-0.7, 0.0, 1.1, 0.0, 0.0, 0.0] # used for simulator only 44 | seed_pose: [0.0, 0.0, 0.5, 0.0, 0.0, 0.0] 45 | target_frame_id: "world" 46 | part_file: "" # relative to known data directory 47 | toolpath_file: "" # relative to known data directory 48 | toolpath_edge_buffer: 0.15 49 | part_rework: 50 | scan_poses: 51 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 52 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 53 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 54 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 55 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 56 | - pose: [0.016, 0.604, 1.700, 2.5848052, -0.6232238, 3.0450751] 57 | tool_frame: "camera_link_optical" 58 | pre_acquisition_pause: 2.0 # seconds 59 | skip_on_failure: true 60 | -------------------------------------------------------------------------------- /crs_application/config/main/motion_planning/MP_config.yaml: -------------------------------------------------------------------------------- 1 | general: 2 | use_trajopt_freespace: false 3 | use_trajopt_surface: true 4 | default_to_descartes: false 5 | default_to_ompl: true 6 | simplify_start_end_freespace: true 7 | manipulator: "manipulator" 8 | world_frame: "world" 9 | robot_base_frame: "base_link" 10 | tool0_frame: "tool0" 11 | required_tool_vel: true 12 | max_joint_vel: 0.3 13 | max_joint_vel_mult: 1.80 14 | max_surface_dist: 0.15 15 | max_rotation_rate: 3.0 16 | max_joint_acc: 0.8 17 | add_approach_and_retreat: true 18 | minimum_raster_length: 4 19 | reachable_radius: 1.35 20 | trajopt_verbose_output: false 21 | combine_strips: true 22 | global_descartes: true 23 | descartes: 24 | axial_step: 0.1 25 | collision_safety_margin: 0.00 26 | additional_tool_offset: [0.0, 0.0, 0.004, 0.0, 0.0, 0.0] 27 | joint_min_vals: [-0.1, -0.1, -0.4, -0.1, 0.1, -0.1] 28 | trajopt_surface: 29 | smooth_velocities: true 30 | smooth_accelerations: true 31 | smooth_jerks: false 32 | longest_valid_segment_fraction: 1.0 33 | longest_valid_segment_length: 1.0 34 | surface_coefficients: [10.0, 10.0, 10.0, 10.0, 10.0, 0.0] 35 | waypoints_critical: true 36 | collision_cost: 37 | enabled: true 38 | buffer_margin: 0.03 39 | special_collision_costs: 40 | - link1: "robot_stand" 41 | link2: "upper_arm_link" 42 | distance: 0.03 43 | cost: 10.0 44 | - link1: "eoat_link" 45 | link2: "forearm_link" 46 | distance: 0.05 47 | cost: 10.0 48 | collision_constraint: 49 | enabled: false 50 | safety_margin: 0.000 51 | special_collision_constraints: 52 | - link1: "eoat_link" 53 | link2: "part_link" 54 | distance: -0.05 55 | cost: 10.0 56 | - link1: "sander_intermediate_link" 57 | link2: "part_link" 58 | distance: -0.05 59 | cost: 10.0 60 | ompl: 61 | collision_safety_margin: 0.01 62 | planning_time: 240.0 63 | simplify: true 64 | range: 0.15 65 | num_threads: 8 66 | max_solutions: 10 67 | default_n_output_states: 20 68 | longest_valid_segment_fraction: 0.05 69 | longest_valid_segment_length: 0.5 70 | retry_failure: true 71 | retry_distance: 0.002 72 | retry_at_zero: true 73 | trajopt_freespace: 74 | smooth_velocities: true 75 | smooth_accelerations: true 76 | smooth_jerks: true 77 | longest_valid_segment_fraction: 0.01 78 | longest_valid_segment_length: 0.5 79 | collision_cost: 80 | enabled: true 81 | buffer_margin: 0.02 82 | special_collision_costs: 83 | - link1: "eoat_link" 84 | link2: "part_link" 85 | distance: 0.05 86 | cost: 10.0 87 | - link1: "eoat_link" 88 | link2: "robot_frame" 89 | distance: 0.05 90 | cost: 10.0 91 | - link1: "eoat_link" 92 | link2: "forearm_link" 93 | distance: 0.05 94 | cost: 10.0 95 | - link1: "eoat_link" 96 | link2: "robot_stand" 97 | distance: 0.05 98 | cost: 10.0 99 | collision_constraint: 100 | enabled: true 101 | safety_margin: 0.005 102 | -------------------------------------------------------------------------------- /crs_application/config/main/part_localization.yaml: -------------------------------------------------------------------------------- 1 | general: 2 | mesh_num_samples: 100000 3 | leaf_size: 0.025 4 | part_leaf_size: 0.0175 5 | part_frame: "part" 6 | enable_debug_visualizations: True 7 | icp: 8 | max_correspondence_dist: 0.08 9 | max_iter: 15000 10 | transformation_eps: 1e-12 11 | rotation_eps: 1e-12 # transformation rotation epsilon in order for an optimization to be considered as having converged to the final solution (epsilon is the cos(angle) 12 | euclidean_fitness: 1e-12 13 | ransac_threshold: 0.05 14 | use_correspondences: False 15 | use_cog_alignment: False 16 | sac: 17 | normal_est_rad: 0.1 18 | feature_est_rad: 0.04 19 | max_iters: 10000 20 | num_samples: 3 21 | correspondence_rand: 5 22 | similarity_threshold: 0.1 23 | max_correspondence_dist: 0.02 24 | inlier_fraction: 0.25 25 | crop_boxes: 26 | box1: 27 | xyz: [-0.4, 0.0, 1.15] 28 | size: [2.0, 2.0, 0.8] 29 | reverse: True 30 | box2: 31 | xyz: [-0.4, 0.0, 0.4] 32 | size: [0.7, 3.0, 0.8] 33 | reverse: False # deletes what's inside the box 34 | box3: 35 | xyz: [0.25, -0.8, 1.3] 36 | size: [0.5, 0.5, 0.5] 37 | reverse: False # deletes what's inside the box 38 | -------------------------------------------------------------------------------- /crs_application/config/main/rework/region_crop.yaml: -------------------------------------------------------------------------------- 1 | scale_factor: 1.25 2 | plane_dist_threshold: 0.2 3 | heigth_limits_min: -0.1 4 | heigth_limits_max: 0.1 5 | dir_estimation_method: 1 # PLANE_NORMAL: 1, NORMAL_AVGR: 2, POSE_Z_AXIS: 3, USER_DEFINED: 4 6 | user_dir: [0.0, 0.0, 0.1] # Used when dir_estimation_method == 4 7 | view_point: [0.0, 0.0, 10.0] 8 | -------------------------------------------------------------------------------- /crs_application/config/main/rework/region_detection.yaml: -------------------------------------------------------------------------------- 1 | opencv: 2 | #methods: ["CLAHE", "EQUALIZE_HIST", "EQUALIZE_HIST_YUV", "HSV", ... 3 | # "INVERT", "GRAYSCALE", "THRESHOLD", "DILATION", "EROSION", "CANNY", "THINNING","RANGE"] 4 | methods: ["GRAYSCALE", "INVERT", "DILATION", "CANNY", "THINNING"] 5 | debug_mode_enable: true 6 | debug_window_name: "DEBUG_REGION_DETECTION" 7 | debug_wait_key: false 8 | hsv: 9 | h: [0, 180] 10 | s: [100, 255] 11 | v: [0, 80] 12 | threshold: 13 | type: 0 14 | value: 180 15 | canny: 16 | lower_threshold: 50 17 | upper_threshold: 210 18 | aperture_size: 1 19 | dilation: 20 | elem: 1 # 0: Rect, 1: Cross, 2: Ellipse 21 | kernel_size: 1 # 2n + 1 22 | erosion: 23 | elem: 1 # 0: Rect, 1: Cross, 2: Ellipse 24 | kernel_size: 1 # 2n + 1 25 | contour: 26 | method: 1 27 | mode: 1 # See cv::RetrievalModes 28 | range: 29 | low: 190 30 | high: 255 31 | clahe: 32 | clip_limit: 4.0 33 | tile_grid_size: [4, 4] 34 | pcl2d: 35 | downsampling_radius: 4.0 # pixel units 36 | split_dist: 6.0 # pixel units 37 | closed_curve_max_dist: 6.0 # pixel units 38 | simplification_min_points: 10 # applies simplification only if the closed curve has 10 points or more 39 | simplification_alpha: 24.0 # pixel units, used in concave hull step 40 | pcl: 41 | debug_mode_enable: false 42 | max_merge_dist: 0.03 43 | closed_curve_max_dist: 0.03 44 | simplification_min_dist: 0.01 45 | split_dist: 0.1 46 | min_num_points: 10 47 | stat_removal: 48 | enable: true 49 | kmeans: 100 50 | stddev: 3.0 51 | normal_est: 52 | downsampling_radius: 0.01 53 | search_radius: 0.02 54 | kdtree_epsilon: 0.001 55 | viewpoint_xyz: [0.0, 0.0, 100.0] 56 | 57 | -------------------------------------------------------------------------------- /crs_application/config/swri/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: false 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994] 8 | process_path: 9 | tool_speed: 0.1 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [px, py, pz, rx, ry, rz] relative to default tool 11 | retreat_dist: 0.03 12 | approach_dist: 0.03 13 | tool_frame: "sander_center_link" 14 | target_force: 50 15 | media_change: 16 | change_time: 300.0 # seconds 17 | change_pose: [-0.381, -0.202, 1.5, 0.0, -0.786, 1.57] # [px, py, pz, rx, ry, rz] in world coordinates 18 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 19 | joint_position: [] 20 | preview: 21 | time_scaling: 1.0 22 | scan_acquisition: 23 | scan_poses: 24 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 25 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 26 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 27 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 28 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 29 | - pose: [0.016, 0.604, 1.700, 2.5848052, -0.6232238, 3.0450751] 30 | tool_frame: "camera_link_optical" 31 | skip_on_failure: true 32 | process_execution: 33 | time_tolerance: 5.0 # seconds 34 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] # radians 35 | cartesian_path_tolerance: [0.015, 0.015, 0.025, 0.15, 0.15, 0.15] 36 | cartesian_goal_tolerance: [0.01, 0.01, 0.01, 0.05, 0.05, 0.05] 37 | force_tolerance: 5 38 | force_controlled_trajectories: true 39 | ur_tool_change_script: 'simpleMove.urp' 40 | execute_tool_change: false 41 | part_registration: 42 | simulation_pose: [-0.7, 0.0, 1.1, 0.0, 0.0, 0.0] # used for simulator only 43 | seed_pose: [0.0, 0.0, 0.5, 0.0, 0.0, 0.0] 44 | target_frame_id: "world" 45 | part_file: "" # relative to known data directory 46 | toolpath_file: "" # relative to known data directory 47 | toolpath_edge_buffer: 0.15 48 | part_rework: 49 | scan_poses: 50 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 51 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 52 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 53 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 54 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 55 | - pose: [0.016, 0.604, 1.700, 2.5848052, -0.6232238, 3.0450751] 56 | tool_frame: "camera_link_optical" 57 | pre_acquisition_pause: 2.0 # seconds 58 | skip_on_failure: true 59 | -------------------------------------------------------------------------------- /crs_application/config/swri/motion_planning/MP_config.yaml: -------------------------------------------------------------------------------- 1 | general: 2 | use_trajopt_freespace: false 3 | use_trajopt_surface: false 4 | default_to_descartes: true 5 | simplify_start_end_freespace: true 6 | manipulator: "manipulator" 7 | world_frame: "world" 8 | robot_base_frame: "base_link" 9 | tool0_frame: "tool0" 10 | required_tool_vel: true 11 | max_joint_vel: 0.3 12 | max_joint_vel_mult: 1.80 13 | max_surface_dist: 0.15 14 | max_rotation_rate: 3.0 15 | max_joint_acc: 0.8 16 | add_approach_and_retreat: true 17 | minimum_raster_length: 2 18 | reachable_radius: 2.5 19 | trajopt_verbose_output: false 20 | combine_strips: true 21 | global_descartes: true 22 | descartes: 23 | axial_step: 0.5 # Pi/36 24 | collision_safety_margin: 0.00 25 | additional_tool_offset: [0.0, 0.0, 0.005, 0.0, 0.0, 0.0] 26 | joint_min_vals: [-0.1, -0.1, -0.4, -0.1, 0.1, -0.1] 27 | trajopt_surface: 28 | smooth_velocities: false 29 | smooth_accelerations: false 30 | smooth_jerks: false 31 | longest_valid_segment_fraction: 1.0 32 | longest_valid_segment_length: 1.0 33 | surface_coefficients: [10.0, 10.0, 10.0, 10.0, 10.0, 0.0] 34 | waypoints_critical: true 35 | collision_cost: 36 | enabled: true 37 | buffer_margin: 0.02 38 | special_collision_costs: 39 | - link1: "eoat_link" 40 | link2: "part_link" 41 | distance: 0.03 42 | cost: 10.0 43 | - link1: "sander_intermediate_link" 44 | link2: "part_link" 45 | distance: 0.03 46 | cost: 10.0 47 | - link1: "robot_stand" 48 | link2: "upper_arm_link" 49 | distance: 0.03 50 | cost: 10.0 51 | collision_constraint: 52 | enabled: false 53 | safety_margin: 0.000 54 | special_collision_constraints: 55 | - link1: "eoat_link" 56 | link2: "part_link" 57 | distance: -0.05 58 | cost: 10.0 59 | - link1: "sander_intermediate_link" 60 | link2: "part_link" 61 | distance: -0.05 62 | cost: 10.0 63 | ompl: 64 | collision_safety_margin: 0.01 65 | planning_time: 120.0 66 | simplify: true 67 | range: 0.25 68 | num_threads: 4 69 | max_solutions: 10 70 | default_n_output_states: 20 71 | longest_valid_segment_fraction: 0.01 72 | longest_valid_segment_length: 0.5 73 | retry_failure: true 74 | retry_distance: 0.002 75 | retry_at_zero: true 76 | trajopt_freespace: 77 | smooth_velocities: true 78 | smooth_accelerations: true 79 | smooth_jerks: true 80 | longest_valid_segment_fraction: 0.01 81 | longest_valid_segment_length: 0.5 82 | collision_cost: 83 | enabled: true 84 | buffer_margin: 0.02 85 | special_collision_costs: 86 | - link1: "eoat_link" 87 | link2: "part_link" 88 | distance: 0.02 89 | cost: 10.0 90 | - link1: "eoat_link" 91 | link2: "robot_frame" 92 | distance: 0.05 93 | cost: 10.0 94 | - link1: "eoat_link" 95 | link2: "forearm_link" 96 | distance: 0.05 97 | cost: 10.0 98 | - link1: "eoat_link" 99 | link2: "robot_stand" 100 | distance: 0.05 101 | cost: 10.0 102 | collision_constraint: 103 | enabled: true 104 | safety_margin: 0.005 105 | -------------------------------------------------------------------------------- /crs_application/config/swri/part_localization.yaml: -------------------------------------------------------------------------------- 1 | general: 2 | mesh_num_samples: 100000 3 | leaf_size: 0.02 4 | part_leaf_size: 0.0175 5 | part_frame: "part" 6 | enable_debug_visualizations: True 7 | icp: 8 | max_correspondence_dist: 0.3 9 | max_iter: 15000 10 | transformation_eps: 1e-12 11 | rotation_eps: 1e-12 # transformation rotation epsilon in order for an optimization to be considered as having converged to the final solution (epsilon is the cos(angle) 12 | euclidean_fitness: 1e-12 13 | ransac_threshold: 0.05 14 | use_correspondences: True 15 | use_cog_alignment: False 16 | sac: 17 | normal_est_rad: 0.1 18 | feature_est_rad: 0.04 19 | max_iters: 10000 20 | num_samples: 3 21 | correspondence_rand: 5 22 | similarity_threshold: 0.1 23 | max_correspondence_dist: 0.02 24 | inlier_fraction: 0.25 25 | crop_boxes: 26 | box1: 27 | xyz: [0.4, -1.1, -0.1] 28 | size: [2.0, 0.8, 0.5] 29 | reverse: True 30 | box2: 31 | xyz: [0.0, -0.875, -0.42] 32 | size: [2.5, 3.0, 0.2] 33 | reverse: False # deletes what's inside the box 34 | -------------------------------------------------------------------------------- /crs_application/config/swri/rework/region_crop.yaml: -------------------------------------------------------------------------------- 1 | scale_factor: 1.2 2 | plane_dist_threshold: 0.2 3 | heigth_limits_min: -0.1 4 | heigth_limits_max: 0.1 5 | dir_estimation_method: 1 # PLANE_NORMAL: 1, NORMAL_AVGR: 2, POSE_Z_AXIS: 3, USER_DEFINED: 4 6 | user_dir: [0.0, 0.0, 0.1] # Used when dir_estimation_method == 4 7 | view_point: [0.0, 0.0, 10.0] -------------------------------------------------------------------------------- /crs_application/config/swri/rework/region_detection.yaml: -------------------------------------------------------------------------------- 1 | opencv: 2 | #methods: ["CLAHE", "EQUALIZE_HIST", "EQUALIZE_HIST_YUV", "HSV", ... 3 | # "INVERT", "GRAYSCALE", "THRESHOLD", "DILATION", "EROSION", "CANNY", "THINNING","RANGE"] 4 | methods: [ "GRAYSCALE", "INVERT","THRESHOLD", "CANNY" , "DILATION", "THINNING"] 5 | debug_mode_enable: true 6 | debug_window_name: "DEBUG_REGION_DETECTION" 7 | debug_wait_key: false 8 | hsv: 9 | h: [0, 180] 10 | s: [0, 255] 11 | v: [0, 60] 12 | threshold: 13 | type: 2 14 | value: 200 15 | canny: 16 | lower_threshold: 40 17 | upper_threshold: 200 18 | aperture_size: 1 19 | dilation: 20 | elem: 0 # 0: Rect, 1: Cross, 2: Ellipse 21 | kernel_size: 2 # 2n + 1 22 | erosion: 23 | elem: 2 # 0: Rect, 1: Cross, 2: Ellipse 24 | kernel_size: 1 # 2n + 1 25 | contour: 26 | method: 1 27 | mode: 0 # See cv::RetrievalModes 28 | range: 29 | low: 190 30 | high: 255 31 | clahe: 32 | clip_limit: 4.0 33 | tile_grid_size: [4, 4] 34 | pcl2d: 35 | downsampling_radius: 4.0 # pixel units 36 | split_dist: 6.0 # pixel units 37 | closed_curve_max_dist: 6.0 # pixel units 38 | simplification_min_points: 10 # applies simplification only if the closed curve has 10 points or more 39 | simplification_alpha: 24.0 # pixel units, used in concave hull step 40 | pcl: 41 | debug_mode_enable: false 42 | max_merge_dist: 0.01 43 | closed_curve_max_dist: 0.01 44 | simplification_min_dist: 0.01 45 | split_dist: 0.1 46 | min_num_points: 10 47 | stat_removal: 48 | enable: true 49 | kmeans: 100 50 | stddev: 3.0 51 | normal_est: 52 | downsampling_radius: 0.01 53 | search_radius: 0.02 54 | kdtree_epsilon: 0.001 55 | viewpoint_xyz: [0.0, 0.0, 100.0] 56 | 57 | -------------------------------------------------------------------------------- /crs_application/include/crs_application/common/datatypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file datatypes.h 4 | * @date Jan 16, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #ifndef INCLUDE_CRS_APPLICATION_COMMON_DATATYPES_H_ 37 | #define INCLUDE_CRS_APPLICATION_COMMON_DATATYPES_H_ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace crs_application 47 | { 48 | namespace datatypes 49 | { 50 | struct ScanAcquisitionResult 51 | { 52 | std::vector point_clouds; 53 | std::vector transforms; 54 | }; 55 | 56 | struct ProcessToolpathData 57 | { 58 | std::vector rasters; 59 | }; 60 | 61 | struct MediaChangeMotionPlan 62 | { 63 | trajectory_msgs::msg::JointTrajectory start_traj; 64 | trajectory_msgs::msg::JointTrajectory return_traj; 65 | }; 66 | 67 | enum class ProcessExecActions : int 68 | { 69 | EXEC_PROCESS = 1, 70 | EXEC_MEDIA_CHANGE, 71 | DONE 72 | }; 73 | 74 | struct ProcessExecutionData 75 | { 76 | trajectory_msgs::msg::JointTrajectory move_to_start; 77 | std::vector process_plans; 78 | std::vector media_change_plans; 79 | }; 80 | 81 | struct PartInspectionResult 82 | { 83 | }; 84 | 85 | } // namespace datatypes 86 | } // namespace crs_application 87 | 88 | #endif /* INCLUDE_CRS_APPLICATION_COMMON_DATATYPES_H_ */ 89 | -------------------------------------------------------------------------------- /crs_application/include/crs_application/common/simulation_object_spawner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file simulation_object_spawner.h 4 | * @date May 4, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #ifndef INCLUDE_CRS_APPLICATION_COMMON_SIMULATION_OBJECT_SPAWNER_H_ 37 | #define INCLUDE_CRS_APPLICATION_COMMON_SIMULATION_OBJECT_SPAWNER_H_ 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | namespace crs_application 45 | { 46 | namespace common 47 | { 48 | class SimulationObjectSpawner 49 | { 50 | public: 51 | SimulationObjectSpawner(rclcpp::Node::SharedPtr node); 52 | virtual ~SimulationObjectSpawner(); 53 | 54 | bool spawn(const std::string& obj_name, 55 | const std::string& reference_frame_id, 56 | const std::string& mesh_path, 57 | const std::array& pose); 58 | bool remove(const std::string& obj_name); 59 | 60 | protected: 61 | rclcpp::Client::SharedPtr spawn_client_; /** @brief used to spawn the object on gazebo 62 | when if in simulation mode*/ 63 | rclcpp::Client::SharedPtr delete_client_; 64 | rclcpp::Node::SharedPtr node_; 65 | }; 66 | 67 | } // namespace common 68 | } // namespace crs_application 69 | #endif /* INCLUDE_CRS_APPLICATION_COMMON_SIMULATION_OBJECT_SPAWNER_H_ */ 70 | -------------------------------------------------------------------------------- /crs_application/launch/crs.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch.frontend import Parser 5 | from launch import LaunchService 6 | 7 | XML_FILE_NAME = 'crs.launch.xml' 8 | """ 9 | @summary: entry point for ros2 launch [pkg] [launch] 10 | @requires: ros-eloquent-launch-xml, install by running apt install ros-eloquent-launch-xml 11 | """ 12 | def generate_launch_description(): 13 | 14 | xml_file_path = str(os.path.join(get_package_share_directory('crs_application'), 'launch', XML_FILE_NAME)) 15 | print('Opening ROS2 launch file: %s'%(xml_file_path)) 16 | root_entity, parser = Parser.load(xml_file_path) 17 | ld = parser.parse_description(root_entity) 18 | return ld 19 | 20 | # main is not needed but it can be used to run as a standalone python program 21 | if __name__ == '__main__': 22 | ld = generate_launch_description() 23 | ls = LaunchService() 24 | ls.include_launch_description(ld) 25 | ls.run() 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /crs_application/launch/crs.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 53 | 54 | 55 | 56 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /crs_application/launch/load_preview_robot_model.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 12 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /crs_application/launch/load_robot_model.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /crs_application/launch/registration_startup.launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | import os 3 | import yaml 4 | from launch import LaunchDescription 5 | from launch.actions import OpaqueFunction 6 | from launch_ros.actions import Node 7 | from launch_ros.actions import ComposableNodeContainer 8 | from launch_ros.descriptions import ComposableNode 9 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 10 | 11 | GLOBAL_NS = '/crs' # WORKAROUND to failure in ComposableNodeContainer to used namespace pushed by calling launch file 12 | 13 | def load_yaml_file(yaml_file_path): 14 | try: 15 | with open(yaml_file_path, 'r') as file: 16 | return yaml.load(file) 17 | except EnvironmentError as e: # parent of IOError, OSError *and* WindowsError where available 18 | print(str(e)) 19 | return None 20 | 21 | def launch_setup(context, *args, **kwargs): 22 | 23 | ## getting path 24 | config_path = launch.substitutions.LaunchConfiguration('config_path').perform(context) 25 | 26 | # Part Localization Config 27 | localization_config= load_yaml_file(os.path.join(config_path, 'part_localization.yaml')) 28 | general_params = {'general' : localization_config['general']} 29 | icp_params = {'icp':localization_config['icp']} 30 | sac_params = {'sac':localization_config['sac']} 31 | crop_boxes_params = {'crop_boxes': localization_config['crop_boxes']} 32 | 33 | 34 | # ComposableNodeContainer not used because it fails to load parameters, using node instead 35 | ''' 36 | container = ComposableNodeContainer( 37 | node_name= 'perception', 38 | node_namespace= GLOBAL_NS, #launch.substitutions.LaunchConfiguration('global_ns'), 39 | package= 'rclcpp_components', 40 | node_executable = 'component_container', 41 | composable_node_descriptions = [ 42 | ComposableNode( 43 | package='crs_perception', 44 | node_plugin='crs_perception::LocalizeToPart', 45 | node_name='part_localization', 46 | node_namespace = GLOBAL_NS , #launch.substitutions.LaunchConfiguration('global_ns')), 47 | parameters =[icp_params, 48 | sac_params, 49 | crop_boxes_params 50 | ]) 51 | ], 52 | output = 'screen' 53 | ) 54 | ''' 55 | 56 | part_localization_node = Node( 57 | node_executable='localize_to_part', 58 | package='crs_perception', 59 | node_name='part_localization', 60 | node_namespace = GLOBAL_NS , 61 | output='screen', 62 | #prefix= 'xterm -e gdb -ex run --args', 63 | #prefix= 'xterm -e', 64 | parameters=[general_params, 65 | icp_params, 66 | sac_params, 67 | crop_boxes_params 68 | ]) 69 | 70 | return [part_localization_node] 71 | 72 | def generate_launch_description(): 73 | return launch.LaunchDescription([ 74 | launch.actions.DeclareLaunchArgument('config_path'), 75 | OpaqueFunction(function = launch_setup) 76 | ]) 77 | -------------------------------------------------------------------------------- /crs_application/launch/rework_startup.launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | import os 3 | import yaml 4 | from launch import LaunchDescription 5 | from launch.actions import OpaqueFunction 6 | from launch_ros.actions import Node 7 | from launch_ros.actions import ComposableNodeContainer 8 | from launch_ros.descriptions import ComposableNode 9 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 10 | 11 | GLOBAL_NS = '/crs' # WORKAROUND to failure in ComposableNodeContainer to used namespace pushed by calling launch file 12 | 13 | def load_yaml_file(yaml_file_path): 14 | try: 15 | with open(yaml_file_path, 'r') as file: 16 | return yaml.load(file) 17 | except EnvironmentError as e: # parent of IOError, OSError *and* WindowsError where available 18 | print(str(e)) 19 | return None 20 | 21 | def launch_setup(context, *args, **kwargs): 22 | 23 | ## getting path 24 | config_path = launch.substitutions.LaunchConfiguration('config_path').perform(context) 25 | 26 | # Load configuration files 27 | region_detection_cfg_file = os.path.join(config_path,'rework/region_detection.yaml') 28 | region_crop_cfg = load_yaml_file(os.path.join(config_path,'rework/region_crop.yaml')) 29 | 30 | # Nodes 31 | region_detector_server = Node( 32 | node_executable='region_detector_server', 33 | package='region_detection_rclcpp', 34 | node_name='region_detector_server', 35 | node_namespace = GLOBAL_NS , 36 | output='screen', 37 | #prefix= 'xterm -e gdb -ex run --args', 38 | prefix= 'xterm -e', 39 | parameters=[{'region_detection_cfg_file': region_detection_cfg_file}]) 40 | 41 | interactive_region_selection = Node( 42 | node_executable='interactive_region_selection', 43 | package='region_detection_rclcpp', 44 | node_name='interactive_region_selection', 45 | node_namespace = GLOBAL_NS , 46 | output='screen', 47 | #prefix= 'xterm -e gdb -ex run --args', 48 | prefix= 'xterm -e', 49 | parameters=[{'region_height': 0.05} 50 | ]) 51 | 52 | crop_data_server = Node( 53 | node_executable='crop_data_server', 54 | package='region_detection_rclcpp', 55 | node_name='crop_data_server', 56 | node_namespace = GLOBAL_NS , 57 | output='screen', 58 | #prefix= 'xterm -e gdb -ex run --args', 59 | prefix= 'xterm -e', 60 | parameters=[{'region_crop': region_crop_cfg} 61 | ], 62 | remappings = [('crop_data','crop_toolpaths')] 63 | ) 64 | 65 | return [region_detector_server, 66 | interactive_region_selection, 67 | crop_data_server] 68 | 69 | def generate_launch_description(): 70 | return launch.LaunchDescription([ 71 | launch.actions.DeclareLaunchArgument('config_path'), 72 | OpaqueFunction(function = launch_setup) 73 | ]) 74 | -------------------------------------------------------------------------------- /crs_application/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_application 5 | 0.1.0 6 | The central application for the Collaborative Robotic Sanding project 7 | ros-industrial 8 | BSD 9 | 10 | ament_cmake 11 | 12 | scxml_core 13 | rclcpp 14 | rclcpp_components 15 | crs_msgs 16 | tf2_eigen 17 | rclcpp_action 18 | control_msgs 19 | yaml_cpp_vendor 20 | crs_motion_planning 21 | gazebo_msgs 22 | region_detection_msgs 23 | 24 | launch_xml 25 | 26 | ament_lint_auto 27 | ament_lint_common 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /crs_area_selection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.0) 2 | project(crs_area_selection VERSION 0.1.0 LANGUAGES CXX) 3 | 4 | # Find all dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(ament_index_cpp REQUIRED) 7 | find_package(rclcpp REQUIRED) 8 | find_package(tf2_ros REQUIRED) 9 | find_package(tf2_eigen REQUIRED) 10 | 11 | find_package(Eigen3 REQUIRED) 12 | find_package(PCL 1.8 REQUIRED COMPONENTS sample_consensus segmentation surface) 13 | 14 | find_package(geometry_msgs REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(shape_msgs REQUIRED) 17 | find_package(std_srvs REQUIRED) 18 | find_package(visualization_msgs REQUIRED) 19 | find_package(crs_msgs REQUIRED) 20 | 21 | # Targets 22 | # Library containing tools for specifying regions and determining inlying points 23 | add_library(${PROJECT_NAME}_area_selection SHARED src/area_selector.cpp src/selection_artist.cpp) 24 | set_target_properties(${PROJECT_NAME}_area_selection PROPERTIES CXX_STANDARD 14) 25 | target_include_directories(${PROJECT_NAME}_area_selection PUBLIC 26 | "$" 27 | "$") 28 | target_include_directories(${PROJECT_NAME}_area_selection PUBLIC ${PCL_INCLUDE_DIRS}) 29 | target_link_libraries (${PROJECT_NAME}_area_selection ${PCL_LIBRARIES}) 30 | foreach(DEF ${PCL_DEFINITIONS}) 31 | string(STRIP ${DEF} DEF) 32 | if (NOT "${DEF}" STREQUAL "") 33 | string(SUBSTRING "${DEF}" 0 2 DEF_PREFIX) 34 | if ("${DEF_PREFIX}" STREQUAL "-m") 35 | string(REPLACE " " ";" DEF ${DEF}) 36 | foreach(OPTION_DEF ${DEF}) 37 | target_compile_options(${PROJECT_NAME}_area_selection PUBLIC ${OPTION_DEF}) 38 | endforeach() 39 | else() 40 | target_compile_definitions(${PROJECT_NAME}_area_selection PUBLIC ${DEF}) 41 | endif() 42 | endif() 43 | endforeach() 44 | ament_target_dependencies(${PROJECT_NAME}_area_selection 45 | ament_index_cpp 46 | rclcpp 47 | crs_msgs 48 | geometry_msgs sensor_msgs shape_msgs std_msgs std_srvs visualization_msgs tf2_ros tf2_eigen) 49 | 50 | 51 | 52 | # Node that runs area selection service 53 | add_executable(${PROJECT_NAME}_area_selection_node src/area_selection_node.cpp) 54 | target_link_libraries(${PROJECT_NAME}_area_selection_node ${PROJECT_NAME}_area_selection) 55 | set_target_properties(${PROJECT_NAME}_area_selection_node PROPERTIES CXX_STANDARD 14) 56 | target_include_directories(${PROJECT_NAME}_area_selection_node PUBLIC 57 | "$" 58 | "$") 59 | 60 | install( 61 | TARGETS ${PROJECT_NAME}_area_selection ${PROJECT_NAME}_area_selection_node 62 | EXPORT ${PACKAGE_NAME}-targets 63 | LIBRARY DESTINATION lib 64 | ARCHIVE DESTINATION lib 65 | RUNTIME DESTINATION lib/${PROJECT_NAME} 66 | INCLUDES DESTINATION include 67 | ) 68 | 69 | ament_export_interfaces(${PACKAGE_NAME}-targets) 70 | 71 | install(DIRECTORY include/ 72 | DESTINATION include 73 | FILES_MATCHING PATTERN "*.h" 74 | ) 75 | 76 | 77 | if(BUILD_TESTING) 78 | find_package(ament_lint_auto REQUIRED) 79 | # the following line skips the linter which checks for copyrights 80 | # uncomment the line when a copyright and license is not present in all source files 81 | #set(ament_cmake_copyright_FOUND TRUE) 82 | # the following line skips cpplint (only works in a git repo) 83 | # uncomment the line when this package is not in a git repo 84 | #set(ament_cmake_cpplint_FOUND TRUE) 85 | ament_lint_auto_find_test_dependencies() 86 | endif() 87 | 88 | 89 | # Setup Ament 90 | ament_export_dependencies( 91 | ament_cmake 92 | ament_index_cpp rclcpp crs_msgs geometry_msgs sensor_msgs shape_msgs std_msgs std_srvs visualization_msgs PCL tf2_ros tf2_eigen) 93 | 94 | ament_package() 95 | -------------------------------------------------------------------------------- /crs_area_selection/include/crs_area_selection/area_selector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_AREA_SELECTION_AREA_SELECTOR_H 18 | #define CRS_AREA_SELECTION_AREA_SELECTOR_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | //#include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "crs_area_selection/area_selector_parameters.h" 31 | 32 | namespace crs_area_selection 33 | { 34 | struct FittedPlane 35 | { 36 | Eigen::Vector3d normal = Eigen::Vector3d::UnitZ(); 37 | Eigen::Vector3d origin = Eigen::Vector3d::Zero(); 38 | }; 39 | 40 | /** 41 | * @brief The AreaSelector class takes a set of 3D points defining a closed polygon and attempts to identify which 42 | * points in a published point cloud lie within the bounds of that polygon. The class fits a plane to the input points, 43 | * projects the input points/polygon onto the fitted plane, extrudes a volume defined by the projected polygon to the 44 | * extents of the searched point cloud, and finds the points in the searched point cloud within the volume. If multiple 45 | * clusters of points are found, the cluster closest to the centroid of the input points is selected. 46 | */ 47 | class AreaSelector 48 | { 49 | public: 50 | /** 51 | * @brief AreaSelector class constructor 52 | */ 53 | AreaSelector() {} 54 | /** 55 | * @brief Finds the points that lie within the selection boundary 56 | * @param roi_cloud_msg 57 | * @return Returns false if there are less than 3 selection boundary points or if there are no search points; 58 | * otherwise returns true. 59 | */ 60 | std::vector getRegionOfInterest(const pcl::PointCloud::Ptr cloud, 61 | const std::vector& points, 62 | const AreaSelectorParameters& params); 63 | 64 | protected: 65 | boost::optional fitPlaneToPoints(const std::vector& points, 66 | const AreaSelectorParameters& params); 67 | 68 | // pcl::PointCloud::Ptr projectPointsOntoPlane(const std::vector& points, 69 | // const FittedPlane& plane); 70 | 71 | // std::vector getPointsInROI(const pcl::PointCloud::Ptr cloud, 72 | // const pcl::PointCloud::Ptr proj_sel_points, 73 | // const FittedPlane& plane, 74 | // const AreaSelectorParameters& params); 75 | }; 76 | 77 | } // namespace crs_area_selection 78 | 79 | #endif // OPP_AREA_SELECTION_AREA_SELECTOR_H 80 | -------------------------------------------------------------------------------- /crs_area_selection/include/crs_area_selection/area_selector_parameters.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_AREA_SELECTION_AREA_SELECTOR_PARAMETERS_H 18 | #define CRS_AREA_SELECTION_AREA_SELECTOR_PARAMETERS_H 19 | 20 | #include 21 | 22 | namespace crs_area_selection 23 | { 24 | struct AreaSelectorParameters 25 | { 26 | double cluster_tolerance = 0.25; 27 | int min_cluster_size = 100; 28 | int max_cluster_size = std::numeric_limits::max(); 29 | double plane_distance_threshold = 1.0; 30 | 31 | double normal_est_radius = 0.17; 32 | int region_growing_nneighbors = 10; 33 | double region_growing_smoothness = 5.0; 34 | double region_growing_curvature = 1.0; 35 | }; 36 | 37 | } // namespace crs_area_selection 38 | 39 | #endif // OPP_AREA_SELECTION_AREA_SELECTOR_PARAMETERS_H 40 | -------------------------------------------------------------------------------- /crs_area_selection/include/crs_area_selection/filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_AREA_SELECTION_FILTER_H 18 | #define CRS_AREA_SELECTION_FILTER_H 19 | 20 | #include 21 | #include 22 | 23 | namespace crs_area_selection 24 | { 25 | namespace data_filtering 26 | { 27 | template 28 | using Cloud = typename pcl::PointCloud; 29 | 30 | template 31 | using CloudPtr = typename Cloud::Ptr; 32 | 33 | template 34 | bool planeFit(const CloudPtr input_cloud, 35 | Cloud& output_cloud, 36 | pcl::ModelCoefficients::Ptr plane_coefficients, 37 | const double threshold = 0.025); 38 | 39 | } // end namespace data_filtering 40 | 41 | } // end namespace crs_area_selection 42 | 43 | #include "crs_area_selection/filter_impl.h" 44 | 45 | #endif // OPP_AREA_SELECTION_FILTER_H 46 | -------------------------------------------------------------------------------- /crs_area_selection/include/crs_area_selection/filter_impl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_AREA_SELECTION_FILTER_IMPL_H 18 | #define CRS_AREA_SELECTION_FILTER_IMPL_H 19 | 20 | #include "crs_area_selection/filter.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | namespace crs_area_selection 30 | { 31 | namespace data_filtering 32 | { 33 | template 34 | bool planeFit(const CloudPtr input_cloud, 35 | Cloud& output_cloud, 36 | pcl::ModelCoefficients::Ptr plane_coefficients, 37 | const double threshold) 38 | { 39 | pcl::ExtractIndices extract; 40 | pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices); 41 | pcl::SACSegmentation seg; 42 | 43 | // Set segmentation parameters 44 | seg.setOptimizeCoefficients(true); 45 | seg.setModelType(pcl::SACMODEL_PLANE); 46 | seg.setMethodType(pcl::SAC_RANSAC); 47 | seg.setDistanceThreshold(threshold); 48 | seg.setInputCloud(input_cloud); 49 | 50 | // Extract best fit cylinder inliers and calculate cylinder coefficients 51 | seg.segment(*plane_inliers, *plane_coefficients); 52 | 53 | if (plane_inliers->indices.size() == 0) 54 | { 55 | // ROS_ERROR("Unable to fit a plane to the data"); 56 | return false; 57 | } 58 | // ROS_DEBUG_NAMED("[a5::data_filtering]", 59 | // "Plane fit: %lu input points, %lu output points", 60 | // input_cloud->points.size(), 61 | // plane_inliers->indices.size()); 62 | 63 | // Create new point cloud from 64 | extract.setInputCloud(input_cloud); 65 | extract.setIndices(plane_inliers); 66 | extract.setNegative(false); 67 | extract.filter(output_cloud); 68 | 69 | return true; 70 | } 71 | 72 | } // end namespace data_filtering 73 | 74 | } // end namespace crs_area_selection 75 | 76 | #endif // crs_area_selection_FILTER_IMPL_H 77 | -------------------------------------------------------------------------------- /crs_area_selection/include/crs_area_selection/selection_artist.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_AREA_SELECTION_SELECTION_AREA_ARTIST_H 18 | #define CRS_AREA_SELECTION_SELECTION_AREA_ARTIST_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace crs_area_selection 34 | { 35 | /** 36 | * @brief The SelectionArtist class uses the Publish Points plugin in Rviz to allow the user to drop points on a mesh or 37 | * geometry primitive, creating a closed polygon region-of-interest (ROI). The ROI is displayed using an Interactive 38 | * Marker, which enables the user to reset the ROI or display the points within the selection polygon by means of a 39 | * drop-down menu. This class also contains a ROS service server to output the last selection points. 40 | */ 41 | class SelectionArtist 42 | { 43 | public: 44 | /** 45 | * @brief SelectionArtist is the class constructor which initializes ROS communication objects and private 46 | * variables.The 'world_frame' argument is the highest-level fixed frame (i.e. "map", "odom", or "world"). The 47 | * "sensor_frame" argument is the aggregated data frame (typically the base frame of the kinematic chain, i.e 48 | * rail_base_link or robot_base_link) 49 | * @param nh 50 | * @param world_frame 51 | * @param sensor_frame 52 | */ 53 | SelectionArtist(rclcpp::Node::SharedPtr node, const std::string& world_frame, const std::string& sensor_frame); 54 | 55 | void clearROIPointsCb(const std_srvs::srv::Trigger::Request::SharedPtr req, 56 | std_srvs::srv::Trigger::Response::SharedPtr res); 57 | 58 | bool collectROIMesh(const shape_msgs::msg::Mesh& mesh_msg, shape_msgs::msg::Mesh& submesh_msg, std::string& message); 59 | 60 | protected: 61 | /** @brief Unimplemented */ 62 | void getSensorData(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); 63 | 64 | void addSelectionPoint(const geometry_msgs::msg::PointStamped::ConstSharedPtr pt_stamped); 65 | 66 | bool transformPoint(const geometry_msgs::msg::PointStamped::ConstSharedPtr pt_stamped, 67 | geometry_msgs::msg::Point& transformed_pt); 68 | 69 | void collectROIPointsCb(crs_msgs::srv::GetROISelection::Request::SharedPtr req, 70 | crs_msgs::srv::GetROISelection::Response::SharedPtr res); 71 | 72 | void filterMesh(const pcl::PolygonMesh& input_mesh, 73 | const std::vector& inlying_indices, 74 | pcl::PolygonMesh& output_mesh); 75 | 76 | rclcpp::Node::SharedPtr node_; 77 | 78 | std::string world_frame_; 79 | 80 | std::string sensor_frame_; 81 | 82 | rclcpp::Subscription::SharedPtr drawn_points_sub_; 83 | 84 | rclcpp::Publisher::SharedPtr marker_pub_; 85 | 86 | rclcpp::Service::SharedPtr clear_roi_points_srv_; 87 | 88 | rclcpp::Service::SharedPtr collect_roi_points_srv_; 89 | 90 | rclcpp::Clock::SharedPtr clock_; 91 | tf2_ros::Buffer tf_buffer_; 92 | tf2_ros::TransformListener tf_listener_; 93 | 94 | visualization_msgs::msg::MarkerArray marker_array_; 95 | }; 96 | 97 | } // namespace crs_area_selection 98 | 99 | #endif // OPP_AREA_SELECTION_SELECTION_AREA_ARTIST_H 100 | -------------------------------------------------------------------------------- /crs_area_selection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | crs_area_selection 3 | 0.1.0 4 | 5 | 6 | The crs_area_selection package contains a number of tools to aid 7 | selection of submeshes in RViz using the publish point tool 8 | 9 | 10 | David Merz, Jr. 11 | David Merz, Jr. 12 | Apache 2.0 13 | 14 | ament_cmake 15 | 16 | geometry_msgs 17 | sensor_msgs 18 | shape_msgs 19 | std_srvs 20 | visualization_msgs 21 | 22 | libpcl-all 23 | rclcpp 24 | tf2_eigen 25 | tf2_ros 26 | 27 | crs_msgs 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /crs_area_selection/src/area_selection_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include "crs_area_selection/area_selector.h" 20 | #include "crs_area_selection/selection_artist.h" 21 | 22 | int main(int argc, char** argv) 23 | { 24 | // Set up ROS. 25 | rclcpp::init(argc, argv); 26 | auto node = std::make_shared("roi_selection_node"); 27 | 28 | std::string world_frame = "map"; 29 | std::string sensor_data_frame = "map"; 30 | RCLCPP_WARN(node->get_logger(), "World frame and sensor_data_frame are not being set from parameters"); 31 | 32 | // Set up the selection artist 33 | crs_area_selection::SelectionArtist artist(node, world_frame, sensor_data_frame); 34 | 35 | // This used an aysnc spinner for some reason. 36 | rclcpp::spin(node); 37 | rclcpp::shutdown(); 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /crs_gazebo_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crs_gazebo_plugins) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(rclcpp_action REQUIRED) 22 | find_package(rclcpp_components REQUIRED) 23 | find_package(control_msgs REQUIRED) 24 | find_package(gazebo_dev REQUIRED) 25 | find_package(gazebo_msgs REQUIRED) 26 | find_package(gazebo_ros REQUIRED) 27 | find_package(gazebo_plugins REQUIRED) 28 | 29 | include_directories(include 30 | ${gazebo_dev_INCLUDE_DIRS} 31 | ${gazebo_ros_INCLUDE_DIRS} 32 | ${gazebo_plugins_INCLUDE_DIRS} 33 | ${control_msgs_INCLUDE_DIRS} 34 | ) 35 | 36 | link_directories(${gazebo_dev_LIBRARY_DIRS}) 37 | 38 | # oint_pose_trajectory plugin 39 | add_library(${PROJECT_NAME}_joint_pose_trajectory SHARED 40 | src/joint_pose_trajectory.cpp 41 | ) 42 | ament_target_dependencies(${PROJECT_NAME}_joint_pose_trajectory 43 | "gazebo_dev" 44 | "gazebo_ros" 45 | "rclcpp" 46 | "control_msgs" 47 | "rclcpp_action" 48 | "rclcpp_components" 49 | ) 50 | ament_export_libraries(${PROJECT_NAME}_joint_pose_trajectory) 51 | 52 | if(BUILD_TESTING) 53 | find_package(ament_lint_auto REQUIRED) 54 | # the following line skips the linter which checks for copyrights 55 | # uncomment the line when a copyright and license is not present in all source files 56 | #set(ament_cmake_copyright_FOUND TRUE) 57 | # the following line skips cpplint (only works in a git repo) 58 | # uncomment the line when this package is not in a git repo 59 | #set(ament_cmake_cpplint_FOUND TRUE) 60 | ament_lint_auto_find_test_dependencies() 61 | endif() 62 | 63 | ament_package() 64 | 65 | install(DIRECTORY include/ 66 | DESTINATION include) 67 | 68 | install(TARGETS 69 | ${PROJECT_NAME}_joint_pose_trajectory 70 | ARCHIVE DESTINATION lib 71 | LIBRARY DESTINATION lib 72 | RUNTIME DESTINATION bin) 73 | 74 | -------------------------------------------------------------------------------- /crs_gazebo_plugins/include/crs_gazebo_plugins/joint_pose_trajectory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file joint_pose_trajectory.hpp 4 | * @date Feb 19, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #ifndef INCLUDE_CRS_GAZEBO_PLUGINS_JOINT_POSE_TRAJECTORY_HPP_ 37 | #define INCLUDE_CRS_GAZEBO_PLUGINS_JOINT_POSE_TRAJECTORY_HPP_ 38 | 39 | #include 40 | #include 41 | 42 | namespace crs_gazebo_plugins 43 | { 44 | class JointPoseTrajectoryPrivate; 45 | 46 | /// Set the trajectory of points to be followed by joints in simulation. 47 | /// Currently only positions specified in the trajectory_msgs are handled. 48 | /** 49 | Example Usage: 50 | \code{.xml} 51 | 53 | 54 | 55 | 56 | 57 | /my_namespace 58 | 59 | 60 | set_joint_trajectory:=my_trajectory 61 | 62 | 63 | 64 | 65 | 2 66 | 67 | 68 | \endcode 69 | */ 70 | class JointPoseTrajectory : public gazebo::ModelPlugin 71 | { 72 | public: 73 | /// Constructor 74 | JointPoseTrajectory(); 75 | 76 | /// Destructor 77 | ~JointPoseTrajectory(); 78 | 79 | protected: 80 | // Documentation inherited 81 | void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override; 82 | 83 | private: 84 | /// Private data pointer 85 | std::unique_ptr impl_; 86 | }; 87 | } // namespace crs_gazebo_plugins 88 | 89 | #endif /* INCLUDE_CRS_GAZEBO_PLUGINS_JOINT_POSE_TRAJECTORY_HPP_ */ 90 | -------------------------------------------------------------------------------- /crs_gazebo_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_gazebo_plugins 5 | 0.1.0 6 | Plugins for gazebo 7 | jnicho 8 | BSD 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | control_msgs 16 | gazebo_dev 17 | gazebo_msgs 18 | gazebo_ros 19 | rclcpp 20 | rclcpp_action 21 | rclcpp_components 22 | gazebo_plugins 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /crs_gui/README.md: -------------------------------------------------------------------------------- 1 | # CRS_GUI 2 | This package contains a series of widgets that make up the system gui. Each widget has a demo node that can be used for testing. 3 | 4 | ## Setup 5 | This will likely change in the future, but current part selection relies on a specific directory structure. Set it up as follows 6 | 1) Create this directory `~/.local/share/offline_generated_paths` 7 | 2) Create a subdirectory per part. Example: `mkdir ~/.local/share/offline_generated_paths/part_1` 8 | 3) In each part subdirectory anything with a .yaml extension will be treated as a toolpath. The mesh should be .ply 9 | 10 | ## CRS Application widget 11 | Main application widget that contains the other widgets. Launch demo with 12 | ``` 13 | ros2 launch crs_gui crs_application_demo.launch.xml 14 | ``` 15 | 16 | ## Part Selection widget 17 | This widget allows the user to select a part for processing. Launch the demo with 18 | ``` 19 | ros2 run crs_gui crs_gui_part_selection_demo 20 | ``` 21 | 22 | ## Polygon Area Selection widget 23 | This widget allows the user to select a region on the mesh. Launch the demo with 24 | ``` 25 | ros2 launch crs_gui area_selection_demo.launch.xml 26 | ``` 27 | 28 | ## State Machine Interface Widget 29 | This widget allows the user to navigate around the state machine. Launch the demo with 30 | ``` 31 | ros2 launch crs_gui state_machine_interface_demo.launch.xml 32 | ``` 33 | -------------------------------------------------------------------------------- /crs_gui/include/crs_gui/application_panel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_GUI_APPLICATION_PANEL_H 18 | #define CRS_GUI_APPLICATION_PANEL_H 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace crs_gui 25 | { 26 | class CRSApplicationWidget; 27 | 28 | /** 29 | * @brief Simple RViz panel that wraps the application widget such that can easily 30 | * be used within the context of RViz 31 | */ 32 | class ApplicationPanel : public rviz_common::Panel 33 | { 34 | Q_OBJECT 35 | public: 36 | ApplicationPanel(QWidget* parent = nullptr); 37 | virtual ~ApplicationPanel(); 38 | 39 | virtual void onInitialize() override; 40 | 41 | private: 42 | rclcpp::Node::SharedPtr node_; 43 | rclcpp::executors::MultiThreadedExecutor executor_; 44 | 45 | std::shared_ptr application_widget_; 46 | }; 47 | } // namespace crs_gui 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /crs_gui/include/crs_gui/register_ros_msgs_for_qt.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_GUI_REGISTER_ROS_MSGS_FOR_QT_H 18 | #define CRS_GUI_REGISTER_ROS_MSGS_FOR_QT_H 19 | 20 | #include 21 | 22 | #include 23 | 24 | Q_DECLARE_METATYPE(shape_msgs::msg::Mesh); 25 | Q_DECLARE_METATYPE(shape_msgs::msg::Mesh::SharedPtr); 26 | Q_DECLARE_METATYPE(std::vector); 27 | Q_DECLARE_METATYPE(std::vector); 28 | 29 | #endif // OPP_GUI_REGISTER_ROS_MSGS_FOR_QT_H 30 | -------------------------------------------------------------------------------- /crs_gui/include/crs_gui/widgets/crs_application_widget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_GUI_WIDGETS_CRS_APPLICATION_WIDGET_H 18 | #define CRS_GUI_WIDGETS_CRS_APPLICATION_WIDGET_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace YAML 32 | { 33 | class Node; 34 | } 35 | 36 | namespace Ui 37 | { 38 | class CRSApplication; 39 | } 40 | 41 | namespace crs_gui 42 | { 43 | class PartSelectionWidget; 44 | class PolygonAreaSelectionWidget; 45 | class StateMachineInterfaceWidget; 46 | 47 | class CRSApplicationWidget : public QWidget 48 | { 49 | Q_OBJECT 50 | public: 51 | CRSApplicationWidget(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); 52 | ~CRSApplicationWidget(); 53 | 54 | std::vector getNodes(); 55 | protected slots: 56 | 57 | void onPartSelected(const QString selected_part, const QString part_mesh); 58 | 59 | void onPartPathSelected(const QString qselected_part, const QString qselected_path); 60 | 61 | bool loadConfig(const std::string config_file); 62 | bool updateConfig(); 63 | bool saveConfig(); 64 | 65 | protected: 66 | std::unique_ptr ui_; 67 | 68 | rclcpp::Node::SharedPtr node_; 69 | rclcpp::Node::SharedPtr support_widgets_node_; 70 | std::string database_directory_; 71 | 72 | /** @brief Service that provides process Config when called */ 73 | rclcpp::Service::SharedPtr get_configuration_srv_; 74 | 75 | /** @brief Callback for get_configuration_srv_*/ 76 | void getConfigurationCb(crs_msgs::srv::GetConfiguration::Request::SharedPtr req, 77 | crs_msgs::srv::GetConfiguration::Response::SharedPtr res); 78 | 79 | rclcpp::Publisher::SharedPtr mesh_marker_pub_; 80 | visualization_msgs::msg::MarkerArray current_mesh_marker_; 81 | rclcpp::TimerBase::SharedPtr mesh_marker_timer_; 82 | void meshMarkerTimerCb(); 83 | 84 | rclcpp::Publisher::SharedPtr toolpath_marker_pub_; 85 | visualization_msgs::msg::MarkerArray current_toolpath_marker_; 86 | rclcpp::TimerBase::SharedPtr toolpath_marker_timer_; 87 | void toolpathMarkerTimerCb(); 88 | 89 | std::string default_config_path_; 90 | std::string config_file_path_; 91 | std::string toolpath_file_; 92 | std::string cad_part_file_; 93 | std::shared_ptr config_yaml_node_; 94 | 95 | visualization_msgs::msg::MarkerArray delete_all_marker_; 96 | 97 | std::unique_ptr part_selector_widget_; 98 | std::unique_ptr area_selection_widget_; 99 | std::unique_ptr state_machine_interface_widget_; 100 | }; 101 | 102 | } // namespace crs_gui 103 | 104 | #endif // crs_GUI_WIDGETS_APPLICATION_WIDGET_BASE_H 105 | -------------------------------------------------------------------------------- /crs_gui/include/crs_gui/widgets/part_selection_widget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_GUI_WIDGETS_PART_SELECTION_WIDGET_H 18 | #define CRS_GUI_WIDGETS_PART_SELECTION_WIDGET_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | class QListWidgetItem; 26 | 27 | namespace Ui 28 | { 29 | class PartSelection; 30 | } 31 | 32 | namespace crs_gui 33 | { 34 | class PartSelectionWidget : public QWidget 35 | { 36 | Q_OBJECT 37 | public: 38 | PartSelectionWidget(QWidget* parent = nullptr, 39 | std::string database_directory = std::string(std::getenv("HOME")) + "/.local/share/" 40 | "offline_generated_paths"); 41 | ~PartSelectionWidget(); 42 | Q_SIGNALS: 43 | /** @brief Signal eitted when Load Selected Part is clicked with name of the part selected*/ 44 | void partSelected(QString, QString); 45 | /** @brief Signal emmited when Load Selected Part is clicked. First arg is part selected. Second arg is path to 46 | * toolpath yaml */ 47 | void partPathSelected(QString, QString); 48 | 49 | protected Q_SLOTS: 50 | 51 | /** @brief Checks for new parts and updates the list */ 52 | void refreshPartsList(); 53 | /** @brief Checks the toolpaths for the selected part and populates the display*/ 54 | void onPartSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous); 55 | /** @brief Gets part name of selection and triggers partSelected and partSelectedPath signals*/ 56 | void onPartSelected(); 57 | 58 | private: 59 | std::unique_ptr ui_; 60 | 61 | /** @brief Directory where the part directories are located */ 62 | std::string database_directory_; 63 | }; 64 | 65 | } // namespace crs_gui 66 | 67 | #endif // OPP_GUI_WIDGETS_TOOL_PATH_PLANNER_WIDGET_H 68 | -------------------------------------------------------------------------------- /crs_gui/include/crs_gui/widgets/polygon_area_selection_widget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_GUI_WIDGETS_POLYGON_AREA_SELECTION_WIDGET_H 18 | #define CRS_GUI_WIDGETS_POLYGON_AREA_SELECTION_WIDGET_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | #include "crs_gui/register_ros_msgs_for_qt.h" 27 | 28 | namespace Ui 29 | { 30 | class PolygonAreaSelectionWidget; 31 | } 32 | 33 | namespace crs_gui 34 | { 35 | class PolygonAreaSelectionWidget : public QWidget 36 | { 37 | Q_OBJECT 38 | 39 | public: 40 | explicit PolygonAreaSelectionWidget(rclcpp::Node::SharedPtr node, 41 | const std::string& selection_world_frame, 42 | const std::string& selection_sensor_frame, 43 | QWidget* parent = nullptr); 44 | ~PolygonAreaSelectionWidget(); 45 | 46 | public Q_SLOTS: 47 | 48 | void init(const shape_msgs::msg::Mesh& mesh); 49 | 50 | Q_SIGNALS: 51 | void selectedSubmesh(const shape_msgs::msg::Mesh::SharedPtr& selected_submesh); 52 | 53 | private Q_SLOTS: 54 | void clearROISelection(); 55 | 56 | void applySelection(); 57 | 58 | private: 59 | std::unique_ptr ui_; 60 | 61 | rclcpp::Node::SharedPtr node_; 62 | 63 | shape_msgs::msg::Mesh::SharedPtr mesh_; 64 | 65 | shape_msgs::msg::Mesh::SharedPtr submesh_; 66 | 67 | crs_area_selection::SelectionArtist selector_; 68 | }; // end class PolygonAreaSelectionWidget 69 | 70 | } // namespace crs_gui 71 | 72 | #endif // OPP_GUI_WIDGETS_POLYGON_AREA_SELECTION_WIDGET_H 73 | -------------------------------------------------------------------------------- /crs_gui/include/crs_gui/widgets/state_machine_interface_widget.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CRS_GUI_WIDGETS_STATE_MACHINE_INTERFACE_WIDGET_H 18 | #define CRS_GUI_WIDGETS_STATE_MACHINE_INTERFACE_WIDGET_H 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace Ui 30 | { 31 | class StateMachineInterface; 32 | } 33 | 34 | namespace crs_gui 35 | { 36 | class StateMachineInterfaceWidget : public QWidget 37 | { 38 | Q_OBJECT 39 | public: 40 | StateMachineInterfaceWidget(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); 41 | ~StateMachineInterfaceWidget(); 42 | 43 | void requestConfiguration(); 44 | 45 | Q_SIGNALS: 46 | /** @brief emitted inside currentStateCB when the state changes with the name of new state as the argument*/ 47 | void onStateChange(std::string); 48 | void show_msg(bool, std::string); 49 | 50 | protected Q_SLOTS: 51 | 52 | /** @brief Calls ROS service to check available sm actions*/ 53 | void onSMQuery(); 54 | /** @brief Applies selected sm action via ROS service*/ 55 | void onSMApply(); 56 | /** @brief Applies user_approves action via ROS service*/ 57 | void onSMApprove(); 58 | /** @brief Applies user_cancels action via ROS service*/ 59 | void onSMCancel(); 60 | 61 | public: 62 | /** @brief Current state machine state */ 63 | std::string current_state_; 64 | 65 | protected: 66 | std::unique_ptr ui_; 67 | 68 | /** @brief Node that is used for service calls and subscriber*/ 69 | rclcpp::Node::SharedPtr node_; 70 | /** @brief Subscriber to topic publishing the current sm state*/ 71 | rclcpp::Subscription::SharedPtr current_state_sub_; 72 | /** @brief Current state subscriber callback*/ 73 | void currentStateCB(const std_msgs::msg::String::ConstSharedPtr current_state); 74 | /** @brief ROS client to get available sm actions*/ 75 | rclcpp::callback_group::CallbackGroup::SharedPtr get_available_actions_callback_group_; 76 | rclcpp::Client::SharedPtr get_available_actions_client_; 77 | /** @brief ROS client to execute sm action*/ 78 | rclcpp::callback_group::CallbackGroup::SharedPtr execute_action_callback_group_; 79 | rclcpp::Client::SharedPtr execute_action_client_; 80 | }; 81 | 82 | } // namespace crs_gui 83 | 84 | #endif // crs_GUI_WIDGETS_APPLICATION_WIDGET_BASE_H 85 | -------------------------------------------------------------------------------- /crs_gui/launch/area_selection_demo.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /crs_gui/launch/crs_application_demo.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /crs_gui/launch/state_machine_interface_demo.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /crs_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | crs_gui 4 | 0.1.0 5 | 6 | description 7 | 8 | 9 | mripperger 10 | mripperger 11 | 12 | Apache 2.0 13 | 14 | ament_cmake 15 | 16 | crs_area_selection 17 | rclcpp 18 | shape_msgs 19 | std_srvs 20 | rviz2 21 | crs_msgs 22 | crs_motion_planning 23 | pluginlib 24 | xterm 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /crs_gui/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | A panel for running the CRS application 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /crs_gui/src/application_panel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace crs_gui 23 | { 24 | ApplicationPanel::ApplicationPanel(QWidget* parent) 25 | : rviz_common::Panel(parent), node_(new rclcpp::Node("application_panel_node")) 26 | { 27 | application_widget_.reset(new CRSApplicationWidget(node_, this)); 28 | auto nodes = application_widget_->getNodes(); 29 | for (auto& n : nodes) 30 | { 31 | executor_.add_node(n); 32 | } 33 | std::thread([this]() { executor_.spin(); }).detach(); 34 | } 35 | 36 | ApplicationPanel::~ApplicationPanel() { executor_.cancel(); } 37 | 38 | void ApplicationPanel::onInitialize() 39 | { 40 | QVBoxLayout* layout = new QVBoxLayout(); 41 | layout->addWidget(application_widget_.get()); 42 | 43 | setLayout(layout); 44 | } 45 | } // namespace crs_gui 46 | 47 | #include 48 | PLUGINLIB_EXPORT_CLASS(crs_gui::ApplicationPanel, rviz_common::Panel) 49 | -------------------------------------------------------------------------------- /crs_gui/src/nodes/area_selection_widget_demo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "crs_gui/widgets/polygon_area_selection_widget.h" 22 | 23 | int main(int argc, char** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | auto node = std::make_shared("area_selection_widget_demo_node"); 27 | 28 | // Create and start the Qt application 29 | QApplication app(argc, argv); 30 | 31 | auto widget = std::make_unique(node, "world", "world"); 32 | widget->show(); 33 | 34 | rclcpp::Rate throttle(100); 35 | while (rclcpp::ok()) 36 | { 37 | app.processEvents(QEventLoop::AllEvents); 38 | throttle.sleep(); 39 | rclcpp::spin_some(node); 40 | if (!widget->isVisible()) 41 | break; 42 | } 43 | app.exit(); 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /crs_gui/src/nodes/crs_application_demo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "crs_gui/widgets/crs_application_widget.h" 22 | 23 | int main(int argc, char** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | auto node = std::make_shared("crs_application_widget_node"); 27 | 28 | // Create and start the Qt application 29 | QApplication app(argc, argv); 30 | 31 | auto widget = std::make_unique(node); 32 | widget->show(); 33 | 34 | rclcpp::Rate throttle(100); 35 | while (rclcpp::ok()) 36 | { 37 | app.processEvents(QEventLoop::AllEvents); 38 | throttle.sleep(); 39 | rclcpp::spin_some(node); 40 | if (!widget->isVisible()) 41 | break; 42 | } 43 | app.exit(); 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /crs_gui/src/nodes/part_selection_widget_demo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "crs_gui/widgets/part_selection_widget.h" 22 | 23 | int main(int argc, char** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | auto node = std::make_shared("crs_part_selection_widget_node"); 27 | 28 | // Create and start the Qt application 29 | QApplication app(argc, argv); 30 | 31 | auto widget = std::make_unique(); 32 | widget->show(); 33 | 34 | rclcpp::Rate throttle(100); 35 | while (rclcpp::ok()) 36 | { 37 | app.processEvents(QEventLoop::AllEvents); 38 | throttle.sleep(); 39 | rclcpp::spin_some(node); 40 | if (!widget->isVisible()) 41 | break; 42 | } 43 | app.exit(); 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /crs_gui/src/nodes/state_machine_interface_widget_demo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "crs_gui/widgets/state_machine_interface_widget.h" 22 | 23 | int main(int argc, char** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | auto node = std::make_shared("state_machine_interface_widget_node"); 27 | 28 | // Create and start the Qt application 29 | QApplication app(argc, argv); 30 | 31 | auto widget = std::make_unique(node); 32 | widget->show(); 33 | 34 | rclcpp::Rate throttle(100); 35 | while (rclcpp::ok()) 36 | { 37 | app.processEvents(QEventLoop::AllEvents); 38 | throttle.sleep(); 39 | rclcpp::spin_some(node); 40 | if (!widget->isVisible()) 41 | break; 42 | } 43 | app.exit(); 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /crs_gui/src/widgets/polygon_area_selection_widget.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "crs_gui/widgets/polygon_area_selection_widget.h" 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | 26 | namespace crs_gui 27 | { 28 | PolygonAreaSelectionWidget::PolygonAreaSelectionWidget(rclcpp::Node::SharedPtr node, 29 | const std::string& selection_world_frame, 30 | const std::string& selection_sensor_frame, 31 | QWidget* parent) 32 | : QWidget(parent) 33 | , ui_(new Ui::PolygonAreaSelectionWidget) 34 | , node_(node) 35 | , selector_(node, selection_world_frame, selection_sensor_frame) 36 | { 37 | ui_->setupUi(this); 38 | connect( 39 | ui_->push_button_clear_selection, &QPushButton::clicked, this, &PolygonAreaSelectionWidget::clearROISelection); 40 | connect(ui_->push_button_apply_selection, &QPushButton::clicked, this, &PolygonAreaSelectionWidget::applySelection); 41 | } 42 | 43 | PolygonAreaSelectionWidget::~PolygonAreaSelectionWidget() = default; 44 | 45 | void PolygonAreaSelectionWidget::init(const shape_msgs::msg::Mesh& mesh) 46 | { 47 | mesh_.reset(new shape_msgs::msg::Mesh(mesh)); 48 | clearROISelection(); 49 | return; 50 | } 51 | 52 | void PolygonAreaSelectionWidget::clearROISelection() 53 | { 54 | auto req = std::make_shared(); 55 | auto res = std::make_shared(); 56 | // Currently, this callback is being called directly, so it will always return true. 57 | selector_.clearROIPointsCb(req, res); 58 | if (!res->success) 59 | { 60 | RCLCPP_ERROR_STREAM(node_->get_logger(), 61 | "Tool Path Parameter Editor Widget: Area Selection error:" << res->message); 62 | } 63 | // This will be uncommented when mesh selection is turned back on 64 | // submesh_.reset(new shape_msgs::msg::Mesh(*mesh_)); 65 | 66 | // emit(selectedSubmesh(submesh_)); 67 | return; 68 | } 69 | 70 | void PolygonAreaSelectionWidget::applySelection() 71 | { 72 | if (!mesh_) 73 | { 74 | // TODO: Currently this will always happen because we are not setting a mesh. Basically we are just drawing on the 75 | // surface an not doing anything 76 | QMessageBox::warning(this, "Tool Path Planning Error", "No mesh available to crop"); 77 | return; 78 | } 79 | submesh_.reset(new shape_msgs::msg::Mesh()); 80 | std::string error_message; 81 | bool success = selector_.collectROIMesh(*mesh_, *submesh_, error_message); 82 | if (!success) 83 | { 84 | RCLCPP_ERROR_STREAM( 85 | node_->get_logger(), 86 | "Tool Path Parameter Editor Widget: Area Selection error: could not compute submesh: " << error_message); 87 | } 88 | if (submesh_->vertices.size() < 3 || submesh_->triangles.size() < 1) 89 | { 90 | submesh_.reset(new shape_msgs::msg::Mesh(*mesh_)); 91 | } 92 | 93 | emit(selectedSubmesh(submesh_)); 94 | return; 95 | } 96 | 97 | } // namespace crs_gui 98 | -------------------------------------------------------------------------------- /crs_gui/ui/crs_application.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | CRSApplication 4 | 5 | 6 | 7 | 0 8 | 0 9 | 667 10 | 673 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | <html><head/><body><p align="center"><span style=" font-size:14pt; font-weight:600;">Collaborative Robotic Sanding</span></p></body></html> 21 | 22 | 23 | 24 | 25 | 26 | 27 | Qt::Horizontal 28 | 29 | 30 | 31 | 32 | 33 | 34 | true 35 | 36 | 37 | 0 38 | 39 | 40 | 41 | Main 42 | 43 | 44 | 45 | 46 | 47 | 1. Load Part 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | Qt::Horizontal 60 | 61 | 62 | 63 | 64 | 65 | 66 | Qt::Horizontal 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 2. Process Control 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | true 89 | 90 | 91 | Log 92 | 93 | 94 | 95 | 96 | 97 | false 98 | 99 | 100 | Log Messages 101 | 102 | 103 | 104 | 0 105 | 106 | 107 | 0 108 | 109 | 110 | 0 111 | 112 | 113 | 0 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | -------------------------------------------------------------------------------- /crs_gui/ui/part_selection.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | PartSelection 4 | 5 | 6 | 7 | 0 8 | 0 9 | 450 10 | 253 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Qt::Vertical 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | Load Selected Part 38 | 39 | 40 | 41 | 42 | 43 | 44 | Refresh Parts List 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /crs_gui/ui/polygon_area_selection_widget.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | PolygonAreaSelectionWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 409 10 | 114 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | Area Selection 21 | (Use Publish Point tool, selection is automatically applied) 22 | 23 | 24 | 25 | 26 | 27 | 28 | Clear Selection 29 | 30 | 31 | 32 | 33 | 34 | 35 | Apply Selection 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /crs_gui/ui/state_machine_interface.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | StateMachineInterface 4 | 5 | 6 | 7 | 0 8 | 0 9 | 602 10 | 92 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Cancel 26 | 27 | 28 | 29 | 30 | 31 | 32 | Approve 33 | 34 | 35 | 36 | 37 | 38 | 39 | State Machine Actions 40 | 41 | 42 | 43 | 44 | 45 | 46 | Current State 47 | 48 | 49 | 50 | 51 | 52 | 53 | true 54 | 55 | 56 | 57 | 58 | 59 | 60 | Apply 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /crs_hardware.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: ros2_intel_realsense, uri: 'https://github.com/intel/ros2_intel_realsense.git', version: 47b313c} 2 | -------------------------------------------------------------------------------- /crs_motion_planning/README.md: -------------------------------------------------------------------------------- 1 | # CRS Motion Planning Package 2 | 3 | ## Testing 4 | Launch main application launch file 5 | ``` 6 | ros2 launch crs_application crs.launch.xml 7 | ``` 8 | ### Freespace Planner 9 | #### Parameters 10 | 11 | - target_link - TF frame of interest that will be moving to the specified goal 12 | - start_position (optional) - specified start joint state, defaults to current state 13 | - goal_position (optional) - specified end joint state, if unused defaults to goal_pose 14 | - goal_pose (optional) - specified end cartesian position, required if goal_position unused 15 | - num_steps (optional) - number of timesteps in returned trajectory, defaults to 200 16 | - execute (optional) - if true the trajectory will be published in addition to being returned 17 | 18 | #### Outputs 19 | 20 | - output_trajectory - The determined trajectory to the goal position from the start position 21 | - success - Whether or not the planner succeeded 22 | - message - Information about the success or failure of the attempt 23 | 24 | #### Usage 25 | This service can be called from the command line, an example is presented below 26 | ``` 27 | ros2 service call /plan_freespace_motion crs_msgs/srv/CallFreespaceMotion "{execute: 1, target_link: 'camera_link_optical', goal_pose: {translation: {x: 0.125, y: 0.0, z: 1.6}, rotation: {w: 0.0, x: 1.0, y: 0.0, z: 0.0}}}" 28 | ``` 29 | ### Process Planner 30 | For simple testing run the process planner test node which can call the process planner when given a service trigger 31 | ``` 32 | ros2 run crs_motion_planning crs_motion_planning_process_planner_test 33 | ``` 34 | Then in another terminal call the service trigger 35 | ``` 36 | ros2 service call /test_process_planner std_srvs/srv/Trigger 37 | ``` 38 | Rviz will first populate with a marker array of blue arrows to show the original path. 39 | 40 | Once planning is complete and successful 2 new marker arrays will be publised. 41 | 42 | The first is an array of light blue spheres to denote the unreachable waypoints. 43 | 44 | The second includes 3 different colored arrows: 45 | - Green: Successfully planned rasters 46 | 47 | - Yellow: Skipped rasters due to raster length being below specified threshold 48 | 49 | - Red: Rasters that failed to pass through trajopt surface planner 50 | -------------------------------------------------------------------------------- /crs_motion_planning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_motion_planning 5 | 0.0.0 6 | Motion Planning nodes and library for the CRS project 7 | ros-industrial 8 | BSD 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rosidl_default_generators 16 | actionlib_msgs 17 | rclcpp_action 18 | geometry_msgs 19 | trajectory_msgs 20 | sensor_msgs 21 | std_srvs 22 | std_msgs 23 | crs_msgs 24 | control_msgs 25 | cartesian_trajectory_msgs 26 | 27 | tesseract_msgs 28 | tesseract 29 | tesseract_monitoring 30 | tesseract_rosutils 31 | tesseract_kinematics 32 | tesseract_visualization 33 | 34 | ur_ikfast_kinematics 35 | 36 | trajopt 37 | trajopt_utils 38 | trajopt_sco 39 | 40 | descartes_light 41 | descartes_ikfast 42 | descartes_samplers 43 | 44 | tf2 45 | 46 | iterative_spline_parameterization 47 | 48 | yaml_cpp_vendor 49 | action_msgs 50 | visualization_msgs 51 | 52 | 53 | ament_cmake 54 | 55 | 56 | -------------------------------------------------------------------------------- /crs_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crs_msgs) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | set(SRV_DEPS 19 | sensor_msgs 20 | std_msgs 21 | geometry_msgs 22 | trajectory_msgs 23 | cartesian_trajectory_msgs 24 | ) 25 | 26 | # find dependencies 27 | find_package(ament_cmake REQUIRED) 28 | find_package(rosidl_default_generators REQUIRED) 29 | find_package(builtin_interfaces REQUIRED) 30 | find_package(sensor_msgs REQUIRED) 31 | find_package(std_msgs REQUIRED) 32 | find_package(geometry_msgs REQUIRED) 33 | find_package(trajectory_msgs REQUIRED) 34 | find_package(cartesian_trajectory_msgs REQUIRED) 35 | 36 | set(MSG_FILES 37 | msg/ProcessConfiguration.msg 38 | msg/ProcessMotionPlan.msg 39 | msg/ToolProcessPath.msg 40 | ) 41 | 42 | set(SRV_FILES 43 | srv/CallFreespaceMotion.srv 44 | srv/GetROISelection.srv 45 | srv/LoadPart.srv 46 | srv/LocalizeToPart.srv 47 | srv/ExecuteAction.srv 48 | srv/GetAvailableActions.srv 49 | srv/PlanProcessMotions.srv 50 | srv/GetConfiguration.srv 51 | srv/RobotPositioner.srv 52 | srv/CropToolpaths.srv 53 | srv/RunRobotScript.srv 54 | ) 55 | 56 | rosidl_generate_interfaces(${PROJECT_NAME} 57 | ${MSG_FILES} 58 | ${SRV_FILES} 59 | DEPENDENCIES ${SRV_DEPS} 60 | ) 61 | 62 | if(BUILD_TESTING) 63 | find_package(ament_lint_auto REQUIRED) 64 | # the following line skips the linter which checks for copyrights 65 | # uncomment the line when a copyright and license is not present in all source files 66 | #set(ament_cmake_copyright_FOUND TRUE) 67 | # the following line skips cpplint (only works in a git repo) 68 | # uncomment the line when this package is not in a git repo 69 | #set(ament_cmake_cpplint_FOUND TRUE) 70 | ament_lint_auto_find_test_dependencies() 71 | endif() 72 | 73 | ament_export_dependencies(rosidl_default_runtime) 74 | ament_package() 75 | -------------------------------------------------------------------------------- /crs_msgs/msg/ProcessConfiguration.msg: -------------------------------------------------------------------------------- 1 | string yaml_config # Configuration for the various managers in a yaml formatter string 2 | -------------------------------------------------------------------------------- /crs_msgs/msg/ProcessMotionPlan.msg: -------------------------------------------------------------------------------- 1 | trajectory_msgs/JointTrajectory start # Move prior to starting the process (empty when it does not apply) 2 | trajectory_msgs/JointTrajectory[] process_motions # The process moves (shall include approach and retreat moves) 3 | cartesian_trajectory_msgs/CartesianTrajectory[] force_controlled_process_motions # Process motions as a cartesian trajectory 4 | trajectory_msgs/JointTrajectory[] free_motions 5 | trajectory_msgs/JointTrajectory end # Move after completing the process (empty when it does not apply) 6 | -------------------------------------------------------------------------------- /crs_msgs/msg/ToolProcessPath.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseArray[] rasters # Array of array of poses -------------------------------------------------------------------------------- /crs_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_msgs 5 | 0.0.0 6 | The ros messages and service definitions for the CRS project 7 | ros-industrial 8 | BSD 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | 13 | builtin_interfaces 14 | sensor_msgs 15 | std_msgs 16 | geometry_msgs 17 | trajectory_msgs 18 | cartesian_trajectory_msgs 19 | 20 | rosidl_default_runtime 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | rosidl_interface_packages 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /crs_msgs/srv/CallFreespaceMotion.srv: -------------------------------------------------------------------------------- 1 | string target_link 2 | 3 | # start joint position 4 | sensor_msgs/JointState start_position # leave empty to use current position 5 | 6 | sensor_msgs/JointState goal_position # looks into this one first 7 | geometry_msgs/Transform goal_pose # use this one when 'goal_position' is empty 8 | 9 | int32 num_steps # Set to 0 in order to use default 10 | 11 | bool execute 12 | --- 13 | trajectory_msgs/JointTrajectory output_trajectory 14 | bool success 15 | string message 16 | -------------------------------------------------------------------------------- /crs_msgs/srv/CropToolpaths.srv: -------------------------------------------------------------------------------- 1 | # Region Data 2 | sensor_msgs/Image[] images 3 | sensor_msgs/PointCloud2[] clouds 4 | geometry_msgs/TransformStamped[] transforms # transforms the pointclouds into the toolpaths frame id 5 | 6 | # Toolpaths to crop 7 | crs_msgs/ToolProcessPath[] toolpaths 8 | --- 9 | 10 | crs_msgs/ToolProcessPath[] cropped_toolpaths 11 | bool succeeded 12 | string err_msg -------------------------------------------------------------------------------- /crs_msgs/srv/ExecuteAction.srv: -------------------------------------------------------------------------------- 1 | string action_id 2 | --- 3 | bool succeeded 4 | string err_msg -------------------------------------------------------------------------------- /crs_msgs/srv/GetAvailableActions.srv: -------------------------------------------------------------------------------- 1 | string PATTERN_USER_ACTIONS="^user*" 2 | 3 | string search_pattern # only actions matching this pattern will be returned, leave empty to get all actions 4 | --- 5 | string[] action_ids 6 | bool succeeded 7 | string err_msg -------------------------------------------------------------------------------- /crs_msgs/srv/GetConfiguration.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | ProcessConfiguration config 4 | bool success 5 | string err_msg -------------------------------------------------------------------------------- /crs_msgs/srv/GetROISelection.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | 3 | # The set of points from which the ROI 4 | # will be selected 5 | sensor_msgs/PointCloud2 input_cloud 6 | 7 | --- 8 | 9 | # Response 10 | bool success 11 | string message 12 | int32[] cloud_indices 13 | -------------------------------------------------------------------------------- /crs_msgs/srv/LoadPart.srv: -------------------------------------------------------------------------------- 1 | string path_to_part 2 | geometry_msgs/Pose part_origin 3 | --- 4 | bool success 5 | string error 6 | -------------------------------------------------------------------------------- /crs_msgs/srv/LocalizeToPart.srv: -------------------------------------------------------------------------------- 1 | # All point clouds should be in the same frame 2 | # The specified frame should be stationary (i.e. world, robot base, etc.) 3 | string frame 4 | sensor_msgs/PointCloud2[] point_clouds 5 | geometry_msgs/TransformStamped[] transforms 6 | --- 7 | bool success 8 | string error 9 | geometry_msgs/TransformStamped transform 10 | -------------------------------------------------------------------------------- /crs_msgs/srv/PlanProcessMotions.srv: -------------------------------------------------------------------------------- 1 | string tool_link 2 | sensor_msgs/JointState start_position 3 | sensor_msgs/JointState end_position 4 | geometry_msgs/Pose tool_offset 5 | float64 approach_dist 6 | float64 retreat_dist 7 | float64 tool_speed 8 | float64 target_force 9 | crs_msgs/ToolProcessPath[] process_paths 10 | 11 | --- 12 | crs_msgs/ProcessMotionPlan[] plans # The motion plans 13 | bool succeeded 14 | string err_msg 15 | -------------------------------------------------------------------------------- /crs_msgs/srv/RobotPositioner.srv: -------------------------------------------------------------------------------- 1 | uint8 POSITION1=0 2 | uint8 POSITION2=1 3 | uint8 POSITION3=2 4 | uint8 robot_position 5 | 6 | uint8 RAIL1=0 7 | uint8 RAIL2=1 8 | uint8 robot_rail 9 | 10 | --- 11 | bool success 12 | string error 13 | -------------------------------------------------------------------------------- /crs_msgs/srv/RunRobotScript.srv: -------------------------------------------------------------------------------- 1 | # Service to load programs or installations 2 | string filename 3 | --- 4 | string answer 5 | bool success 6 | -------------------------------------------------------------------------------- /crs_perception/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crs_perception) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(crs_msgs REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(rclcpp_components REQUIRED) 22 | find_package(sensor_msgs REQUIRED) 23 | find_package(tf2_ros REQUIRED) 24 | find_package(tf2_geometry_msgs REQUIRED) 25 | find_package(visualization_msgs REQUIRED) 26 | find_package(cv_bridge REQUIRED) 27 | find_package(tf2_eigen REQUIRED) 28 | find_package(pcl_conversions REQUIRED) 29 | find_package(PCL REQUIRED) 30 | find_package(VTK REQUIRED NO_MODULE) 31 | 32 | include_directories(include 33 | SYSTEM 34 | ${PCL_INCLUDE_DIRS} 35 | ${VTK_INCLUDE_DIRS}) 36 | 37 | add_library(${PROJECT_NAME} SHARED 38 | src/model_to_point_cloud.cpp 39 | ) 40 | target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ${VTK_LIBRARIES}) 41 | 42 | # node as component plugin 43 | add_library(${PROJECT_NAME}_nodes SHARED 44 | src/nodes/localize_to_part.cpp) 45 | rclcpp_components_register_nodes(${PROJECT_NAME}_nodes "crs_perception::LocalizeToPart") 46 | target_link_libraries(${PROJECT_NAME}_nodes ${PROJECT_NAME} ${PCL_LIBRARIES}) 47 | ament_target_dependencies(${PROJECT_NAME}_nodes rclcpp rclcpp_components crs_msgs tf2_ros visualization_msgs) 48 | 49 | # node as executable, component plugin can't load parameters 50 | add_executable(localize_to_part src/nodes/localize_to_part.cpp src/model_to_point_cloud.cpp) 51 | target_include_directories(localize_to_part PUBLIC 52 | "$" 53 | "$") 54 | target_include_directories(localize_to_part SYSTEM PUBLIC) 55 | target_link_libraries(localize_to_part ${PCL_LIBRARIES}) 56 | ament_target_dependencies(localize_to_part rclcpp rclcpp_components crs_msgs tf2_eigen tf2_ros std_msgs visualization_msgs) 57 | 58 | # test executable 59 | add_executable(test_localize_to_part src/nodes/test_localize_to_part.cpp) 60 | target_link_libraries(test_localize_to_part ${PROJECT_NAME} ${PCL_LIBRARIES}) 61 | ament_target_dependencies(test_localize_to_part rclcpp crs_msgs std_msgs) 62 | 63 | ### Install 64 | install(TARGETS localize_to_part test_localize_to_part 65 | DESTINATION lib/${PROJECT_NAME}) 66 | 67 | install(DIRECTORY include/ 68 | DESTINATION include 69 | ) 70 | 71 | install(TARGETS 72 | ${PROJECT_NAME} 73 | ${PROJECT_NAME}_nodes 74 | RUNTIME DESTINATION bin 75 | LIBRARY DESTINATION lib 76 | ARCHIVE DESTINATION lib 77 | ) 78 | 79 | if(BUILD_TESTING) 80 | find_package(ament_lint_auto REQUIRED) 81 | # the following line skips the linter which checks for copyrights 82 | # uncomment the line when a copyright and license is not present in all source files 83 | #set(ament_cmake_copyright_FOUND TRUE) 84 | # the following line skips cpplint (only works in a git repo) 85 | # uncomment the line when this package is not in a git repo 86 | #set(ament_cmake_cpplint_FOUND TRUE) 87 | ament_lint_auto_find_test_dependencies() 88 | endif() 89 | 90 | ament_export_dependencies(ament_cmake) 91 | ament_package() 92 | -------------------------------------------------------------------------------- /crs_perception/README.md: -------------------------------------------------------------------------------- 1 | # Components 2 | ## LocalizeToPart 3 | ### Parameters 4 | - mesh_num_samples - Number of samples to take on the mesh for converting the part model into a point cloud 5 | - leaf_size - XYZ leaf size for the VoxelGrid (for data reduction) for converting the part model into a point cloud 6 | - part_frame - frame ID for the loaded part 7 | ### Services 8 | - load_part - Specifies the path to the part model (obj format) 9 | - localize_to_part - Takes a list of point clouds (multiple view points of an object) to run ICP against the refernce part previously loaded and returns a transform 10 | # Test Node 11 | ## test_localize_to_part 12 | ### Parameters 13 | - point_cloud_files - List of pcd files that represent multiple view points of an object in the same fixed reference frame 14 | - part_file - Path to the model file to localize against 15 | # Run 16 | In order to test the node, run the component and the tester node as follows 17 | - `ros2 component standalone crs_perception crs_perception::LocalizeToPart` 18 | - `ros2 run crs_perception test_localize_to_part ` 19 | -------------------------------------------------------------------------------- /crs_perception/include/crs_perception/model_to_point_cloud.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace crs_perception 7 | { 8 | class ModelToPointCloud 9 | { 10 | public: 11 | ModelToPointCloud(int num_samples, float leaf_size) : num_samples_(num_samples), leaf_size_(leaf_size) {} 12 | bool convertToPCL(const std::string& file_path, 13 | pcl::PointCloud::Ptr point_cloud, 14 | std::string& err_msg); 15 | 16 | private: 17 | std::string file_path_; 18 | int num_samples_; 19 | float leaf_size_; 20 | 21 | void uniformSampling(vtkSmartPointer polydata, 22 | std::size_t n_samples, 23 | pcl::PointCloud& cloud_out); 24 | inline void randPSurface(vtkSmartPointer polydata, 25 | std::vector* cumulativeAreas, 26 | double totalArea, 27 | Eigen::Vector3f& p); 28 | inline void randomPointTriangle(float a1, 29 | float a2, 30 | float a3, 31 | float b1, 32 | float b2, 33 | float b3, 34 | float c1, 35 | float c2, 36 | float c3, 37 | float r1, 38 | float r2, 39 | Eigen::Vector3f& p); 40 | inline double uniformDeviate(int seed); 41 | }; 42 | 43 | } // namespace crs_perception 44 | -------------------------------------------------------------------------------- /crs_perception/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_perception 5 | 0.0.0 6 | The perception nodes and library for the CRS project 7 | ros-industrial 8 | BSD 9 | 10 | ament_cmake 11 | 12 | crs_msgs 13 | geometry_msgs 14 | pcl_conversions 15 | rclcpp 16 | rclcpp_components 17 | sensor_msgs 18 | tf2_ros 19 | tf2_geometry_msgs 20 | visualization_msgs 21 | cv_bridge 22 | tf2_eigen 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /crs_perception/src/nodes/test_localize_to_part.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | int main(int argc, char** argv) 15 | { 16 | rclcpp::init(argc, argv); 17 | std::shared_ptr node = rclcpp::Node::make_shared("test_localize_to_part"); 18 | 19 | // parameters 20 | std::vector point_cloud_files; 21 | std::string part_file; 22 | node->get_parameter("point_cloud_files", point_cloud_files); 23 | node->get_parameter("part_file", part_file); 24 | 25 | std::vector::Ptr> point_clouds; 26 | 27 | for (auto& pcf : point_cloud_files) 28 | { 29 | pcl::PointCloud::Ptr point_cloud = boost::make_shared >(); 30 | 31 | if (pcl::io::loadPCDFile(pcf, *point_cloud) == -1) 32 | { 33 | RCLCPP_ERROR(node->get_logger(), "Couldn't read file %s", pcf.c_str()); 34 | rclcpp::shutdown(); 35 | return 1; 36 | } 37 | 38 | point_clouds.push_back(point_cloud); 39 | } 40 | 41 | rclcpp::Client::SharedPtr load_part_client = 42 | node->create_client("load_part"); 43 | rclcpp::Client::SharedPtr localize_to_part_client = 44 | node->create_client("localize_to_part"); 45 | 46 | while (!localize_to_part_client->wait_for_service() || !load_part_client->wait_for_service()) 47 | { 48 | RCLCPP_INFO(node->get_logger(), "service not available, waiting again."); 49 | } 50 | 51 | auto load_part_request = std::make_shared(); 52 | load_part_request->path_to_part = part_file; 53 | 54 | auto result_future = load_part_client->async_send_request(load_part_request); 55 | if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::executor::FutureReturnCode::SUCCESS) 56 | { 57 | RCLCPP_ERROR(node->get_logger(), "service call failed :("); 58 | rclcpp::shutdown(); 59 | return 1; 60 | } 61 | auto result = result_future.get(); 62 | RCLCPP_INFO(node->get_logger(), "%d", result->success); 63 | 64 | // pcl::transformPointcloud 65 | std::pair transform = std::make_pair( 66 | Eigen::Vector3d(1, 10, 3), 67 | Eigen::Quaterniond(Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) * 68 | Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) * 69 | Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ()))); 70 | 71 | auto localize_to_part_request = std::make_shared(); 72 | for (auto& point_cloud : point_clouds) 73 | { 74 | pcl::PointCloud tf_point_cloud; 75 | pcl::transformPointCloud(*point_cloud, tf_point_cloud, transform.first, transform.second); 76 | sensor_msgs::msg::PointCloud2 point_cloud_msg; 77 | pcl::toROSMsg(tf_point_cloud, point_cloud_msg); 78 | localize_to_part_request->point_clouds.push_back(point_cloud_msg); 79 | } 80 | 81 | auto localize_result_future = localize_to_part_client->async_send_request(localize_to_part_request); 82 | if (rclcpp::spin_until_future_complete(node, localize_result_future) != rclcpp::executor::FutureReturnCode::SUCCESS) 83 | { 84 | RCLCPP_ERROR(node->get_logger(), "service call failed :("); 85 | rclcpp::shutdown(); 86 | return 1; 87 | } 88 | auto localize_result = localize_result_future.get(); 89 | RCLCPP_INFO(node->get_logger(), "%d", localize_result->success); 90 | RCLCPP_INFO(node->get_logger(), 91 | "Transform : (%f, %f, %f)", 92 | localize_result->transform.transform.translation.x, 93 | localize_result->transform.transform.translation.y, 94 | localize_result->transform.transform.translation.z); 95 | 96 | rclcpp::shutdown(); 97 | return 0; 98 | } 99 | -------------------------------------------------------------------------------- /crs_process_data/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crs_process_data) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | install(DIRECTORY data 25 | DESTINATION share/${PROJECT_NAME}/ 26 | PATTERN ".svn" EXCLUDE 27 | ) 28 | 29 | if(BUILD_TESTING) 30 | find_package(ament_lint_auto REQUIRED) 31 | # the following line skips the linter which checks for copyrights 32 | # uncomment the line when a copyright and license is not present in all source files 33 | #set(ament_cmake_copyright_FOUND TRUE) 34 | # the following line skips cpplint (only works in a git repo) 35 | # uncomment the line when this package is not in a git repo 36 | #set(ament_cmake_cpplint_FOUND TRUE) 37 | ament_lint_auto_find_test_dependencies() 38 | endif() 39 | 40 | ament_package() 41 | -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_1/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: false 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994] 8 | process_path: 9 | tool_speed: 0.1 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 11 | retreat_dist: 0.03 12 | approach_dist: 0.03 13 | tool_frame: sander_center_link 14 | target_force: 15 15 | media_change: 16 | change_time: 300.0 17 | change_pose: [-0.381, -0.202, 1.5, 0.0, -0.786, 1.57] 18 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 19 | joint_position: [] 20 | preview: 21 | time_scaling: 4.0 22 | scan_acquisition: 23 | scan_poses: 24 | - pose: [-1.444, -0.322, 1.495, 2.5029407, 0.9721462, -2.3962962] 25 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 26 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 27 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 28 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 29 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 30 | - pose: [-0.045, 0.794, 1.580, 2.3330813, -0.3216955, -2.8142385] 31 | tool_frame: camera_link_optical 32 | skip_on_failure: true 33 | process_execution: 34 | time_tolerance: 5.0 35 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] 36 | cartesian_path_tolerance: [0.015, 0.015, 0.025, 0.15, 0.15, 0.15] 37 | cartesian_goal_tolerance: [0.01, 0.01, 0.01, 0.05, 0.05, 0.05] 38 | force_tolerance: 10 39 | force_controlled_trajectories: false 40 | ur_tool_change_script: simpleMove.urp 41 | execute_tool_change: false 42 | part_registration: 43 | simulation_pose: [-0.7, 0.0, 1.1, 0.0, 0.0, 1.57] 44 | seed_pose: [-0.7, 0.0, 1.1, 0.0, 0.0, 1.57] 45 | target_frame_id: world 46 | part_file: "" 47 | toolpath_file: "" 48 | part_rework: 49 | scan_poses: 50 | - pose: [-1.444, -0.322, 1.495, 2.5029407, 0.9721462, -2.3962962] 51 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 52 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 53 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 54 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 55 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 56 | - pose: [-0.045, 0.794, 1.580, 2.3330813, -0.3216955, -2.8142385] 57 | tool_frame: camera_link_optical 58 | pre_acquisition_pause: 2.0 59 | skip_on_failure: true 60 | -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_1/part_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_process_data/data/main/toolpaths/part_1/part_1.stl -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_2/configs/job_0_degrees_20200520T192241.997282_config.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: true 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994] 8 | process_path: 9 | tool_speed: 0.1 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 11 | retreat_dist: 0.03 12 | approach_dist: 0.03 13 | tool_frame: sander_center_link 14 | target_force: 20 15 | media_change: 16 | change_time: 300.0 17 | change_pose: [-0.381, -0.202, 1.5, 0.0, -0.786, 1.57] 18 | preview: 19 | time_scaling: 4.0 20 | scan_acquisition: 21 | scan_poses: 22 | - pose: [0.0, -0.50, 1.7, 0.0, -2.35619, 1.57] 23 | - pose: [0.0, -0.25, 1.7, 0.0, -2.35619, 1.57] 24 | - pose: [0.0, -0.10, 1.7, 0.5, -2.35619, 1.57] 25 | - pose: [0.0, 0.0, 1.7, 0.0, -2.35619, 1.57] 26 | - pose: [0.0, 0.1, 1.7, -0.8, -2.35619, 1.57] 27 | tool_frame: camera_link_optical 28 | skip_on_failure: true 29 | process_execution: 30 | time_tolerance: 5.0 31 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] 32 | part_registration: 33 | simulation_pose: [-0.7, 0.0, 1.1, 0.0, 0.0, 1.57] 34 | seed_pose: [-0.68, 0.0, 1.0, 0.0, 0.0, 1.57] 35 | target_frame_id: world 36 | part_file: "" 37 | toolpath_file: "" 38 | part_rework: {} 39 | -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_2/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: true 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994] 8 | process_path: 9 | tool_speed: 0.1 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [px, py, pz, rx, ry, rz] relative to default tool 11 | retreat_dist: 0.03 12 | approach_dist: 0.03 13 | tool_frame: "sander_center_link" 14 | target_force: 15 15 | media_change: 16 | change_time: 300.0 # seconds 17 | change_pose: [-0.381, -0.202, 1.5, 0.0, -0.786, 1.57] # [px, py, pz, rx, ry, rz] in world coordinates 18 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 19 | joint_position: [] 20 | preview: 21 | time_scaling: 4.0 # larger values will play the preview faster 22 | scan_acquisition: 23 | scan_poses: 24 | - pose: [-1.444, -0.322, 1.495, 2.5029407, 0.9721462, -2.3962962] 25 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 26 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 27 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 28 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 29 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 30 | - pose: [-0.045, 0.794, 1.580, 2.3330813, -0.3216955, -2.8142385] 31 | tool_frame: "camera_link_optical" 32 | skip_on_failure: true 33 | process_execution: 34 | time_tolerance: 5.0 # seconds 35 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] # radians 36 | cartesian_path_tolerance: [0.015, 0.015, 0.025, 0.15, 0.15, 0.15] 37 | cartesian_goal_tolerance: [0.01, 0.01, 0.01, 0.05, 0.05, 0.05] 38 | force_tolerance: 10 39 | force_controlled_trajectories: false 40 | ur_tool_change_script: simpleMove.urp 41 | execute_tool_change: false 42 | part_registration: 43 | simulation_pose: [-0.7, 0.0, 1.1, 0.0, 0.0, 1.57] 44 | seed_pose: [-0.68, 0.0, 1.0, 0.0, 0.0, 1.57] 45 | target_frame_id: "world" 46 | part_file: "" # relative to known data directory 47 | toolpath_file: "" # relative to known data directory 48 | part_rework: 49 | scan_poses: 50 | - pose: [-1.444, -0.322, 1.495, 2.5029407, 0.9721462, -2.3962962] 51 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 52 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 53 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 54 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 55 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 56 | - pose: [-0.045, 0.794, 1.580, 2.3330813, -0.3216955, -2.8142385] 57 | tool_frame: "camera_link_optical" 58 | pre_acquisition_pause: 2.0 # seconds 59 | skip_on_failure: true 60 | -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_2/part_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_process_data/data/main/toolpaths/part_2/part_2.stl -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_3/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: false 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994] 8 | process_path: 9 | tool_speed: 0.1 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 11 | retreat_dist: 0.03 12 | approach_dist: 0.03 13 | tool_frame: sander_center_link 14 | target_force: 15 15 | media_change: 16 | change_time: 300.0 17 | change_pose: [-0.381, -0.202, 1.5, 0.0, -0.786, 1.57] 18 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 19 | joint_position: [] 20 | preview: 21 | time_scaling: 4.0 22 | scan_acquisition: 23 | scan_poses: 24 | - pose: [-1.444, -0.322, 1.495, 2.5029407, 0.9721462, -2.3962962] 25 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 26 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 27 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 28 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 29 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 30 | - pose: [-0.045, 0.794, 1.580, 2.3330813, -0.3216955, -2.8142385] 31 | tool_frame: camera_link_optical 32 | skip_on_failure: true 33 | process_execution: 34 | time_tolerance: 5.0 35 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] 36 | cartesian_path_tolerance: [0.015, 0.015, 0.025, 0.15, 0.15, 0.15] 37 | cartesian_goal_tolerance: [0.01, 0.01, 0.01, 0.05, 0.05, 0.05] 38 | force_tolerance: 10 39 | force_controlled_trajectories: false 40 | ur_tool_change_script: simpleMove.urp 41 | execute_tool_change: false 42 | part_registration: 43 | simulation_pose: [-0.7, 0.0, 1.1, 0.0, 0.0, 0.0] 44 | seed_pose: [-0.72, 0.0, 1.0, 0.0, 0.0, 0.0] 45 | target_frame_id: world 46 | part_file: "" 47 | toolpath_file: "" 48 | part_rework: 49 | scan_poses: 50 | - pose: [-1.444, -0.322, 1.495, 2.5029407, 0.9721462, -2.3962962] 51 | - pose: [-0.869, -0.397, 1.559, -3.1289652, 0.6427091, 3.1099935] 52 | - pose: [0.005, 0.621, 1.534, -2.6741055, -0.6119461, -1.9862345] 53 | - pose: [-0.348, -0.211, 1.555, -2.2509677, -0.1362007, -3.0876068] 54 | - pose: [0.036, 0.258, 1.549, -2.4653191, -0.7150566, 2.9155958] 55 | - pose: [-0.201, -0.062, 1.604, 2.4117773, -0.3157807, -2.7579127] 56 | - pose: [-0.045, 0.794, 1.580, 2.3330813, -0.3216955, -2.8142385] 57 | tool_frame: camera_link_optical 58 | pre_acquisition_pause: 2.0 59 | skip_on_failure: true 60 | -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_3/part_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_process_data/data/main/toolpaths/part_3/part_3.stl -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_5/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: false 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-1.8444, -1.85261, -1.6167, -1.6665, 0.8773, -0.9489] 8 | process_path: 9 | tool_speed: 0.05 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 11 | retreat_dist: 0.025 12 | approach_dist: 0.025 13 | tool_frame: sander_center_link 14 | target_force: 15 15 | media_change: 16 | change_time: 1200.0 17 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 18 | joint_position: [-0.02467, -1.7719, -1.8680, -0.6212, 0.90265, -2.2430] 19 | preview: 20 | time_scaling: 4.0 21 | scan_acquisition: 22 | scan_poses: 23 | - pose: [-1.483, -0.229, 1.735, 2.797, 1.17, -2.43] 24 | - pose: [-1.527, -0.239, 1.731, 2.74, 0.66, -2.79] 25 | - pose: [-1.698, 0.357, 1.445, 2.955, 0.904, -1.36] 26 | tool_frame: camera_link_optical 27 | skip_on_failure: true 28 | process_execution: 29 | time_tolerance: 120.0 30 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] 31 | cartesian_path_tolerance: [0.015, 0.015, 0.035, 0.15, 0.15, 0.15] 32 | cartesian_goal_tolerance: [0.01, 0.01, 0.025, 0.05, 0.05, 0.05] 33 | force_tolerance: 5 34 | force_controlled_trajectories: false 35 | ur_tool_change_script: simpleMove.urp 36 | execute_tool_change: false 37 | part_registration: 38 | simulation_pose: [-0.55, 0.0, 0.98, 0.0, 0.0, -2.8] 39 | seed_pose: [-0.55, 0.0, 0.98, 0.0, 0.0, -2.8] 40 | #seed_pose: [-0.75, 0.0, 1.1, 0.0, 0.0, -2.8] 41 | target_frame_id: world 42 | part_file: "" 43 | toolpath_file: "" 44 | toolpath_edge_buffer: 0.15 45 | part_rework: 46 | scan_poses: 47 | - pose: [0.0, -0.50, 1.7, 0.0, -2.35619, 1.57] 48 | - pose: [0.0, -0.25, 1.7, 0.0, -2.35619, 1.57] 49 | - pose: [0.0, -0.10, 1.7, 0.5, -2.35619, 1.57] 50 | - pose: [0.0, 0.0, 1.7, 0.0, -2.35619, 1.57] 51 | - pose: [0.0, 0.1, 1.7, -0.8, -2.35619, 1.57] 52 | tool_frame: camera_link_optical 53 | pre_acquisition_pause: 2.0 54 | skip_on_failure: true 55 | -------------------------------------------------------------------------------- /crs_process_data/data/main/toolpaths/part_5/part_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_process_data/data/main/toolpaths/part_5/part_5.stl -------------------------------------------------------------------------------- /crs_process_data/data/surrogate1/toolpaths/fake_part/configs/fake_toolpath_config.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: false 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-1.8444, -1.85261, -1.6167, -1.6665, 0.8773, -0.9489] 8 | process_path: 9 | tool_speed: 0.1 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 11 | retreat_dist: 0.025 12 | approach_dist: 0.025 13 | tool_frame: sander_center_link 14 | target_force: 50 15 | media_change: 16 | change_time: 15.0 17 | change_pose: [0.771, -0.279, 0.220, 3.138, -0.005, -0.486] 18 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 19 | joint_position: [-0.02467, -1.7719, -1.8680, -0.6212, 0.90265, -2.2430] 20 | preview: 21 | time_scaling: 4.0 22 | scan_acquisition: 23 | scan_poses: 24 | - pose: [0.668, -0.423, 0.320, 2.232, -0.014, -0.457] 25 | - pose: [0.794, -0.412, 0.320, 2.233, -0.016, -0.745] 26 | tool_frame: camera_link_optical 27 | skip_on_failure: true 28 | process_execution: 29 | time_tolerance: 60.0 30 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] 31 | cartesian_path_tolerance: [0.015, 0.015, 0.025, 0.15, 0.15, 0.15] 32 | cartesian_goal_tolerance: [0.01, 0.01, 0.01, 0.05, 0.05, 0.05] 33 | force_tolerance: 10 34 | force_controlled_trajectories: true 35 | ur_tool_change_script: simpleMove.urp 36 | execute_tool_change: false 37 | part_registration: 38 | simulation_pose: [0.0, -0.875, -0.36, 0.0, 0.0, 0.0] 39 | seed_pose: [0.0, -0.875, -0.36, 0.0, 0.0, 0.0] 40 | target_frame_id: world 41 | part_file: "" 42 | toolpath_file: "" 43 | part_rework: {} 44 | -------------------------------------------------------------------------------- /crs_process_data/data/surrogate1/toolpaths/fake_part/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: false 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-1.8444, -1.85261, -1.6167, -1.6665, 0.8773, -0.9489] 8 | process_path: 9 | tool_speed: 0.05 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 11 | retreat_dist: 0.03 12 | approach_dist: 0.03 13 | tool_frame: sander_center_link 14 | target_force: 30 15 | media_change: 16 | change_time: 120.0 17 | change_pose: [0.771, -0.279, 0.220, 3.138, -0.005, -0.486] 18 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 19 | joint_position: [-0.02467, -1.7719, -1.8680, -0.6212, 0.90265, -2.2430] 20 | preview: 21 | time_scaling: 4.0 # larger values will play the preview faster 22 | scan_acquisition: 23 | scan_poses: 24 | - pose: [0.668, -0.423, 0.320, 2.232, -0.014, -0.457] 25 | - pose: [0.794, -0.412, 0.320, 2.233, -0.016, -0.745] 26 | tool_frame: camera_link_optical 27 | skip_on_failure: true 28 | process_execution: 29 | time_tolerance: 120.0 30 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] 31 | cartesian_path_tolerance: [0.015, 0.015, 0.035, 0.15, 0.15, 0.15] 32 | cartesian_goal_tolerance: [0.01, 0.01, 0.025, 0.05, 0.05, 0.05] 33 | force_tolerance: 5 34 | force_controlled_trajectories: false 35 | ur_tool_change_script: 'simpleMove.urp' 36 | execute_tool_change: false 37 | part_registration: 38 | simulation_pose: [0.0, -0.875, -0.36, 0.0, 0.0, 0.0] 39 | seed_pose: [0.0, -0.875, -0.4, 0.0, 0.0, 0.0] 40 | target_frame_id: world 41 | part_file: "" 42 | toolpath_file: "" 43 | part_rework: 44 | scan_poses: 45 | - pose: [0.111, -0.972, 0.307, 3.049, -0.064, -0.074] 46 | - pose: [0.638, -0.913, 0.258, 2.8819846, 0.1335439, -1.1955488] 47 | - pose: [0.334, -1.005, 0.273, 3.067, -0.042, 0.175] 48 | - pose: [0.701, -0.402, 0.189, 2.393, 0.115, -0.280] 49 | tool_frame: "camera_link_optical" 50 | pre_acquisition_pause: 2.0 # seconds 51 | skip_on_failure: true 52 | 53 | -------------------------------------------------------------------------------- /crs_process_data/data/surrogate1/toolpaths/fake_part/fake_part.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_process_data/data/surrogate1/toolpaths/fake_part/fake_part.stl -------------------------------------------------------------------------------- /crs_process_data/data/surrogate1/toolpaths/part_2/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: false 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-1.8444, -1.85261, -1.6167, -1.6665, 0.8773, -0.9489] 8 | process_path: 9 | tool_speed: 0.1 10 | offset_pose: [0.0, 0.0, 0.028, 0.0, 0.0, 0.0] 11 | retreat_dist: 0.01 12 | approach_dist: 0.01 13 | tool_frame: sander_center_link 14 | target_force: 50 15 | media_change: 16 | change_time: 110.0 17 | change_pose: [0.771, -0.279, 0.220, 3.138, -0.005, -0.486] 18 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 19 | joint_position: [-0.02467, -1.7719, -1.8680, -0.6212, 0.90265, -2.2430] 20 | preview: 21 | time_scaling: 4.0 # larger values will play the preview faster 22 | scan_acquisition: 23 | scan_poses: 24 | - pose: [0.701, -0.402, 0.189, 2.393, 0.115, -0.280] 25 | - pose: [0.638, -0.913, 0.258, 2.8819846, 0.1335439, -1.1955488] 26 | - pose: [0.334, -1.005, 0.273, 3.067, -0.042, 0.175] 27 | - pose: [0.111, -0.972, 0.307, 3.049, -0.064, -0.074] 28 | # - pose: [0.274, -0.982, 0.189, -3.1054148, -0.0123395, -0.8671079] 29 | # - pose: [0.668, -0.423, 0.320, 2.232, -0.014, -0.457] 30 | # - pose: [0.794, -0.412, 0.320, 2.233, -0.016, -0.745] 31 | tool_frame: camera_link_optical 32 | skip_on_failure: true 33 | process_execution: 34 | time_tolerance: 120.0 35 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] 36 | cartesian_path_tolerance: [0.015, 0.015, 0.05, 0.15, 0.15, 0.15] 37 | cartesian_goal_tolerance: [0.01, 0.01, 0.035, 0.05, 0.05, 0.05] 38 | force_tolerance: 10 39 | force_controlled_trajectories: false 40 | ur_tool_change_script: 'simpleMove.urp' 41 | execute_tool_change: false 42 | part_registration: 43 | simulation_pose: [0.45, -1.04, -0.23, 0.0, 0.0, 3.14] 44 | seed_pose: [0.45, -1.04, -0.23, 0.0, 0.0, 3.14] 45 | target_frame_id: world 46 | part_file: "" 47 | toolpath_file: "" 48 | part_rework: 49 | scan_poses: 50 | - pose: [0.111, -0.972, 0.307, 3.049, -0.064, -0.074] 51 | - pose: [0.55, -0.913, 0.258, 2.8819846, 0.1335439, -1.1955488] 52 | - pose: [0.334, -1.005, 0.273, 3.067, -0.042, 0.175] 53 | - pose: [0.701, -0.402, 0.189, 2.393, 0.115, -0.280] 54 | tool_frame: "camera_link_optical" 55 | pre_acquisition_pause: 2.0 # seconds 56 | skip_on_failure: true 57 | -------------------------------------------------------------------------------- /crs_process_data/data/surrogate1/toolpaths/part_2/part_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_process_data/data/surrogate1/toolpaths/part_2/part_2.stl -------------------------------------------------------------------------------- /crs_process_data/data/surrogate1/toolpaths/part_2/part_2_original.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_process_data/data/surrogate1/toolpaths/part_2/part_2_original.stl -------------------------------------------------------------------------------- /crs_process_data/data/surrogate1/toolpaths/swri_part/crs.yaml: -------------------------------------------------------------------------------- 1 | crs: 2 | general: {} 3 | motion_planning: 4 | pre_move_home: false 5 | home_position: 6 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 7 | joint_position: [-1.8444, -1.85261, -1.6167, -1.6665, 0.8773, -0.9489] 8 | process_path: 9 | tool_speed: 0.075 10 | offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 11 | retreat_dist: 0.025 12 | approach_dist: 0.025 13 | tool_frame: sander_center_link 14 | target_force: 30 15 | media_change: 16 | change_time: 1200.0 17 | change_pose: [0.771, -0.279, 0.220, 3.138, -0.005, -0.486] 18 | joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 19 | joint_position: [-0.02467, -1.7719, -1.8680, -0.6212, 0.90265, -2.2430] 20 | preview: 21 | time_scaling: 4.0 22 | scan_acquisition: 23 | scan_poses: 24 | - pose: [0.565, 0.105, 0.330, -1.8801855, -0.4188469, 2.9097765] 25 | - pose: [0.682, 0.048, 0.273, -1.8517454, 0.0086693, 3.0399052] 26 | tool_frame: camera_link_optical 27 | skip_on_failure: true 28 | process_execution: 29 | time_tolerance: 3000.0 30 | joint_tolerance: [0.035, 0.035, 0.035, 0.035, 0.035, 0.035] 31 | cartesian_path_tolerance: [0.015, 0.015, 0.05, 0.15, 0.15, 0.15] 32 | cartesian_goal_tolerance: [0.015, 0.015, 0.035, 0.05, 0.05, 0.05] 33 | force_tolerance: 7.5 34 | force_controlled_trajectories: true 35 | ur_tool_change_script: simpleMove.urp 36 | execute_tool_change: false 37 | part_registration: 38 | simulation_pose: [0.0, 0.8, 0.2, -1.57, 3.14, -1.57] 39 | seed_pose: [0.05, 0.95, 0.0, -1.57, 3.14, -1.57] 40 | target_frame_id: world 41 | part_file: "" 42 | toolpath_file: "" 43 | part_rework: 44 | scan_poses: 45 | - pose: [0.0, -0.50, 1.7, 0.0, -2.35619, 1.57] 46 | - pose: [0.0, -0.25, 1.7, 0.0, -2.35619, 1.57] 47 | - pose: [0.0, -0.10, 1.7, 0.5, -2.35619, 1.57] 48 | - pose: [0.0, 0.0, 1.7, 0.0, -2.35619, 1.57] 49 | - pose: [0.0, 0.1, 1.7, -0.8, -2.35619, 1.57] 50 | tool_frame: camera_link_optical 51 | pre_acquisition_pause: 2.0 52 | skip_on_failure: true 53 | -------------------------------------------------------------------------------- /crs_process_data/data/surrogate1/toolpaths/swri_part/swri_part.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_process_data/data/surrogate1/toolpaths/swri_part/swri_part.stl -------------------------------------------------------------------------------- /crs_process_data/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_process_data 5 | 0.0.0 6 | Part models and toolpath files for the Collaborative Robotic Sanding application 7 | Jorge Nicho 8 | BSD 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /crs_robot_comms/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crs_robot_comms) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(ament_index_cpp REQUIRED) 21 | 22 | find_package(rosidl_default_generators REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | 25 | find_package(crs_msgs REQUIRED) 26 | 27 | find_package(std_srvs REQUIRED) 28 | find_package(std_msgs REQUIRED) 29 | find_package(ur_dashboard_msgs REQUIRED) 30 | 31 | add_executable(${PROJECT_NAME}_ur_comms src/ur_comms.cpp) 32 | target_include_directories(${PROJECT_NAME}_ur_comms PUBLIC 33 | "$" 34 | "$") 35 | target_include_directories(${PROJECT_NAME}_ur_comms PUBLIC 36 | ${rclcpp_INCLUDE_DIRS} 37 | ${rclcpp_action_INCLUDE_DIRS} 38 | ${crs_msgs_INCLUDE_DIRS} 39 | ${std_srvs_INCLUDE_DIRS} 40 | ${ur_dashboard_msgs_INCLUDE_DIRS} 41 | ) 42 | target_link_libraries(${PROJECT_NAME}_ur_comms 43 | ${rclcpp_LIBRARIES} 44 | ${rclcpp_action_LIBRARIES} 45 | ${crs_msgs_LIBRARIES} 46 | ${std_srvs_LIBRARIES} 47 | ${ur_dashboard_msgs_LIBRARIES} 48 | ) 49 | 50 | add_executable(${PROJECT_NAME}_ur_comms_sim src/ur_comms_sim.cpp) 51 | target_include_directories(${PROJECT_NAME}_ur_comms_sim PUBLIC 52 | "$" 53 | "$") 54 | target_include_directories(${PROJECT_NAME}_ur_comms_sim PUBLIC 55 | ${rclcpp_INCLUDE_DIRS} 56 | ${rclcpp_action_INCLUDE_DIRS} 57 | ${crs_msgs_INCLUDE_DIRS} 58 | ${std_srvs_INCLUDE_DIRS} 59 | ${ur_dashboard_msgs_INCLUDE_DIRS} 60 | ) 61 | target_link_libraries(${PROJECT_NAME}_ur_comms_sim 62 | ${rclcpp_LIBRARIES} 63 | ${rclcpp_action_LIBRARIES} 64 | ${crs_msgs_LIBRARIES} 65 | ${std_srvs_LIBRARIES} 66 | ${ur_dashboard_msgs_LIBRARIES} 67 | ) 68 | 69 | if(BUILD_TESTING) 70 | find_package(ament_lint_auto REQUIRED) 71 | # the following line skips the linter which checks for copyrights 72 | # uncomment the line when a copyright and license is not present in all source files 73 | #set(ament_cmake_copyright_FOUND TRUE) 74 | # the following line skips cpplint (only works in a git repo) 75 | # uncomment the line when this package is not in a git repo 76 | #set(ament_cmake_cpplint_FOUND TRUE) 77 | ament_lint_auto_find_test_dependencies() 78 | endif() 79 | 80 | 81 | set(ROS_LIB_DESTINATION lib) 82 | set(ROS_BIN_DESTINATION bin) 83 | set(ROS_INCLUDE_DESTINATION include) 84 | 85 | install(TARGETS 86 | ${PROJECT_NAME}_ur_comms 87 | ${PROJECT_NAME}_ur_comms_sim 88 | EXPORT ${PACKAGE_NAME}-targets 89 | ARCHIVE DESTINATION lib 90 | LIBRARY DESTINATION lib 91 | RUNTIME DESTINATION lib/${PROJECT_NAME} 92 | ) 93 | 94 | ament_export_interfaces(${PACKAGE_NAME}-targets) 95 | ament_export_dependencies( 96 | ament_cmake 97 | ament_index_cpp 98 | rosidl_default_generators 99 | rclcpp 100 | crs_msgs 101 | std_srvs 102 | std_msgs 103 | ur_dashboard_msgs) 104 | 105 | ament_package() 106 | -------------------------------------------------------------------------------- /crs_robot_comms/README.md: -------------------------------------------------------------------------------- 1 | # CRS Robot Communication Package 2 | 3 | This package is meant to be used to send commands to the UR in order for it to execute a program 4 | -------------------------------------------------------------------------------- /crs_robot_comms/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_robot_comms 5 | 0.0.0 6 | Nodes to simplify robot communication for CRS application 7 | ros-industrial 8 | BSD 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rosidl_default_generators 16 | std_srvs 17 | std_msgs 18 | ur_dashboard_msgs 19 | crs_msgs 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /crs_robot_comms/src/ur_comms_sim.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | static const std::string CONTROLLER_CHANGER_SERVICE = "compliance_controller_on"; 6 | static const std::string TOGGLE_SANDER_SERVICE = "toggle_sander"; 7 | 8 | class URRobotCommsSim : public rclcpp::Node 9 | { 10 | public: 11 | URRobotCommsSim() : Node("ur_robot_comms_sim") 12 | { 13 | // ROS communications 14 | controller_changer_service_ = this->create_service( 15 | CONTROLLER_CHANGER_SERVICE, 16 | std::bind(&URRobotCommsSim::simControllerChangeCB, this, std::placeholders::_1, std::placeholders::_2)); 17 | ur_io_service_ = this->create_service( 18 | TOGGLE_SANDER_SERVICE, 19 | std::bind(&URRobotCommsSim::simUrIoCB, this, std::placeholders::_1, std::placeholders::_2)); 20 | } 21 | 22 | private: 23 | void simControllerChangeCB(std::shared_ptr request, 24 | std::shared_ptr response) 25 | { 26 | response->success = true; 27 | } 28 | void simUrIoCB(std::shared_ptr request, 29 | std::shared_ptr response) 30 | { 31 | response->success = true; 32 | } 33 | rclcpp::Service::SharedPtr controller_changer_service_; 34 | rclcpp::Service::SharedPtr ur_io_service_; 35 | }; 36 | 37 | int main(int argc, char** argv) 38 | { 39 | rclcpp::init(argc, argv); 40 | rclcpp::executors::MultiThreadedExecutor executor; 41 | std::shared_ptr node = std::make_shared(); 42 | executor.add_node(node); 43 | executor.spin(); 44 | rclcpp::shutdown(); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /crs_support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crs_support) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | # the following line skips the linter which checks for copyrights 27 | # uncomment the line when a copyright and license is not present in all source files 28 | #set(ament_cmake_copyright_FOUND TRUE) 29 | # the following line skips cpplint (only works in a git repo) 30 | # uncomment the line when this package is not in a git repo 31 | #set(ament_cmake_cpplint_FOUND TRUE) 32 | ament_lint_auto_find_test_dependencies() 33 | endif() 34 | 35 | install(DIRECTORY 36 | launch 37 | meshes 38 | urdf 39 | sdf 40 | worlds 41 | toolpaths 42 | DESTINATION share/${PROJECT_NAME}/ 43 | ) 44 | 45 | ament_package() 46 | -------------------------------------------------------------------------------- /crs_support/launch/gazebo_swri_demo.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from pathlib import Path 3 | import shutil 4 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 5 | 6 | import launch 7 | import launch_ros.actions 8 | 9 | def generate_launch_description(): 10 | 11 | if not "tesseract_collision" in os.environ["AMENT_PREFIX_PATH"]: 12 | # workaround for pluginlib ClassLoader bug: manually add tesseract_collision to the AMENT_PREFIX_PATH env variable 13 | head, tail = os.path.split(get_package_prefix('crs_support')) 14 | path = os.path.join(head, 'tesseract_collision') 15 | os.environ["AMENT_PREFIX_PATH"] += os.pathsep + path 16 | 17 | urdf = os.path.join(get_package_share_directory('crs_support'), 'urdf', 'swri_demo.urdf') 18 | srdf = os.path.join(get_package_share_directory('crs_support'), 'urdf', 'ur10e_robot.srdf') 19 | gzworld = os.path.join(get_package_share_directory('crs_support'), 'worlds', 'crs.world') 20 | 21 | try: 22 | crs_models_dir = str(Path.home().joinpath('.gazebo', 'models', 'crs_support').resolve(strict=True)) 23 | except FileNotFoundError: #os.path.exists(crs_models_dir): 24 | gazebo_path = str(Path.home().joinpath('.gazebo', 'models').resolve()) 25 | os.mkdir(gazebo_path + "/crs_support") 26 | shutil.copytree(os.path.join(get_package_share_directory('crs_support'), 'meshes'), Path.home().joinpath(Path.home().joinpath('.gazebo','models','crs_support','meshes').resolve())) 27 | 28 | tesseract_env = launch_ros.actions.Node( 29 | node_name='env_node', 30 | package='tesseract_monitoring', 31 | node_executable='tesseract_monitoring_environment_node', 32 | output='screen', 33 | parameters=[{'use_sim_time': 'true', 34 | 'desc_param': 'robot_description', 35 | 'robot_description': urdf, 36 | 'robot_description_semantic': srdf}]) 37 | 38 | gzserver = launch.actions.ExecuteProcess( 39 | cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so', '--world', gzworld], 40 | output='screen' 41 | ) 42 | 43 | spawner1 = launch_ros.actions.Node( 44 | node_name='spawn_node', 45 | package='gazebo_ros', 46 | node_executable='spawn_entity.py', 47 | arguments=['-entity', 'test', '-x', '0', '-y', '0', '-z', '0.05', '-file', urdf]) 48 | 49 | return launch.LaunchDescription([ 50 | # environment 51 | tesseract_env, 52 | 53 | # gazebo 54 | gzserver, 55 | spawner1 56 | 57 | ]) 58 | 59 | -------------------------------------------------------------------------------- /crs_support/launch/gazebo_test.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from pathlib import Path 3 | import shutil 4 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 5 | 6 | import launch 7 | import launch_ros.actions 8 | 9 | def generate_launch_description(): 10 | 11 | if not "tesseract_collision" in os.environ["AMENT_PREFIX_PATH"]: 12 | # workaround for pluginlib ClassLoader bug: manually add tesseract_collision to the AMENT_PREFIX_PATH env variable 13 | head, tail = os.path.split(get_package_prefix('crs_support')) 14 | path = os.path.join(head, 'tesseract_collision') 15 | os.environ["AMENT_PREFIX_PATH"] += os.pathsep + path 16 | 17 | xacro = os.path.join(get_package_share_directory('crs_support'), 'urdf', 'crs.urdf.xacro') 18 | urdf = os.path.join(get_package_share_directory('crs_support'), 'urdf', 'crs.urdf') 19 | srdf = os.path.join(get_package_share_directory('crs_support'), 'urdf', 'ur10e_robot.srdf') 20 | gzworld = os.path.join(get_package_share_directory('crs_support'), 'worlds', 'crs.world') 21 | 22 | try: 23 | crs_models_dir = str(Path.home().joinpath('.gazebo', 'models', 'crs_support').resolve(strict=True)) 24 | except FileNotFoundError: #os.path.exists(crs_models_dir): 25 | gazebo_path = str(Path.home().joinpath('.gazebo', 'models').resolve()) 26 | os.mkdir(gazebo_path + "/crs_support") 27 | shutil.copytree(os.path.join(get_package_share_directory('crs_support'), 'meshes'), Path.home().joinpath(Path.home().joinpath('.gazebo','models','crs_support','meshes').resolve())) 28 | 29 | required_cmd = False 30 | cmd1 = 'ln -s %s %s'%(crs_model_path, gazebo_model_path) 31 | cmd2 = 'xacro %s > %s'%(xacro, urdf) 32 | cmd3 = 'killall -9 gazebo & killall -9 gzserver & killall -9 gzclient' 33 | cmd_dict = {cmd1: True, cmd2 : True, cmd3: False} 34 | 35 | # check if soft link exists 36 | gazebo_model_path = os.path.join(os.environ['HOME'],'.gazebo', 'models', 'crs_support') 37 | crs_model_path = get_package_share_directory('crs_support') 38 | cmd1 = 'ln -s %s %s'%(crs_model_path, gazebo_model_path) 39 | if not os.path.exists(gazebo_model_path): 40 | cmd_dict[cmd1] = True 41 | 42 | # run commands 43 | try: 44 | 45 | for cmd, req_ in cmd_dict.items(): 46 | required_cmd = req_ 47 | print('Running cmd: %s' % (cmd)) 48 | process = subprocess.run(cmd , shell=True, check=True, stdout=subprocess.PIPE, universal_newlines=True) 49 | 50 | except subprocess.CalledProcessError as e: 51 | print(e.output) 52 | if required_cmd: 53 | return None 54 | 55 | 56 | tesseract_env = launch_ros.actions.Node( 57 | node_name='env_node', 58 | package='tesseract_monitoring', 59 | node_executable='tesseract_monitoring_environment_node', 60 | output='screen', 61 | parameters=[{'use_sim_time': 'true', 62 | 'desc_param': 'robot_description', 63 | 'robot_description': urdf, 64 | 'robot_description_semantic': srdf}]) 65 | 66 | gzserver = launch.actions.ExecuteProcess( 67 | cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so', '--world', gzworld], 68 | output='screen' 69 | ) 70 | 71 | spawner1 = launch_ros.actions.Node( 72 | node_name='spawn_node', 73 | package='gazebo_ros', 74 | node_executable='spawn_entity.py', 75 | arguments=['-entity', 'robot', '-x', '0', '-y', '0', '-z', '0.05', '-file', urdf]) 76 | 77 | motion_planning_server = launch_ros.actions.Node( 78 | node_executable='crs_motion_planning_motion_planning_server', 79 | package='crs_motion_planning', 80 | node_name='motion_planning_server', 81 | output='screen', 82 | parameters=[{'urdf_path': urdf, 83 | 'srdf_path': srdf, 84 | 'process_planner_service': "plan_process_motion", 85 | 'freespace_motion_service': "plan_freespace_motion", 86 | 'trajectory_topic': "set_trajectory_test", 87 | 'base_link_frame': "base_link", 88 | 'world_frame': "world", 89 | 'tool0_frame': "tool0", 90 | 'manipulator_group': "manipulator", 91 | 'num_steps': 20, 92 | 'max_joint_velocity': 5.0, 93 | 'min_raster_length': 4, 94 | 'use_gazebo_simulation_time': True, 95 | 'set_trajopt_verbose': False}]) 96 | 97 | return launch.LaunchDescription([ 98 | # environment 99 | tesseract_env, 100 | 101 | # gazebo 102 | gzserver, 103 | spawner1, 104 | 105 | # planning 106 | motion_planning_server, 107 | 108 | ]) 109 | 110 | -------------------------------------------------------------------------------- /crs_support/launch/spawn_cart.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from pathlib import Path 3 | import shutil 4 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 5 | 6 | import launch 7 | import launch_ros.actions 8 | 9 | def generate_launch_description(): 10 | 11 | cart_sdf = os.path.join(get_package_share_directory('crs_support'), 'sdf', 'cart.sdf') 12 | 13 | cart_spawner = launch_ros.actions.Node( 14 | node_name='spawn_node', 15 | package='gazebo_ros', 16 | node_executable='spawn_entity.py', 17 | arguments=['-entity', 'cart', '-x', '0', '-y', '0.2', '-z', '0.05', '-file', cart_sdf]) 18 | 19 | return launch.LaunchDescription([ 20 | cart_spawner 21 | 22 | ]) 23 | 24 | -------------------------------------------------------------------------------- /crs_support/launch/spawn_part.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from pathlib import Path 3 | import shutil 4 | from ament_index_python.packages import get_package_share_directory, get_package_prefix 5 | 6 | import launch 7 | import launch_ros.actions 8 | 9 | def generate_launch_description(): 10 | 11 | part_sdf = os.path.join(get_package_share_directory('crs_support'), 'sdf', 'part1.sdf') 12 | 13 | part_spawner = launch_ros.actions.Node( 14 | node_name='spawn_node', 15 | package='gazebo_ros', 16 | node_executable='spawn_entity.py', 17 | arguments=['-entity', 'part1', '-x', '0', '-y', '0.2', '-z', '0.05', '-file', part_sdf]) 18 | 19 | return launch.LaunchDescription([ 20 | part_spawner 21 | 22 | ]) 23 | 24 | -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/collision/Mockup3_6_Cart_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/collision/Mockup3_6_Cart_collision.stl -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/collision/empty_cart_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/collision/empty_cart_collision.stl -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/collision/mockup_cart_collision_v3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/collision/mockup_cart_collision_v3.stl -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/visual/Mockup3_6_Frame_noTools_small.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/visual/Mockup3_6_Frame_noTools_small.stl -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/visual/empty_cart.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/visual/empty_cart.stl -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/visual/mock_up_frame.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/visual/mock_up_frame.ply -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/visual/mock_up_frame.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/visual/mock_up_frame.stl -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/visual/mockup_cart_v3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/visual/mockup_cart_v3.stl -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/visual/mockup_cart_v4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/visual/mockup_cart_v4.stl -------------------------------------------------------------------------------- /crs_support/meshes/Mockup/visual/mockup_frame_v3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Mockup/visual/mockup_frame_v3.stl -------------------------------------------------------------------------------- /crs_support/meshes/Parts/collision/part1_ch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Parts/collision/part1_ch.stl -------------------------------------------------------------------------------- /crs_support/meshes/Parts/visual/part1_ch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Parts/visual/part1_ch.stl -------------------------------------------------------------------------------- /crs_support/meshes/Parts/visual/part_from_mockup.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Parts/visual/part_from_mockup.stl -------------------------------------------------------------------------------- /crs_support/meshes/Swri_demo/mold_transformed.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/Swri_demo/mold_transformed.stl -------------------------------------------------------------------------------- /crs_support/meshes/end_effector/collision/sander_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/end_effector/collision/sander_collision.stl -------------------------------------------------------------------------------- /crs_support/meshes/end_effector/collision/tool_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/end_effector/collision/tool_collision.stl -------------------------------------------------------------------------------- /crs_support/meshes/end_effector/visual/ee1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/end_effector/visual/ee1.stl -------------------------------------------------------------------------------- /crs_support/meshes/end_effector/visual/ee2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/end_effector/visual/ee2.stl -------------------------------------------------------------------------------- /crs_support/meshes/end_effector/visual/ee3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/end_effector/visual/ee3.stl -------------------------------------------------------------------------------- /crs_support/meshes/end_effector/visual/sander.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/end_effector/visual/sander.stl -------------------------------------------------------------------------------- /crs_support/meshes/end_effector/visual/tool.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/end_effector/visual/tool.stl -------------------------------------------------------------------------------- /crs_support/meshes/ur10e/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/ur10e/collision/base.stl -------------------------------------------------------------------------------- /crs_support/meshes/ur10e/collision/forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/ur10e/collision/forearm.stl -------------------------------------------------------------------------------- /crs_support/meshes/ur10e/collision/shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/ur10e/collision/shoulder.stl -------------------------------------------------------------------------------- /crs_support/meshes/ur10e/collision/upperarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/ur10e/collision/upperarm.stl -------------------------------------------------------------------------------- /crs_support/meshes/ur10e/collision/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/ur10e/collision/wrist1.stl -------------------------------------------------------------------------------- /crs_support/meshes/ur10e/collision/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/ur10e/collision/wrist2.stl -------------------------------------------------------------------------------- /crs_support/meshes/ur10e/collision/wrist3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_support/meshes/ur10e/collision/wrist3.stl -------------------------------------------------------------------------------- /crs_support/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_support 5 | 0.0.0 6 | URDF and other support files for the CRS projects 7 | ros-industrial 8 | BSD 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | xacro 15 | gazebo 16 | gazebo_ros 17 | gazebo_plugins 18 | gazebo_dev 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /crs_support/sdf/cart.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0 -1.571 0 0 5 | 6 | 0 0 0 0 -0 0 7 | 0.1 8 | 9 | 0.03 10 | 0 11 | 0 12 | 0.03 13 | 0 14 | 0.03 15 | 16 | 17 | 18 | 0 0 -0.1168 1.571 -0 0 19 | 20 | 21 | 1 1 1 22 | model://crs_support/meshes/Mockup/collision/empty_cart_collision.stl 23 | 24 | 25 | 26 | 27 | 0 0 -0.1168 1.571 -0 0 28 | 29 | 30 | 1 1 1 31 | model://crs_support/meshes/Mockup/visual/empty_cart.stl 32 | 33 | 34 | 35 | 0.25 0 0 1.0 36 | 0.25 0 0 1.0 37 | 0.25 0 0 1.0 38 | 0.25 0 0 1.0 39 | 40 | 41 | 42 | 43 | cart_link 44 | world 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /crs_support/sdf/part1.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0 -1.571 0 0 5 | 6 | 0 0 0 0 -0 0 7 | 0.1 8 | 9 | 0.03 10 | 0 11 | 0 12 | 0.03 13 | 0 14 | 0.03 15 | 16 | 17 | 18 | 0 0 -0.1168 1.571 -0 0 19 | 20 | 21 | 1 1 1 22 | model://crs_support/meshes/Parts/collision/part_from_mockup.stl 23 | 24 | 25 | 26 | 27 | 0 0 -0.1168 1.571 -0 0 28 | 29 | 30 | 1 1 1 31 | model://crs_support/meshes/Parts/visual/part_from_mockup.stl 32 | 33 | 34 | 35 | 0 0.25 0 1.0 36 | 0 0.25 0 1.0 37 | 0 0.25 0 1.0 38 | 0 0.25 0 1.0 39 | 40 | 41 | 42 | 43 | part_link 44 | world 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /crs_support/urdf/ur10e_robot.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /crs_support/worlds/crs.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | 0.005 15 | 1 16 | 200 17 | 18 | 19 | quick 20 | 0 21 | 22 | 23 | 0.00001 24 | 1.0 25 | 0.0001 26 | 27 | 28 | 29 | 30 | 0 0 0.0 31 | 32 | 34 | 35 | /crs 36 | 37 | 2 38 | 39 | 40 | 41 | 43 | 44 | /crs 45 | 46 | 10 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /crs_toolpath_gen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crs_toolpath_gen) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | # the following line skips the linter which checks for copyrights 27 | # uncomment the line when a copyright and license is not present in all source files 28 | #set(ament_cmake_copyright_FOUND TRUE) 29 | # the following line skips cpplint (only works in a git repo) 30 | # uncomment the line when this package is not in a git repo 31 | #set(ament_cmake_cpplint_FOUND TRUE) 32 | ament_lint_auto_find_test_dependencies() 33 | endif() 34 | 35 | ament_package() 36 | -------------------------------------------------------------------------------- /crs_toolpath_gen/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_toolpath_gen 5 | 0.0.0 6 | The toolpath generation nodes and library for the CRS project 7 | ros-industrial 8 | BSD 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /crs_utils_py/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /crs_utils_py/crs_utils_py/.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__* 2 | -------------------------------------------------------------------------------- /crs_utils_py/crs_utils_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swri-robotics/collaborative-robotic-sanding/3a29df9fa6b4ae17cc05b67e6ba0ae48d5c05e8c/crs_utils_py/crs_utils_py/__init__.py -------------------------------------------------------------------------------- /crs_utils_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crs_utils_py 5 | 0.1.0 6 | Python utilities for ros2 7 | Jorge Nicho 8 | BSD 9 | 10 | rclpy 11 | sensor_msgs 12 | 13 | 15 | ament_copyright 16 | ament_flake8 17 | ament_pep257 18 | python3-pytest 19 | 20 | 21 | ament_python 22 | 23 | 24 | -------------------------------------------------------------------------------- /crs_utils_py/resource/crs_utils_py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /crs_utils_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/crs_utils_py 3 | [install] 4 | install-scripts=$base/lib/crs_utils_py -------------------------------------------------------------------------------- /crs_utils_py/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from glob import glob 3 | from setuptools import find_packages 4 | from setuptools import setup 5 | 6 | package_name = 'crs_utils_py' 7 | 8 | setup( 9 | name=package_name, 10 | version='0.0.0', 11 | # Packages to export 12 | packages=find_packages(), 13 | # Files we want to install, specifically launch files 14 | data_files=[ 15 | ('share/ament_index/resource_index/packages', ['resource/' + package_name]), 16 | # Include our package.xml file 17 | (os.path.join('share', package_name), ['package.xml']), 18 | # Include all launch files. 19 | (os.path.join('share', package_name, 'crs_utils_py'), glob('*.py')) 20 | ], 21 | # This is important as well 22 | install_requires=['setuptools'], 23 | zip_safe=True, 24 | author='ROS 2 Developer', 25 | author_email='ros2@ros.com', 26 | maintainer='ROS 2 Developer', 27 | maintainer_email='ros2@ros.com', 28 | keywords=['ROS', 'ROS2'], 29 | classifiers=[ 30 | 'Intended Audience :: Developers', 31 | 'License :: TODO', 32 | 'Programming Language :: Python', 33 | 'Topic :: Software Development', 34 | ], 35 | description='My awesome package.', 36 | license='TODO', 37 | # Like the CMakeLists add_executable macro, you can add your python 38 | # scripts here. 39 | entry_points={ 40 | 'console_scripts': [ 41 | 'joint_state_publisher = crs_utils_py.joint_state_publisher:main' 42 | ], 43 | }, 44 | ) 45 | -------------------------------------------------------------------------------- /skip.mixin: -------------------------------------------------------------------------------- 1 | { 2 | "build": { 3 | "skip": { 4 | "packages-skip": ["tesseract_python", 5 | "tesseract_viewer_python" 6 | ], 7 | } 8 | } 9 | } 10 | --------------------------------------------------------------------------------