├── .gitignore ├── .gitlab-ci.yml ├── .travis.yml ├── LICENSE ├── README.md ├── snp_demo_gui ├── CMakeLists.txt ├── package.xml ├── plugin.xml └── src │ ├── command.h │ ├── command_widget.cpp │ ├── command_widget.h │ ├── demo_panel.cpp │ ├── demo_panel.h │ ├── demo_widget.cpp │ ├── demo_widget.h │ ├── godel_bridge.cpp │ ├── godel_bridge.h │ └── ui │ ├── command_widget.ui │ └── demo_widget.ui ├── snp_prbt.rosinstall ├── snp_prbt_bringup ├── CMakeLists.txt ├── config │ ├── blending_plan.yaml │ ├── plugins.yaml │ ├── robot_scan.yaml │ ├── scan_plan.yaml │ └── surface_detection.yaml ├── launch │ ├── application_bringup.launch │ ├── hardware_bringup.launch │ ├── prbt_gazebo.launch │ ├── prbt_godel.launch │ ├── prbt_real.launch │ └── rviz.launch ├── package.xml └── rviz │ └── rviz_config.rviz ├── snp_prbt_description ├── CMakeLists.txt ├── meshes │ ├── acf_with_grinder │ │ ├── acf_110_01_xs_with_grinder.STL │ │ └── acf_111_04_with_grinder.STL │ ├── ensenso_n35.STL │ ├── table.stl │ └── tool_fixture.STL ├── models │ └── obj_01 │ │ ├── model.config │ │ └── model.sdf ├── package.xml ├── urdf │ ├── acf_with_grinder │ │ ├── acf_110_01_xs_with_grinder.urdf.xacro │ │ └── acf_111_04_with_grinder.urdf.xacro │ ├── ensenso.urdf.xacro │ ├── prbt_table.urdf.xacro │ ├── snp_prbt_demo.urdf.xacro │ └── snp_tool.urdf.xacro └── worlds │ └── prbt_robot.world ├── snp_prbt_moveit_config ├── .setup_assistant ├── CMakeLists.txt ├── config │ ├── controllers.yaml │ ├── fake_controllers.yaml │ ├── kinematics.yaml │ ├── ompl_planning.yaml │ └── snp_prbt_demo.srdf ├── launch │ ├── command_planner_planning_pipeline.launch.xml │ ├── default_warehouse_db.launch │ ├── demo.launch │ ├── fake_moveit_controller_manager.launch.xml │ ├── joystick_control.launch │ ├── move_group.launch │ ├── moveit.rviz │ ├── moveit_rviz.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── planning_pipeline.launch.xml │ ├── run_benchmark_ompl.launch │ ├── sensor_manager.launch.xml │ ├── setup_assistant.launch │ ├── snp_prbt_description_moveit_controller_manager.launch.xml │ ├── snp_prbt_description_moveit_sensor_manager.launch.xml │ ├── trajectory_execution.launch.xml │ ├── warehouse.launch │ └── warehouse_settings.launch.xml └── package.xml └── snp_prbt_support ├── CMakeLists.txt ├── launch └── setup_camera.launch ├── package.xml ├── scripts ├── ensenso_interface └── set_region_of_interest └── src ├── prbt_blend_process_service_node.cpp └── spindle_sim.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Temporary files 2 | *~ 3 | # Compiled Object files 4 | *.slo 5 | *.lo 6 | *.o 7 | 8 | # Compiled Dynamic libraries 9 | *.so 10 | *.dylib 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | 17 | # Cmake/catkin files 18 | CMakeFiles 19 | Makefile 20 | catkin_generated 21 | *.cmake 22 | CMakeCache.txt 23 | devel 24 | 25 | # Eclipse files 26 | .project 27 | .cproject 28 | 29 | # mongodb instance 30 | default_warehouse_mongo_db 31 | 32 | # editor files 33 | *.autosave -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | image: docker:git 2 | services: 3 | - docker:dind 4 | 5 | variables: 6 | UPSTREAM_WORKSPACE: file 7 | ROSINSTALL_FILENAME: snp_prbt.rosinstall 8 | ROSDEP_SKIP_KEYS: "pilz_modbus pilz_sto_modbus_adapter pilz_trajectory_generation prbt_pg70_support" 9 | BEFORE_SCRIPT: "wget -q https://download.ensenso.com/s/ensensosdk/download?files=ensenso-sdk-2.2.65-x64.deb -O /tmp/ensenso.deb && dpkg -i /tmp/ensenso.deb" 10 | DOCKER_RUN_OPTS: "-e ENSENSO_INSTALL=/opt/ensenso" 11 | ADDITIONAL_DEBS: "pkg-config" 12 | CATKIN_CONFIG: "--no-install" 13 | 14 | before_script: 15 | - apk add --update bash coreutils tar 16 | - git clone --depth=1 https://github.com/ros-industrial/industrial_ci .industrial_ci 17 | kinetic: 18 | script: .industrial_ci/gitlab.sh ROS_DISTRO=kinetic ROS_REPO=ros 19 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | 5 | env: 6 | global: 7 | - UPSTREAM_WORKSPACE=file 8 | - ROSINSTALL_FILENAME=snp_prbt.rosinstall 9 | - ROSDEP_SKIP_KEYS="pilz_modbus pilz_sto_modbus_adapter pilz_trajectory_generation prbt_pg70_support" 10 | - BEFORE_SCRIPT="wget -q https://download.ensenso.com/s/ensensosdk/download?files=ensenso-sdk-2.2.65-x64.deb -O /tmp/ensenso.deb && dpkg -i /tmp/ensenso.deb" 11 | - DOCKER_RUN_OPTS="-e ENSENSO_INSTALL=/opt/ensenso" 12 | - ADDITIONAL_DEBS="pkg-config" 13 | - CATKIN_CONFIG="--no-install" 14 | matrix: 15 | - ROS_DISTRO=kinetic ROS_REPO=ros 16 | 17 | install: 18 | - git clone --depth=1 https://github.com/ros-industrial/industrial_ci.git .ci_config 19 | script: 20 | - .ci_config/travis.sh 21 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Scan and Polish application 2 | [![Build Status](https://travis-ci.com/rosin-project/automatica18_scan_and_plan_demo.svg?branch=master)](https://travis-ci.com/rosin-project/automatica18_scan_and_plan_demo) 3 | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) 4 | 5 | 6 | 7 | >> 3D animation 8 | 9 | 10 | ## Application implements 11 | - `godel` (Scan and Plan): https://github.com/ros-industrial-consortium/godel 12 | - `ensenso_driver`: https://github.com/ensenso/ros_driver ( 13 | rosin_logo 15 | [funded](http://rosin-project.eu/ftps)) 16 | - `pilz_robots`: https://github.com/PilzDE/pilz_robots 17 | - `pilz_industrial_motion`: https://github.com/PilzDE/pilz_industrial_motion ( 18 | rosin_logo 20 | [funded](http://rosin-project.eu/ftps)) 21 | 22 | ## Installation 23 | 24 | ```shell 25 | 26 | # Install Ensenso SDK (from https://www.ensenso.com/support/sdk-download) by: 27 | wget -q https://download.ensenso.com/s/ensensosdk/download?files=ensenso-sdk-2.2.65-x64.deb -O /tmp/ensenso.deb && dpkg -i /tmp/ensenso.deb 28 | 29 | # Create a new ROS workspace 30 | mkdir -p ~/snp_demo_ws/src && cd ~/snp_demo_ws/src 31 | 32 | # Download demo repository 33 | git clone https://github.com/rosin-project/automatica18_scan_and_plan_demo.git 34 | 35 | # Download dependencies 36 | wstool init . 37 | wstool merge ~/snp_demo_ws/src/automatica18_scan_and_plan_demo/snp_prbt.rosinstall 38 | wstool up 39 | 40 | # Reset ROS_PACKAGE_PATH 41 | source /opt/ros/kinetic/setup.bash 42 | 43 | # Install dependencies 44 | rosdep update && rosdep install --from-paths ~/snp_demo_ws/src --ignore-src --skip-keys="pilz_modbus pilz_sto_modbus_adapter prbt_pg70_support" 45 | 46 | 47 | # Build workspace 48 | cd ~/snp_demo_ws && catkin build 49 | 50 | # Source workspace 51 | source ~/snp_demo_ws/devel/setup.bash 52 | 53 | ``` 54 | 55 | 56 | ## Run application 57 | 58 | ```shell 59 | # Simulation: 60 | roslaunch snp_prbt_bringup application_bringup.launch 61 | # Info: Make sure to press play in Gazebo 62 | 63 | # Real robot: 64 | roslaunch snp_prbt_bringup application_bringup.launch sim_robot:=false 65 | # Info: Real robot requires pilz_modbus and pilz_sto_modbus_adapter 66 | 67 | ``` 68 | 69 | ## Scan and Plan: Remove local scan_parameter cache 70 | Make sure to clear possible local `godel_robot_scan_parameters` since they would overwrite the ones of this repo (`snp_prbt_bringup/config/robot_scan.yaml`). 71 | 72 | ```shell 73 | rm -f ~/.ros/godel_robot_scan_parameters.msg 74 | ``` 75 | 76 | ## Systems settings for real devices 77 | 78 | ### Camera: uEye Driver 79 | ``` 80 | wget -q https://download.ensenso.com/s/idsdrivers/download?files=uEye_4.90.0_Linux_64.tgz -O /tmp/ueye.tgz 81 | tar -xvzf /tmp/ueye.tgz -C /tmp 82 | sudo /tmp/ueyesdk-setup-4.90-eth-amd64.gz.run 83 | ``` 84 | 85 | ### Robot: CAN interface 86 | Add in `/etc/network/interfaces` the following config for the [socketcan_interface]( http://wiki.ros.org/socketcan_interface). 87 | 88 | ``` 89 | allow-hotplug can0 90 | iface can0 can static 91 | bitrate 1000000 92 | up ip link set $IFACE txqueuelen 15 93 | ``` 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /snp_demo_gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(snp_demo_gui) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | actionlib 8 | class_loader 9 | godel_msgs 10 | moveit_ros_planning_interface 11 | pluginlib 12 | roscpp 13 | rviz 14 | ) 15 | 16 | find_package(Qt5Widgets REQUIRED) 17 | find_package(Qt5Core REQUIRED) 18 | find_package(Qt5Gui REQUIRED) 19 | 20 | catkin_package() 21 | 22 | set(${PROJECT_NAME}_SRCS 23 | src/command_widget.cpp 24 | src/demo_panel.cpp 25 | src/demo_widget.cpp 26 | src/godel_bridge.cpp 27 | ) 28 | 29 | set(${PROJECT_NAME}_HDRS 30 | src/command_widget.h 31 | src/demo_panel.h 32 | src/demo_widget.h 33 | ) 34 | 35 | set(${PROJECT_NAME}_UIS 36 | src/ui/command_widget.ui 37 | src/ui/demo_widget.ui 38 | ) 39 | 40 | add_definitions(-DQT_NO_KEYWORDS) 41 | qt5_wrap_cpp(${PROJECT_NAME}_MOCS ${${PROJECT_NAME}_HDRS}) 42 | qt5_wrap_ui(${PROJECT_NAME}_UIS_H ${${PROJECT_NAME}_UIS}) 43 | 44 | include_directories(${CMAKE_CURRENT_BINARY_DIR} ${catkin_INCLUDE_DIRS}) 45 | 46 | ## Declare a cpp library 47 | add_library(${PROJECT_NAME} 48 | ${${PROJECT_NAME}_UIS_H} 49 | ${${PROJECT_NAME}_MOCS} 50 | ${${PROJECT_NAME}_SRCS} 51 | ) 52 | 53 | target_link_libraries(${PROJECT_NAME} 54 | ${catkin_LIBRARIES} 55 | Qt5::Widgets 56 | Qt5::Gui 57 | ) 58 | 59 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 60 | 61 | class_loader_hide_library_symbols(${PROJECT_NAME}) 62 | 63 | # Install 64 | install(TARGETS ${PROJECT_NAME} 65 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 68 | 69 | install(FILES plugin.xml 70 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 71 | -------------------------------------------------------------------------------- /snp_demo_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | snp_demo_gui 4 | 0.0.0 5 | SNP demo panel 6 | 7 | Harsh Deshpande 8 | Jonathan Hechtbauer 9 | 10 | Mathias Lüdtke 11 | 12 | Apache 2.0 13 | 14 | catkin 15 | 16 | actionlib 17 | class_loader 18 | godel_msgs 19 | pluginlib 20 | moveit_ros_planning_interface 21 | roscpp 22 | rviz 23 | 24 | actionlib 25 | godel_msgs 26 | pluginlib 27 | moveit_ros_planning_interface 28 | roscpp 29 | rviz 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /snp_demo_gui/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | SNP demo panel 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /snp_demo_gui/src/command.h: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SNP_COMMAND_H 16 | #define SNP_COMMAND_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | namespace snp_demo_gui 23 | { 24 | 25 | using CommandResult = std::tuple; 26 | using CommandLogFunc = std::function; 27 | using CommandFunc = std::function; 28 | } 29 | 30 | #endif // !SNP_COMMAND_H 31 | -------------------------------------------------------------------------------- /snp_demo_gui/src/command_widget.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "command_widget.h" 16 | #include "ui_command_widget.h" 17 | 18 | #include 19 | 20 | namespace snp_demo_gui 21 | { 22 | 23 | CommandWidget::CommandWidget(QString name, CommandFunc func, QWidget *parent) 24 | : QWidget(parent), ui_(new Ui::CommandWidget), name_(name), func_(func) 25 | { 26 | ui_->setupUi(this); 27 | ui_->pushButton->setText(name); 28 | reset(); 29 | connect(ui_->pushButton, &QPushButton::clicked, this, &CommandWidget::start); 30 | connect(this, &CommandWidget::started, this, &CommandWidget::disable); 31 | connect(this, &CommandWidget::failed, this, &CommandWidget::enable); 32 | connect(this, &CommandWidget::succeeded, this, &CommandWidget::enable); 33 | } 34 | 35 | void CommandWidget::start(){ 36 | if(!ui_->pushButton->isEnabled()) return; 37 | Q_EMIT started(); 38 | ui_->label->setText("RUN"); 39 | ui_->label->setStyleSheet("QLabel { background-color: yellow; }"); 40 | QtConcurrent::run([this](){ 41 | CommandResult res = func_([this](const std::string &msg) { Q_EMIT log(QString::fromStdString(msg));}); 42 | if (std::get<0>(res)){ 43 | ui_->label->setText("OK"); 44 | ui_->label->setStyleSheet("QLabel { background-color: green; }"); 45 | Q_EMIT succeeded(); 46 | }else{ 47 | ui_->label->setText("ERR"); 48 | ui_->label->setStyleSheet("QLabel { background-color: red; }"); 49 | Q_EMIT failed(QString::fromStdString(std::get<1>(res))); 50 | } 51 | }); 52 | } 53 | 54 | void CommandWidget::reset() { 55 | ui_->label->setText(""); 56 | ui_->label->setStyleSheet(""); 57 | disable(); 58 | } 59 | void CommandWidget::disable() { 60 | ui_->pushButton->setEnabled(false); 61 | } 62 | void CommandWidget::enable() { 63 | ui_->pushButton->setEnabled(true); 64 | } 65 | CommandWidget::~CommandWidget() = default; 66 | 67 | } 68 | -------------------------------------------------------------------------------- /snp_demo_gui/src/command_widget.h: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SNP_COMMAND_WIDGET_H 16 | #define SNP_COMMAND_WIDGET_H 17 | 18 | #include 19 | #include 20 | 21 | #include "command.h" 22 | 23 | namespace Ui 24 | { 25 | class CommandWidget; 26 | } 27 | 28 | namespace snp_demo_gui 29 | { 30 | 31 | class CommandWidget : public QWidget 32 | { 33 | Q_OBJECT 34 | public: 35 | CommandWidget(QString name, CommandFunc func, QWidget* parent = 0); 36 | virtual ~CommandWidget(); 37 | QString getName() const { return name_; } 38 | 39 | public Q_SLOTS: 40 | void disable(); 41 | void enable(); 42 | void start(); 43 | void reset(); 44 | 45 | Q_SIGNALS: 46 | void started(); 47 | void succeeded(); 48 | void failed(QString); 49 | void log(QString); 50 | 51 | private: 52 | std::unique_ptr ui_; 53 | QString name_; 54 | CommandFunc func_; 55 | }; 56 | } 57 | 58 | #endif // !SNP_COMMAND_WIDGET_H 59 | -------------------------------------------------------------------------------- /snp_demo_gui/src/demo_panel.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "demo_panel.h" 16 | 17 | #include "demo_widget.h" 18 | #include 19 | 20 | namespace snp_demo_gui { 21 | 22 | DemoPanel::DemoPanel( QWidget* parent) 23 | : rviz::Panel(parent) { 24 | QVBoxLayout* layout = new QVBoxLayout(this); 25 | widget_ = new DemoWidget(); 26 | layout->addWidget(widget_); 27 | setLayout(layout); 28 | } 29 | 30 | } 31 | 32 | #include 33 | PLUGINLIB_EXPORT_CLASS(snp_demo_gui::DemoPanel, rviz::Panel ) 34 | -------------------------------------------------------------------------------- /snp_demo_gui/src/demo_panel.h: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SNP_DEMO_PANEL_H 16 | #define SNP_DEMO_PANEL_H 17 | 18 | #include 19 | 20 | namespace snp_demo_gui { 21 | 22 | class DemoWidget; 23 | 24 | class DemoPanel : public rviz::Panel { 25 | Q_OBJECT 26 | public: 27 | DemoPanel( QWidget* parent = 0 ); 28 | private: 29 | DemoWidget *widget_; 30 | }; 31 | 32 | } 33 | #endif // SNP_DEMO_PANEL_H 34 | -------------------------------------------------------------------------------- /snp_demo_gui/src/demo_widget.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "demo_widget.h" 16 | #include "ui_demo_widget.h" 17 | 18 | #include "command_widget.h" 19 | 20 | #include 21 | 22 | namespace snp_demo_gui 23 | { 24 | 25 | enum class LoopMode : int { 26 | StepMode, 27 | UntilEnd, 28 | LoopAll, 29 | LoopExecute 30 | }; 31 | 32 | DemoWidget::DemoWidget(QWidget *parent) 33 | : QWidget(parent), ui_(new Ui::DemoWidget) 34 | { 35 | ui_->setupUi(this); 36 | 37 | CommandWidget *home = setupCmd("Home", [this](CommandLogFunc log)-> CommandResult {return bridge_.home(log);}, true); 38 | CommandWidget *scan = setupCmd("Scan", [this](CommandLogFunc log)-> CommandResult {return bridge_.scan(log);}); 39 | CommandWidget *plan = setupCmd("Plan", std::bind(&GodelBridge::plan, &bridge_, std::placeholders::_1)); 40 | CommandWidget *simulate = setupCmd("Simulate", std::bind(&GodelBridge::simulate, &bridge_, std::placeholders::_1)); 41 | CommandWidget *execute = setupCmd("Execute", std::bind(&GodelBridge::execute, &bridge_, std::placeholders::_1)); 42 | 43 | connect(plan, &CommandWidget::failed, this, [this,scan]() { 44 | setNext(scan); 45 | if(ui_->modeComboBox->currentIndex()) { 46 | next_->start(); 47 | } 48 | }); 49 | 50 | connect(ui_->simulateCheckBox, &QCheckBox::clicked, simulate, &CommandWidget::setVisible); 51 | 52 | setNext(commands_.front()); 53 | connect(ui_->stepButton, &QPushButton::clicked, this, [this]() { next_->start(); }); 54 | } 55 | 56 | DemoWidget::~DemoWidget() = default; 57 | 58 | void DemoWidget::setNext(CommandWidget* cmd){ 59 | next_ = cmd; 60 | ui_->stepButton->setText(next_->getName()); 61 | next_->enable(); 62 | } 63 | 64 | bool DemoWidget::selectNext(CommandWidget* cmd){ 65 | bool overflow = cmd == commands_.back(); 66 | CommandWidget* next = overflow ? commands_.front() : *(++(std::find(commands_.begin(), commands_.end(), cmd))); 67 | if( !next->isVisible()) { 68 | return selectNext(next) || overflow; 69 | } 70 | setNext(next); 71 | return overflow; 72 | } 73 | 74 | CommandWidget* DemoWidget::setupCmd(QString name, CommandFunc func, bool clear){ 75 | CommandWidget* cmd = new CommandWidget(name, func); 76 | 77 | clear |= commands_.empty(); 78 | 79 | for(CommandWidget* c: commands_){ 80 | connect(c, &CommandWidget::started, cmd, &CommandWidget::reset); 81 | connect(cmd, &CommandWidget::started, c, &CommandWidget::disable); 82 | connect(cmd, &CommandWidget::failed, c, &CommandWidget::enable); 83 | connect(cmd, &CommandWidget::succeeded, c, &CommandWidget::enable); 84 | } 85 | commands_.push_back(cmd); 86 | 87 | ui_->commandsLayout->addWidget(cmd); 88 | 89 | if(clear) connect(cmd, &CommandWidget::started, ui_->log, &QTextEdit::clear); 90 | connect(cmd, &CommandWidget::started, this, [this,name]() { 91 | ui_->stepButton->setEnabled(false); 92 | ui_->log->append(QString("%1: started").arg(name)); 93 | }); 94 | 95 | connect(cmd, &CommandWidget::succeeded, this, [this,name, cmd]() { 96 | ui_->log->append(QString("%1: finished").arg(name)); 97 | bool overflow = selectNext(cmd); 98 | bool run = false; 99 | switch(static_cast(ui_->modeComboBox->currentIndex())) { 100 | case LoopMode::UntilEnd: 101 | run = !overflow; 102 | break; 103 | case LoopMode::LoopExecute: 104 | if(overflow) { 105 | setNext(commands_.back()); 106 | } 107 | // no break 108 | case LoopMode::LoopAll: 109 | run = true; 110 | break; 111 | default: 112 | break; 113 | }; 114 | if(run) { 115 | next_->start(); 116 | } else { 117 | ui_->stepButton->setEnabled(true); 118 | } 119 | }); 120 | 121 | connect(cmd, &CommandWidget::log, this, [this, name](QString s) { 122 | ui_->log->append(QString("%1: %2").arg(name).arg(s)); 123 | }); 124 | 125 | connect(cmd, &CommandWidget::failed, this, [this, name, cmd](QString s) { 126 | setNext(cmd); 127 | ui_->stepButton->setEnabled(true); 128 | if(!s.isEmpty()) ui_->log->append(QString("%1: failed with error '%2'").arg(name).arg(s)); 129 | else ui_->log->append(QString("%1: failed with unkown error").arg(name)); 130 | }); 131 | 132 | return cmd; 133 | }; 134 | 135 | } 136 | -------------------------------------------------------------------------------- /snp_demo_gui/src/demo_widget.h: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SNP_DEMO_WIDGET_H 16 | #define SNP_DEMO_WIDGET_H 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "godel_bridge.h" 23 | #include "command_widget.h" 24 | 25 | namespace Ui 26 | { 27 | class DemoWidget; 28 | } 29 | 30 | namespace snp_demo_gui 31 | { 32 | 33 | class GodelCommand; 34 | 35 | class DemoWidget : public QWidget 36 | { 37 | Q_OBJECT 38 | public: 39 | DemoWidget(QWidget* parent = 0); 40 | virtual ~DemoWidget(); 41 | private: 42 | using CommandArray = std::vector; 43 | CommandArray commands_; 44 | CommandWidget* next_; 45 | CommandWidget* setupCmd(QString name, CommandFunc func, bool clear=false); 46 | bool selectNext(CommandWidget* cmd); 47 | void setNext(CommandWidget* cmd); 48 | std::unique_ptr ui_; 49 | GodelBridge bridge_; 50 | }; 51 | } 52 | 53 | #endif // !SNP_DEMO_WIDGET_H 54 | -------------------------------------------------------------------------------- /snp_demo_gui/src/godel_bridge.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "godel_bridge.h" 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "godel_msgs/SurfaceDetection.h" 22 | #include "godel_msgs/SelectSurface.h" 23 | 24 | #include 25 | #include 26 | 27 | #include "godel_msgs/GetAvailableMotionPlans.h" 28 | #include "godel_msgs/SelectMotionPlanAction.h" 29 | 30 | namespace snp_demo_gui { 31 | 32 | CommandResult makeError(const std::string &e) { 33 | ROS_ERROR_STREAM(e); 34 | return CommandResult(false,e); 35 | } 36 | CommandResult makeActionError(const actionlib::SimpleClientGoalState &state) { 37 | std::stringstream sstr; 38 | sstr << state.toString(); 39 | std::string text = state.getText(); 40 | if(!text.empty()) sstr << " - " << text; 41 | return makeError(sstr.str()); 42 | } 43 | 44 | CommandResult run_scan(CommandLogFunc log) { 45 | godel_msgs::SurfaceDetection srv; 46 | srv.request.action = srv.request.SCAN_AND_FIND_ONLY; 47 | srv.request.use_default_parameters = true; 48 | if (!ros::service::call("surface_detection", srv)) return makeError("Unable to call surface detection service"); 49 | else if (!srv.response.surfaces_found) return makeError("No surface found"); 50 | 51 | godel_msgs::SelectSurface srv2; 52 | srv2.request.action = srv2.request.SELECT_ALL; 53 | if (!ros::service::call("select_surface", srv2)) return makeError("Unable to call surface selection service"); 54 | return CommandResult(srv2.response.succeeded , srv2.response.succeeded ? "": "Could not select_surface"); 55 | } 56 | 57 | class GodelBridgeImpl { 58 | ros::NodeHandle nh_; 59 | using ProcessPlanningActionClient = actionlib::SimpleActionClient; 60 | ProcessPlanningActionClient process_planning_action_client_; 61 | using SelectMotionPlanActionClient = actionlib::SimpleActionClient; 62 | SelectMotionPlanActionClient select_motion_plan_action_client_; 63 | std::vector plans_; 64 | moveit::planning_interface::MoveGroupInterface move_group_; 65 | public: 66 | GodelBridgeImpl() 67 | : process_planning_action_client_("process_planning_as"), 68 | select_motion_plan_action_client_("select_motion_plan_as"), 69 | move_group_(nh_.param("/godel_process_planning/blend_group", std::string("manipulator_tcp"))) 70 | { 71 | } 72 | 73 | CommandResult home(CommandLogFunc log) { 74 | move_group_.setPlannerId("PTP"); 75 | move_group_.setNamedTarget("home"); 76 | move_group_.setStartStateToCurrentState(); 77 | moveit::planning_interface::MoveItErrorCode eCode = move_group_.move(); 78 | if(!eCode) { 79 | std::stringstream sstr; 80 | sstr << eCode; 81 | return makeError(sstr.str()); 82 | } 83 | return CommandResult(true, ""); 84 | } 85 | 86 | CommandResult plan(CommandLogFunc log) { 87 | plans_.clear(); 88 | godel_msgs::ProcessPlanningGoal goal; 89 | goal.action = goal.GENERATE_MOTION_PLAN_AND_PREVIEW; 90 | goal.use_default_parameters = true; 91 | process_planning_action_client_.sendGoal( 92 | goal, 0, 0, 93 | [log](const godel_msgs::ProcessPlanningFeedbackConstPtr& feedback) { log(feedback->last_completed); } 94 | ); 95 | if(!process_planning_action_client_.waitForServer(ros::Duration(1.0))) return makeError("Cannot reach planning server"); 96 | if(!process_planning_action_client_.waitForResult()) return makeError("Timeout while waiting for result"); 97 | actionlib::SimpleClientGoalState state = process_planning_action_client_.getState(); 98 | if(state != actionlib::SimpleClientGoalState::SUCCEEDED) return makeActionError(state); 99 | 100 | if(!process_planning_action_client_.getResult()->succeeded) return makeError(state.getText()); 101 | 102 | godel_msgs::GetAvailableMotionPlans srv; 103 | if (!ros::service::call("get_available_motion_plans", srv)) return makeError("Unable to call get_available_motion_plans service"); 104 | 105 | plans_ = srv.response.names; 106 | if(plans_.empty()) return makeError("No motion plans available"); 107 | 108 | std::stringstream sstr; 109 | sstr << "Plan(s):"; 110 | for(const std::string &s: plans_) sstr << " " << s; 111 | log(sstr.str()); 112 | 113 | return CommandResult(true, ""); 114 | } 115 | 116 | CommandResult runPlans(CommandLogFunc log, bool simulate) { 117 | if(plans_.empty()) return makeError("No motion plans available"); 118 | 119 | godel_msgs::SelectMotionPlanGoal goal; 120 | goal.simulate = simulate; 121 | goal.wait_for_execution = true; 122 | 123 | for(const std::string &plan: plans_){ 124 | log("Run " + plan); 125 | goal.name = plan; 126 | 127 | if(!select_motion_plan_action_client_.waitForServer(ros::Duration(1.0))) return makeError("Cannot reach motion server"); 128 | 129 | actionlib::SimpleClientGoalState state = select_motion_plan_action_client_.sendGoalAndWait(goal); 130 | 131 | switch(select_motion_plan_action_client_.getResult()->code) { 132 | case godel_msgs::SelectMotionPlanResult::NO_SUCH_NAME: 133 | return makeError("no such name"); 134 | case godel_msgs::SelectMotionPlanResult::TIMEOUT: 135 | return makeError("timed out"); 136 | case godel_msgs::SelectMotionPlanResult::SUCCESS: 137 | if(state == actionlib::SimpleClientGoalState::SUCCEEDED) continue; 138 | // else fall-through 139 | default: 140 | return makeActionError(state); 141 | } 142 | } 143 | return CommandResult(true, ""); 144 | } 145 | }; 146 | 147 | GodelBridge::GodelBridge() 148 | : impl_(new GodelBridgeImpl()) 149 | {} 150 | 151 | CommandResult GodelBridge::home(CommandLogFunc log) { return impl_->home(log); } 152 | CommandResult GodelBridge::scan(CommandLogFunc log) { return run_scan(log); } 153 | CommandResult GodelBridge::plan(CommandLogFunc log) { return impl_->plan(log); } 154 | CommandResult GodelBridge::simulate(CommandLogFunc log) { return impl_->runPlans(log, true); } 155 | CommandResult GodelBridge::execute(CommandLogFunc log) { return impl_->runPlans(log, false); } 156 | 157 | GodelBridge::~GodelBridge() = default; 158 | 159 | } 160 | -------------------------------------------------------------------------------- /snp_demo_gui/src/godel_bridge.h: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SNP_GODEL_BRIDGE_H 16 | #define SNP_GODEL_BRIDGE_H 17 | 18 | #include 19 | #include "command.h" 20 | 21 | namespace snp_demo_gui 22 | { 23 | 24 | class GodelBridgeImpl; 25 | 26 | class GodelBridge 27 | { 28 | public: 29 | GodelBridge(); 30 | 31 | CommandResult home(CommandLogFunc); 32 | CommandResult scan(CommandLogFunc); 33 | CommandResult plan(CommandLogFunc); 34 | CommandResult simulate(CommandLogFunc); 35 | CommandResult execute(CommandLogFunc); 36 | 37 | virtual ~GodelBridge(); 38 | private: 39 | std::unique_ptr impl_; 40 | }; 41 | } 42 | 43 | #endif // !SNP_GODEL_BRIDGE_H 44 | -------------------------------------------------------------------------------- /snp_demo_gui/src/ui/command_widget.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | CommandWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 136 10 | 46 11 | 12 | 13 | 14 | 15 | 0 16 | 0 17 | 18 | 19 | 20 | 21 | 106 22 | 0 23 | 24 | 25 | 26 | Form 27 | 28 | 29 | 30 | 31 | 32 | 33 | 0 34 | 0 35 | 36 | 37 | 38 | PushButton 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 30 47 | 30 48 | 49 | 50 | 51 | ERR 52 | 53 | 54 | Qt::AlignCenter 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /snp_demo_gui/src/ui/demo_widget.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | DemoWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 280 10 | 497 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | true 24 | 25 | 26 | 27 | 28 | 29 | 30 | Step 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 0 41 | 0 42 | 43 | 44 | 45 | Simulate 46 | 47 | 48 | true 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | Manual 57 | 58 | 59 | 60 | 61 | One cycle 62 | 63 | 64 | 65 | 66 | Loop all 67 | 68 | 69 | 70 | 71 | Keep executing 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /snp_prbt.rosinstall: -------------------------------------------------------------------------------- 1 | # Required for godel 2 | - git: {local-name: godel_openvoronoi, uri: 'https://github.com/ros-industrial-consortium/godel_openvoronoi.git', version: c219960} 3 | - git: {local-name: godel, uri: 'https://github.com/ipa-hsd/godel.git', version: automatica18} 4 | - git: {local-name: industrial_core, uri: 'https://github.com/ros-industrial/industrial_core.git', version: 3db06b0} 5 | - git: {local-name: keyence_experimental, uri: 'https://github.com/ros-industrial/keyence_experimental.git', version: adc1bc6} 6 | - git: {local-name: libsocket, uri: 'https://github.com/dermesser/libsocket.git', version: 66cc79} 7 | - git: {local-name: swri_profiler, uri: 'https://github.com/swri-robotics/swri_profiler.git', version: 71ae85e} 8 | - git: {local-name: descartes, uri: 'https://github.com/ipa-hsd/descartes.git', version: automatica18} 9 | # Required for this implementation 10 | - git: {local-name: ensenso_ros_driver, uri: 'https://github.com/ensenso/ros_driver.git', version: aa2f899} 11 | - git: {local-name: pilz_robots, uri: 'https://github.com/PilzDE/pilz_robots', version: 8b529fe} 12 | - git: {local-name: pilz_industrial_motion, uri: 'https://github.com/PilzDE/pilz_industrial_motion', version: 783a6f5} 13 | - git: {local-name: code_coverage, uri: 'https://github.com/mikeferguson/code_coverage', version: 9793649} 14 | -------------------------------------------------------------------------------- /snp_prbt_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(snp_prbt_bringup) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch config rviz 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | -------------------------------------------------------------------------------- /snp_prbt_bringup/config/blending_plan.yaml: -------------------------------------------------------------------------------- 1 | blending_plan: 2 | tool_radius: 0.0125 # (m) actual radius of blending tool 3 | margin: 0.001 # (m) additional distance to leave near surface boundaries 4 | overlap: 0.00 # (m) overlap distance between adjacent paths 5 | approach_spd: 0.005 # (m/s) Speed to approach surface of part 6 | blending_spd: 0.3 # (m/s) Speed to perform blending 7 | retract_spd: 0.02 # (m/s) Speed to move away from surface of part 8 | traverse_spd: 0.3 # (m/s) Speed to travel while at traverse height 9 | discretization: 0.0025 # (m) How densely to space adjacent Cartesian points 10 | safe_traverse_height: 0.05 # (m) height above surface to rise during rapid traversal moves 11 | min_boundary_length: .03 # (m) Boundaries below threshold are ignored during process path creation 12 | tool_force: 0.0 # (kg) 13 | spindle_speed: 0.0 # (rad/s) -------------------------------------------------------------------------------- /snp_prbt_bringup/config/plugins.yaml: -------------------------------------------------------------------------------- 1 | meshing_plugin_name: "concave_hull_mesher::ConcaveHullMesher" 2 | blend_tool_planning_plugin_name: "path_planning_plugins::openveronoi::BlendPlanner" 3 | scan_tool_planning_plugin_name: "path_planning_plugins::openveronoi::ScanPlanner" 4 | -------------------------------------------------------------------------------- /snp_prbt_bringup/config/robot_scan.yaml: -------------------------------------------------------------------------------- 1 | robot_scan: 2 | group_name: manipulator_ensenso 3 | move_scan_vel_scaling: 0.3 4 | home_position: home_asus 5 | world_frame: world_frame 6 | tcp_frame: ensenso_move_frame 7 | planner_id: PTP 8 | tcp_to_cam_pose: 9 | trans: 10 | x: 0 11 | y: 0 12 | z: 0 13 | quat: 14 | x: 0 15 | y: 0 16 | z: 0 17 | w: 1 18 | world_to_obj_pose: 19 | trans: 20 | x: 0.0 21 | y: -0.4 22 | z: 0.18 23 | quat: 24 | x: 0 25 | y: 0 26 | z: 0 27 | w: 1 28 | cam_to_obj_zoffset: 0.4 29 | cam_to_obj_xoffset: 0.065 30 | cam_tilt_angle: 1.57 31 | sweep_angle_start: 0 32 | sweep_angle_end: 6.28 33 | scan_topic: /point_cloud_workspace 34 | scan_target_frame: world_frame 35 | reachable_scan_points_ratio: 1.0 36 | stop_on_planning_error: true 37 | num_scan_points: 4 38 | -------------------------------------------------------------------------------- /snp_prbt_bringup/config/scan_plan.yaml: -------------------------------------------------------------------------------- 1 | scan_plan: 2 | scan_width: 0.01 3 | traverse_speed: 0.2 4 | approach_distance: 0.15 5 | overlap: 0.001 6 | margin: 0 7 | quality_metric: "rms" 8 | window_width: 0.02 9 | min_qa_value: 0.05 10 | max_qa_value: 0.05 11 | -------------------------------------------------------------------------------- /snp_prbt_bringup/config/surface_detection.yaml: -------------------------------------------------------------------------------- 1 | surface_detection: 2 | frame_id: "world_frame" 3 | k_search: 20 4 | marker_alpha: 1 5 | 6 | meanK: 10 7 | stdv_threshold: 4 8 | 9 | rg_min_cluster_size: 100 10 | rg_max_cluster_size: 10000 11 | rg_neighbors: 20 12 | rg_smoothness_threshold: 0.07853981633974483 13 | rg_curvature_threshold: 2.0 14 | 15 | stout_mean: 1.0 16 | stout_stdev_threshold: 3.0 17 | 18 | voxel_leaf: 0.005 19 | tabletop_seg_distance_thresh: 0.005 20 | use_tabletop_segmentation: False 21 | ignore_largest_cluster: False 22 | 23 | mls_point_density: 100 24 | mls_upsampling_radius: 0.01 25 | mls_search_radius: 0.04 26 | 27 | pa_enabled: True 28 | pa_seg_max_iterations: 200 29 | pa_seg_dist_threshold: 0.01 30 | pa_sac_plane_distance: 0.01 31 | pa_kdtree_radius: 0.01 32 | 33 | use_octomap: False 34 | occupancy_threshold: 0.5 35 | -------------------------------------------------------------------------------- /snp_prbt_bringup/launch/application_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /snp_prbt_bringup/launch/hardware_bringup.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /snp_prbt_bringup/launch/prbt_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 24 | 25 | 26 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /snp_prbt_bringup/launch/prbt_godel.launch: -------------------------------------------------------------------------------- 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 | 34 | 35 | 36 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | min_x: -0.2 52 | max_x: 0.2 53 | min_y: -0.6 54 | max_y: -0.2 55 | min_z: 0.10 56 | max_z: 0.25 57 | input_frame: world_frame 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /snp_prbt_bringup/launch/prbt_real.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /snp_prbt_bringup/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /snp_prbt_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | snp_prbt_bringup 4 | 0.0.0 5 | The snp_prbt_bringup package 6 | 7 | Harsh Deshpande 8 | Jonathan Hechtbauer 9 | 10 | Apache License 2.0 11 | 12 | catkin 13 | 14 | controller_manager 15 | gazebo_ros 16 | godel_process_planning 17 | godel_surface_detection 18 | industrial_robot_simulator_service 19 | joint_state_controller 20 | joint_trajectory_controller 21 | meshing_plugins 22 | nodelet 23 | path_planning_plugins 24 | pcl_ros 25 | pilz_modbus 26 | pilz_sto_modbus_adapter 27 | position_controllers 28 | prbt_moveit_config 29 | prbt_support 30 | snp_prbt_moveit_config 31 | roslaunch 32 | rviz 33 | snp_prbt_description 34 | snp_prbt_support 35 | velocity_controllers 36 | xacro 37 | 38 | 39 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /snp_prbt_bringup/rviz/rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: godel_simple_gui/BlendingPanel 3 | Name: BlendingPanel 4 | - Class: rviz/Displays 5 | Help Height: 78 6 | Name: Displays 7 | Property Tree Widget: 8 | Expanded: 9 | - /SimulatedRobot1 10 | - /TF1/Frames1 11 | - /PointCloud21 12 | Splitter Ratio: 0.5 13 | Tree Height: 930 14 | - Class: godel_plugins/RobotBlendingPanel 15 | Name: RobotBlendingPanel 16 | TextEntry: test_field 17 | - Class: snp_demo_gui::DemoPanel 18 | Name: DemoPanel 19 | - Class: rviz/Selection 20 | Name: Selection 21 | Visualization Manager: 22 | Class: "" 23 | Displays: 24 | - Class: rviz/InteractiveMarkers 25 | Enable Transparency: true 26 | Enabled: false 27 | Name: EligibleSurfaces 28 | Show Axes: false 29 | Show Descriptions: true 30 | Show Visual Aids: false 31 | Update Topic: /surface_marker_server/update 32 | Value: false 33 | - Alpha: 0.400000006 34 | Autocompute Intensity Bounds: true 35 | Autocompute Value Bounds: 36 | Max Value: 10 37 | Min Value: -10 38 | Value: true 39 | Axis: Z 40 | Channel Name: intensity 41 | Class: rviz/PointCloud2 42 | Color: 255; 255; 255 43 | Color Transformer: Intensity 44 | Decay Time: 0 45 | Enabled: false 46 | Invert Rainbow: false 47 | Max Color: 255; 255; 255 48 | Max Intensity: 4096 49 | Min Color: 0; 0; 0 50 | Min Intensity: 0 51 | Name: Macro Scan Cloud 52 | Position Transformer: XYZ 53 | Queue Size: 1 54 | Selectable: false 55 | Size (Pixels): 1 56 | Size (m): 0.00999999978 57 | Style: Points 58 | Topic: /sensor_point_cloud 59 | Unreliable: false 60 | Use Fixed Frame: true 61 | Use rainbow: true 62 | Value: false 63 | - Alpha: 1 64 | Arrow Length: 0.0500000007 65 | Axes Length: 0.100000001 66 | Axes Radius: 0.00999999978 67 | Class: rviz/PoseArray 68 | Color: 0; 255; 255 69 | Enabled: true 70 | Head Length: 0.0700000003 71 | Head Radius: 0.0299999993 72 | Name: Camera Image Points 73 | Shaft Length: 0.230000004 74 | Shaft Radius: 0.00999999978 75 | Shape: Arrow (Flat) 76 | Topic: robot_scan_path_preview 77 | Unreliable: false 78 | Value: true 79 | - Alpha: 1 80 | Class: rviz/RobotModel 81 | Collision Enabled: false 82 | Enabled: true 83 | Links: 84 | All Links Enabled: true 85 | Expand Joint Details: false 86 | Expand Link Details: false 87 | Expand Tree: false 88 | Link Tree Style: Links in Alphabetic Order 89 | acf_111_04_and_grinder: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | prbt_base_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | ensenso: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | ensenso_frame: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | ensenso_move_frame: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | ensenso_optical_frame: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | ensenso_optical_frame_sim: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | ensenso_sensor_optical_frame: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | polish_disc: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | prbt_flange: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | prbt_link_1: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | prbt_link_2: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | Value: true 143 | prbt_link_3: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | Value: true 148 | prbt_link_4: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | prbt_link_5: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | Value: true 158 | prbt_link_foot: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | prbt_tcp: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | table: 168 | Alpha: 1 169 | Show Axes: false 170 | Show Trail: false 171 | Value: true 172 | tcp_frame: 173 | Alpha: 1 174 | Show Axes: false 175 | Show Trail: false 176 | prbt_tool0: 177 | Alpha: 1 178 | Show Axes: false 179 | Show Trail: false 180 | tool_fixture: 181 | Alpha: 1 182 | Show Axes: false 183 | Show Trail: false 184 | Value: true 185 | tool_stroke: 186 | Alpha: 1 187 | Show Axes: false 188 | Show Trail: false 189 | Value: true 190 | world_frame: 191 | Alpha: 1 192 | Show Axes: false 193 | Show Trail: false 194 | Name: ActualRobot 195 | Robot Description: robot_description 196 | TF Prefix: "" 197 | Update Interval: 0 198 | Value: true 199 | Visual Enabled: true 200 | - Alpha: 0.300000012 201 | Class: rviz/RobotModel 202 | Collision Enabled: false 203 | Enabled: true 204 | Links: 205 | All Links Enabled: true 206 | Expand Joint Details: false 207 | Expand Link Details: false 208 | Expand Tree: false 209 | Link Tree Style: Links in Alphabetic Order 210 | acf_111_04_and_grinder: 211 | Alpha: 1 212 | Show Axes: false 213 | Show Trail: false 214 | Value: true 215 | prbt_base_link: 216 | Alpha: 1 217 | Show Axes: false 218 | Show Trail: false 219 | ensenso: 220 | Alpha: 1 221 | Show Axes: false 222 | Show Trail: false 223 | Value: true 224 | ensenso_frame: 225 | Alpha: 1 226 | Show Axes: false 227 | Show Trail: false 228 | ensenso_move_frame: 229 | Alpha: 1 230 | Show Axes: false 231 | Show Trail: false 232 | ensenso_optical_frame: 233 | Alpha: 1 234 | Show Axes: false 235 | Show Trail: false 236 | ensenso_optical_frame_sim: 237 | Alpha: 1 238 | Show Axes: false 239 | Show Trail: false 240 | ensenso_sensor_optical_frame: 241 | Alpha: 1 242 | Show Axes: false 243 | Show Trail: false 244 | polish_disc: 245 | Alpha: 1 246 | Show Axes: false 247 | Show Trail: false 248 | Value: true 249 | prbt_flange: 250 | Alpha: 1 251 | Show Axes: false 252 | Show Trail: false 253 | Value: true 254 | prbt_link_1: 255 | Alpha: 1 256 | Show Axes: false 257 | Show Trail: false 258 | Value: true 259 | prbt_link_2: 260 | Alpha: 1 261 | Show Axes: false 262 | Show Trail: false 263 | Value: true 264 | prbt_link_3: 265 | Alpha: 1 266 | Show Axes: false 267 | Show Trail: false 268 | Value: true 269 | prbt_link_4: 270 | Alpha: 1 271 | Show Axes: false 272 | Show Trail: false 273 | Value: true 274 | prbt_link_5: 275 | Alpha: 1 276 | Show Axes: false 277 | Show Trail: false 278 | Value: true 279 | prbt_link_foot: 280 | Alpha: 1 281 | Show Axes: false 282 | Show Trail: false 283 | Value: true 284 | prbt_tcp: 285 | Alpha: 1 286 | Show Axes: false 287 | Show Trail: false 288 | table: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | tcp_frame: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | prbt_tool0: 298 | Alpha: 1 299 | Show Axes: false 300 | Show Trail: false 301 | tool_fixture: 302 | Alpha: 1 303 | Show Axes: false 304 | Show Trail: false 305 | Value: true 306 | tool_stroke: 307 | Alpha: 1 308 | Show Axes: false 309 | Show Trail: false 310 | Value: true 311 | world_frame: 312 | Alpha: 1 313 | Show Axes: false 314 | Show Trail: false 315 | Name: SimulatedRobot 316 | Robot Description: robot_description 317 | TF Prefix: simulation 318 | Update Interval: 0 319 | Value: true 320 | Visual Enabled: true 321 | - Alpha: 1 322 | Autocompute Intensity Bounds: true 323 | Autocompute Value Bounds: 324 | Max Value: 10 325 | Min Value: -10 326 | Value: true 327 | Axis: Z 328 | Channel Name: intensity 329 | Class: rviz/PointCloud2 330 | Color: 255; 255; 255 331 | Color Transformer: RGB8 332 | Decay Time: 0 333 | Enabled: true 334 | Invert Rainbow: false 335 | Max Color: 255; 255; 255 336 | Max Intensity: 4096 337 | Min Color: 0; 0; 0 338 | Min Intensity: 0 339 | Name: Laser Scan Cloud 340 | Position Transformer: XYZ 341 | Queue Size: 10 342 | Selectable: true 343 | Size (Pixels): 3 344 | Size (m): 0.00999999978 345 | Style: Points 346 | Topic: /color_cloud 347 | Unreliable: false 348 | Use Fixed Frame: true 349 | Use rainbow: true 350 | Value: true 351 | - Class: rviz/MarkerArray 352 | Enabled: true 353 | Marker Topic: /tool_path_preview 354 | Name: Tool Path Preview 355 | Namespaces: 356 | blend_paths: true 357 | Queue Size: 100 358 | Value: true 359 | - Class: rviz/TF 360 | Enabled: false 361 | Frame Timeout: 15 362 | Frames: 363 | All Enabled: false 364 | Marker Scale: 10 365 | Name: TF 366 | Show Arrows: true 367 | Show Axes: true 368 | Show Names: true 369 | Tree: 370 | {} 371 | Update Interval: 0 372 | Value: false 373 | - Alpha: 1 374 | Arrow Length: 0.300000012 375 | Axes Length: 0.00499999989 376 | Axes Radius: 0.00100000005 377 | Class: rviz/PoseArray 378 | Color: 255; 25; 0 379 | Enabled: true 380 | Head Length: 0.0700000003 381 | Head Radius: 0.0299999993 382 | Name: EdgePoseArray 383 | Shaft Length: 0.230000004 384 | Shaft Radius: 0.00999999978 385 | Shape: Axes 386 | Topic: /edge_visualization 387 | Unreliable: false 388 | Value: true 389 | - Alpha: 1 390 | Arrow Length: 0.300000012 391 | Axes Length: 0.00499999989 392 | Axes Radius: 0.00100000005 393 | Class: rviz/PoseArray 394 | Color: 255; 25; 0 395 | Enabled: false 396 | Head Length: 0.0700000003 397 | Head Radius: 0.0299999993 398 | Name: BlendPoseArray 399 | Shaft Length: 0.230000004 400 | Shaft Radius: 0.00999999978 401 | Shape: Axes 402 | Topic: /blend_visualization 403 | Unreliable: false 404 | Value: false 405 | - Alpha: 1 406 | Arrow Length: 0.300000012 407 | Axes Length: 0.00499999989 408 | Axes Radius: 0.00100000005 409 | Class: rviz/PoseArray 410 | Color: 255; 25; 0 411 | Enabled: false 412 | Head Length: 0.0700000003 413 | Head Radius: 0.0299999993 414 | Name: ScanPoseArray 415 | Shaft Length: 0.230000004 416 | Shaft Radius: 0.00999999978 417 | Shape: Axes 418 | Topic: /scan_visualization 419 | Unreliable: false 420 | Value: false 421 | - Alpha: 0.300000012 422 | Autocompute Intensity Bounds: true 423 | Autocompute Value Bounds: 424 | Max Value: 0.111644775 425 | Min Value: 0.100011706 426 | Value: true 427 | Axis: Z 428 | Channel Name: intensity 429 | Class: rviz/PointCloud2 430 | Color: 255; 255; 255 431 | Color Transformer: FlatColor 432 | Decay Time: 0 433 | Enabled: true 434 | Invert Rainbow: false 435 | Max Color: 255; 255; 255 436 | Max Intensity: 4096 437 | Min Color: 0; 0; 0 438 | Min Intensity: 0 439 | Name: PointCloud2 440 | Position Transformer: XYZ 441 | Queue Size: 10 442 | Selectable: true 443 | Size (Pixels): 3 444 | Size (m): 0.00999999978 445 | Style: Points 446 | Topic: /region_colored_cloud 447 | Unreliable: false 448 | Use Fixed Frame: true 449 | Use rainbow: true 450 | Value: true 451 | Enabled: true 452 | Global Options: 453 | Background Color: 48; 48; 48 454 | Default Light: true 455 | Fixed Frame: world_frame 456 | Frame Rate: 30 457 | Name: root 458 | Tools: 459 | - Class: rviz/Interact 460 | Hide Inactive Objects: true 461 | - Class: rviz/Select 462 | - Class: rviz/SetInitialPose 463 | Topic: initialpose 464 | - Class: rviz/SetGoal 465 | Topic: goal 466 | - Class: rviz/Measure 467 | Value: true 468 | Views: 469 | Current: 470 | Class: rviz/Orbit 471 | Distance: 1.3849448 472 | Enable Stereo Rendering: 473 | Stereo Eye Separation: 0.0599999987 474 | Stereo Focal Distance: 1 475 | Swap Stereo Eyes: false 476 | Value: false 477 | Focal Point: 478 | X: 0.00219828705 479 | Y: -0.354831398 480 | Z: 0.302058011 481 | Focal Shape Fixed Size: true 482 | Focal Shape Size: 0.0500000007 483 | Invert Z Axis: false 484 | Name: Current View 485 | Near Clip Distance: 0.00999999978 486 | Pitch: 0.889796972 487 | Target Frame: 488 | Value: Orbit (rviz) 489 | Yaw: 6.25971699 490 | Saved: ~ 491 | Window Geometry: 492 | BlendingPanel: 493 | collapsed: false 494 | DemoPanel: 495 | collapsed: false 496 | Displays: 497 | collapsed: false 498 | Height: 1176 499 | Hide Left Dock: false 500 | Hide Right Dock: false 501 | QMainWindow State: 000000ff00000000fd0000000100000000000001b400000444fc0200000003fc0000003a000004440000023701000019fa000000000100000004fb0000001200440065006d006f00500061006e0065006c0100000000ffffffff0000015000fffffffb000000240052006f0062006f00740042006c0065006e00640069006e006700500061006e0065006c0100000000ffffffff000001b400fffffffb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c0100000000000001ba0000015400fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c0000000448000000160000000000000000fb0000001200530065006c0065006300740069006f006e000000033e000000b60000005c00ffffff000005c60000044400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 502 | RobotBlendingPanel: 503 | collapsed: false 504 | Selection: 505 | collapsed: false 506 | Width: 1920 507 | X: 0 508 | Y: 24 509 | -------------------------------------------------------------------------------- /snp_prbt_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(snp_prbt_description) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY meshes models urdf worlds 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | -------------------------------------------------------------------------------- /snp_prbt_description/meshes/acf_with_grinder/acf_110_01_xs_with_grinder.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rosin-project/automatica18_scan_and_plan_demo/9ce16c6f6bc3d7ee58d33576faf96d0da17bbe60/snp_prbt_description/meshes/acf_with_grinder/acf_110_01_xs_with_grinder.STL -------------------------------------------------------------------------------- /snp_prbt_description/meshes/acf_with_grinder/acf_111_04_with_grinder.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rosin-project/automatica18_scan_and_plan_demo/9ce16c6f6bc3d7ee58d33576faf96d0da17bbe60/snp_prbt_description/meshes/acf_with_grinder/acf_111_04_with_grinder.STL -------------------------------------------------------------------------------- /snp_prbt_description/meshes/ensenso_n35.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rosin-project/automatica18_scan_and_plan_demo/9ce16c6f6bc3d7ee58d33576faf96d0da17bbe60/snp_prbt_description/meshes/ensenso_n35.STL -------------------------------------------------------------------------------- /snp_prbt_description/meshes/table.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rosin-project/automatica18_scan_and_plan_demo/9ce16c6f6bc3d7ee58d33576faf96d0da17bbe60/snp_prbt_description/meshes/table.stl -------------------------------------------------------------------------------- /snp_prbt_description/meshes/tool_fixture.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rosin-project/automatica18_scan_and_plan_demo/9ce16c6f6bc3d7ee58d33576faf96d0da17bbe60/snp_prbt_description/meshes/tool_fixture.STL -------------------------------------------------------------------------------- /snp_prbt_description/models/obj_01/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | obj_01 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Harsh Deshpande 10 | 11 | 12 | 13 | Object 1 14 | 15 | 16 | -------------------------------------------------------------------------------- /snp_prbt_description/models/obj_01/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0 0 0 0 5 | true 6 | 7 | 8 | 0 0 0 0 0 0 9 | 10 | 11 | 0.13 0.13 0.05 12 | 13 | 14 | 15 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /snp_prbt_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | snp_prbt_description 4 | 0.0.0 5 | The snp_prbt_description package 6 | 7 | Harsh Deshpande 8 | Jonathan Hechtbauer 9 | Qiyaoe Shi 10 | 11 | Apache License 2.0 12 | 13 | catkin 14 | 15 | gazebo_ros 16 | gazebo_ros_control 17 | prbt_support 18 | urdf 19 | xacro 20 | 21 | 22 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /snp_prbt_description/urdf/acf_with_grinder/acf_110_01_xs_with_grinder.urdf.xacro: -------------------------------------------------------------------------------- 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /snp_prbt_description/urdf/acf_with_grinder/acf_111_04_with_grinder.urdf.xacro: -------------------------------------------------------------------------------- 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /snp_prbt_description/urdf/ensenso.urdf.xacro: -------------------------------------------------------------------------------- 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 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | true 105 | 20.0 106 | 107 | ${60.0*3.142/180.0} 108 | 109 | B8G8R8 110 | 640 111 | 480 112 | 113 | 114 | 0.001 115 | 8.0 116 | 117 | 118 | 119 | ensenso 120 | true 121 | 10 122 | rgb/image_raw 123 | depth/image_raw 124 | /point_cloud 125 | rgb/camera_info 126 | depth/camera_info 127 | ${prefix}ensenso_optical_frame_sim 128 | 0.1 129 | 0.0 130 | 0.0 131 | 0.0 132 | 0.0 133 | 0.0 134 | 0.1 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | -------------------------------------------------------------------------------- /snp_prbt_description/urdf/prbt_table.urdf.xacro: -------------------------------------------------------------------------------- 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /snp_prbt_description/urdf/snp_prbt_demo.urdf.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /snp_prbt_description/urdf/snp_tool.urdf.xacro: -------------------------------------------------------------------------------- 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 | 52 | -------------------------------------------------------------------------------- /snp_prbt_description/worlds/prbt_robot.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | model://obj_01 12 | 0 -0.4 0.18 0 0 0 13 | 14 | 0 0 0 15 | 16 | 17 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: snp_prbt_description 4 | relative_path: urdf/snp_prbt_demo.urdf.xacro 5 | SRDF: 6 | relative_path: config/snp_prbt_demo.srdf 7 | CONFIG: 8 | author_name: Harsh Deshpande 9 | author_email: harshavardhan.deshpande@ipa.fraunhofer.de 10 | generated_timestamp: 1525265693 11 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(snp_prbt_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: "/prbt/manipulator_joint_trajectory_controller" 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | joints: [prbt_joint_1, prbt_joint_2, prbt_joint_3, prbt_joint_4, prbt_joint_5, prbt_joint_6] 6 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_manipulator_ensenso_controller 3 | joints: 4 | - prbt_joint_1 5 | - prbt_joint_2 6 | - prbt_joint_3 7 | - prbt_joint_4 8 | - prbt_joint_5 9 | - prbt_joint_6 10 | - name: fake_manipulator_tcp_controller 11 | joints: 12 | - prbt_joint_1 13 | - prbt_joint_2 14 | - prbt_joint_3 15 | - prbt_joint_4 16 | - prbt_joint_5 17 | - prbt_joint_6 -------------------------------------------------------------------------------- /snp_prbt_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator_ensenso: 2 | kinematics_solver: prbt_manipulator_kinematics/IKFastKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 6 | manipulator_tcp: 7 | kinematics_solver: prbt_manipulator_kinematics/IKFastKinematicsPlugin 8 | kinematics_solver_search_resolution: 0.005 9 | kinematics_solver_timeout: 0.005 10 | kinematics_solver_attempts: 3 11 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | ESTkConfigDefault: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECEkConfigDefault: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECEkConfigDefault: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECEkConfigDefault: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRTkConfigDefault: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnectkConfigDefault: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstarkConfigDefault: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRTkConfigDefault: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 50 | PRMkConfigDefault: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstarkConfigDefault: 54 | type: geometric::PRMstar 55 | FMTkConfigDefault: 56 | type: geometric::FMT 57 | num_samples: 1000 # number of states that the planner should sample. default: 1000 58 | radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 59 | nearest_k: 1 # use Knearest strategy. default: 1 60 | cache_cc: 1 # use collision checking cache. default: 1 61 | heuristics: 0 # activate cost to go heuristics. default: 0 62 | extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 63 | BFMTkConfigDefault: 64 | type: geometric::BFMT 65 | num_samples: 1000 # number of states that the planner should sample. default: 1000 66 | radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 67 | nearest_k: 1 # use the Knearest strategy. default: 1 68 | balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 69 | optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 70 | heuristics: 1 # activates cost to go heuristics. default: 1 71 | cache_cc: 1 # use the collision checking cache. default: 1 72 | extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 73 | PDSTkConfigDefault: 74 | type: geometric::PDST 75 | STRIDEkConfigDefault: 76 | type: geometric::STRIDE 77 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 78 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 79 | use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 80 | degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 81 | max_degree: 18 # max degree of a node in the GNAT. default: 12 82 | min_degree: 12 # min degree of a node in the GNAT. default: 12 83 | max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 84 | estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 85 | min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 86 | BiTRRTkConfigDefault: 87 | type: geometric::BiTRRT 88 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 89 | temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 90 | init_temperature: 100 # initial temperature. default: 100 91 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 92 | frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 93 | cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf 94 | LBTRRTkConfigDefault: 95 | type: geometric::LBTRRT 96 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 97 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 98 | epsilon: 0.4 # optimality approximation factor. default: 0.4 99 | BiESTkConfigDefault: 100 | type: geometric::BiEST 101 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 102 | ProjESTkConfigDefault: 103 | type: geometric::ProjEST 104 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 105 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 106 | LazyPRMkConfigDefault: 107 | type: geometric::LazyPRM 108 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 109 | LazyPRMstarkConfigDefault: 110 | type: geometric::LazyPRMstar 111 | SPARSkConfigDefault: 112 | type: geometric::SPARS 113 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 114 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 115 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 116 | max_failures: 1000 # maximum consecutive failure limit. default: 1000 117 | SPARStwokConfigDefault: 118 | type: geometric::SPARStwo 119 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 120 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 121 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 122 | max_failures: 5000 # maximum consecutive failure limit. default: 5000 123 | manipulator_ensenso: 124 | planner_configs: 125 | - SBLkConfigDefault 126 | - ESTkConfigDefault 127 | - LBKPIECEkConfigDefault 128 | - BKPIECEkConfigDefault 129 | - KPIECEkConfigDefault 130 | - RRTkConfigDefault 131 | - RRTConnectkConfigDefault 132 | - RRTstarkConfigDefault 133 | - TRRTkConfigDefault 134 | - PRMkConfigDefault 135 | - PRMstarkConfigDefault 136 | - FMTkConfigDefault 137 | - BFMTkConfigDefault 138 | - PDSTkConfigDefault 139 | - STRIDEkConfigDefault 140 | - BiTRRTkConfigDefault 141 | - LBTRRTkConfigDefault 142 | - BiESTkConfigDefault 143 | - ProjESTkConfigDefault 144 | - LazyPRMkConfigDefault 145 | - LazyPRMstarkConfigDefault 146 | - SPARSkConfigDefault 147 | - SPARStwokConfigDefault 148 | manipulator_tcp: 149 | planner_configs: 150 | - SBLkConfigDefault 151 | - ESTkConfigDefault 152 | - LBKPIECEkConfigDefault 153 | - BKPIECEkConfigDefault 154 | - KPIECEkConfigDefault 155 | - RRTkConfigDefault 156 | - RRTConnectkConfigDefault 157 | - RRTstarkConfigDefault 158 | - TRRTkConfigDefault 159 | - PRMkConfigDefault 160 | - PRMstarkConfigDefault 161 | - FMTkConfigDefault 162 | - BFMTkConfigDefault 163 | - PDSTkConfigDefault 164 | - STRIDEkConfigDefault 165 | - BiTRRTkConfigDefault 166 | - LBTRRTkConfigDefault 167 | - BiESTkConfigDefault 168 | - ProjESTkConfigDefault 169 | - LazyPRMkConfigDefault 170 | - LazyPRMstarkConfigDefault 171 | - SPARSkConfigDefault 172 | - SPARStwokConfigDefault 173 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/config/snp_prbt_demo.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 | 58 | 59 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/command_planner_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | [/move_group/fake_controller_joint_states] 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 | 58 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | [/prbt/joint_states] 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 | 58 | 59 | 63 | 64 | 65 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /MotionPlanning1 8 | Splitter Ratio: 0.74256 9 | Tree Height: 664 10 | - Class: rviz/Help 11 | Name: Help 12 | - Class: rviz/Views 13 | Expanded: 14 | - /Current View1 15 | Name: Views 16 | Splitter Ratio: 0.5 17 | Visualization Manager: 18 | Class: "" 19 | Displays: 20 | - Alpha: 0.5 21 | Cell Size: 1 22 | Class: rviz/Grid 23 | Color: 160; 160; 164 24 | Enabled: true 25 | Line Style: 26 | Line Width: 0.03 27 | Value: Lines 28 | Name: Grid 29 | Normal Cell Count: 0 30 | Offset: 31 | X: 0 32 | Y: 0 33 | Z: 0 34 | Plane: XY 35 | Plane Cell Count: 10 36 | Reference Frame: 37 | Value: true 38 | - Class: moveit_rviz_plugin/MotionPlanning 39 | Enabled: true 40 | MoveIt_Goal_Tolerance: 0 41 | MoveIt_Planning_Time: 5 42 | MoveIt_Use_Constraint_Aware_IK: true 43 | MoveIt_Warehouse_Host: 127.0.0.1 44 | MoveIt_Warehouse_Port: 33829 45 | Name: MotionPlanning 46 | Planned Path: 47 | Links: 48 | base_bellow_link: 49 | Alpha: 1 50 | Show Axes: false 51 | Show Trail: false 52 | Value: true 53 | base_footprint: 54 | Alpha: 1 55 | Show Axes: false 56 | Show Trail: false 57 | Value: true 58 | base_link: 59 | Alpha: 1 60 | Show Axes: false 61 | Show Trail: false 62 | Value: true 63 | bl_caster_l_wheel_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | bl_caster_r_wheel_link: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | bl_caster_rotation_link: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | br_caster_l_wheel_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | br_caster_r_wheel_link: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | br_caster_rotation_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | double_stereo_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | fl_caster_l_wheel_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | fl_caster_r_wheel_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | fl_caster_rotation_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | fr_caster_l_wheel_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | fr_caster_r_wheel_link: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | fr_caster_rotation_link: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | head_mount_kinect_ir_link: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | head_mount_kinect_rgb_link: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | head_mount_link: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | Value: true 143 | head_mount_prosilica_link: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | Value: true 148 | head_pan_link: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | head_plate_frame: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | Value: true 158 | head_tilt_link: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | l_elbow_flex_link: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | Value: true 168 | l_forearm_link: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | Value: true 173 | l_forearm_roll_link: 174 | Alpha: 1 175 | Show Axes: false 176 | Show Trail: false 177 | Value: true 178 | l_gripper_l_finger_link: 179 | Alpha: 1 180 | Show Axes: false 181 | Show Trail: false 182 | Value: true 183 | l_gripper_l_finger_tip_link: 184 | Alpha: 1 185 | Show Axes: false 186 | Show Trail: false 187 | Value: true 188 | l_gripper_motor_accelerometer_link: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Value: true 193 | l_gripper_palm_link: 194 | Alpha: 1 195 | Show Axes: false 196 | Show Trail: false 197 | Value: true 198 | l_gripper_r_finger_link: 199 | Alpha: 1 200 | Show Axes: false 201 | Show Trail: false 202 | Value: true 203 | l_gripper_r_finger_tip_link: 204 | Alpha: 1 205 | Show Axes: false 206 | Show Trail: false 207 | Value: true 208 | l_shoulder_lift_link: 209 | Alpha: 1 210 | Show Axes: false 211 | Show Trail: false 212 | Value: true 213 | l_shoulder_pan_link: 214 | Alpha: 1 215 | Show Axes: false 216 | Show Trail: false 217 | Value: true 218 | l_upper_arm_link: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | Value: true 223 | l_upper_arm_roll_link: 224 | Alpha: 1 225 | Show Axes: false 226 | Show Trail: false 227 | Value: true 228 | l_wrist_flex_link: 229 | Alpha: 1 230 | Show Axes: false 231 | Show Trail: false 232 | Value: true 233 | l_wrist_roll_link: 234 | Alpha: 1 235 | Show Axes: false 236 | Show Trail: false 237 | Value: true 238 | laser_tilt_mount_link: 239 | Alpha: 1 240 | Show Axes: false 241 | Show Trail: false 242 | Value: true 243 | r_elbow_flex_link: 244 | Alpha: 1 245 | Show Axes: false 246 | Show Trail: false 247 | Value: true 248 | r_forearm_link: 249 | Alpha: 1 250 | Show Axes: false 251 | Show Trail: false 252 | Value: true 253 | r_forearm_roll_link: 254 | Alpha: 1 255 | Show Axes: false 256 | Show Trail: false 257 | Value: true 258 | r_gripper_l_finger_link: 259 | Alpha: 1 260 | Show Axes: false 261 | Show Trail: false 262 | Value: true 263 | r_gripper_l_finger_tip_link: 264 | Alpha: 1 265 | Show Axes: false 266 | Show Trail: false 267 | Value: true 268 | r_gripper_motor_accelerometer_link: 269 | Alpha: 1 270 | Show Axes: false 271 | Show Trail: false 272 | Value: true 273 | r_gripper_palm_link: 274 | Alpha: 1 275 | Show Axes: false 276 | Show Trail: false 277 | Value: true 278 | r_gripper_r_finger_link: 279 | Alpha: 1 280 | Show Axes: false 281 | Show Trail: false 282 | Value: true 283 | r_gripper_r_finger_tip_link: 284 | Alpha: 1 285 | Show Axes: false 286 | Show Trail: false 287 | Value: true 288 | r_shoulder_lift_link: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | r_shoulder_pan_link: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | Value: true 298 | r_upper_arm_link: 299 | Alpha: 1 300 | Show Axes: false 301 | Show Trail: false 302 | Value: true 303 | r_upper_arm_roll_link: 304 | Alpha: 1 305 | Show Axes: false 306 | Show Trail: false 307 | Value: true 308 | r_wrist_flex_link: 309 | Alpha: 1 310 | Show Axes: false 311 | Show Trail: false 312 | Value: true 313 | r_wrist_roll_link: 314 | Alpha: 1 315 | Show Axes: false 316 | Show Trail: false 317 | Value: true 318 | sensor_mount_link: 319 | Alpha: 1 320 | Show Axes: false 321 | Show Trail: false 322 | Value: true 323 | torso_lift_link: 324 | Alpha: 1 325 | Show Axes: false 326 | Show Trail: false 327 | Value: true 328 | Loop Animation: true 329 | Robot Alpha: 0.5 330 | Show Robot Collision: false 331 | Show Robot Visual: true 332 | Show Trail: false 333 | State Display Time: 0.05 s 334 | Trajectory Topic: move_group/display_planned_path 335 | Planning Metrics: 336 | Payload: 1 337 | Show Joint Torques: false 338 | Show Manipulability: false 339 | Show Manipulability Index: false 340 | Show Weight Limit: false 341 | Planning Request: 342 | Colliding Link Color: 255; 0; 0 343 | Goal State Alpha: 1 344 | Goal State Color: 250; 128; 0 345 | Interactive Marker Size: 0 346 | Joint Violation Color: 255; 0; 255 347 | Planning Group: left_arm 348 | Query Goal State: true 349 | Query Start State: false 350 | Show Workspace: false 351 | Start State Alpha: 1 352 | Start State Color: 0; 255; 0 353 | Planning Scene Topic: move_group/monitored_planning_scene 354 | Robot Description: robot_description 355 | Scene Geometry: 356 | Scene Alpha: 1 357 | Scene Color: 50; 230; 50 358 | Scene Display Time: 0.2 359 | Show Scene Geometry: true 360 | Voxel Coloring: Z-Axis 361 | Voxel Rendering: Occupied Voxels 362 | Scene Robot: 363 | Attached Body Color: 150; 50; 150 364 | Links: 365 | base_bellow_link: 366 | Alpha: 1 367 | Show Axes: false 368 | Show Trail: false 369 | Value: true 370 | base_footprint: 371 | Alpha: 1 372 | Show Axes: false 373 | Show Trail: false 374 | Value: true 375 | base_link: 376 | Alpha: 1 377 | Show Axes: false 378 | Show Trail: false 379 | Value: true 380 | bl_caster_l_wheel_link: 381 | Alpha: 1 382 | Show Axes: false 383 | Show Trail: false 384 | Value: true 385 | bl_caster_r_wheel_link: 386 | Alpha: 1 387 | Show Axes: false 388 | Show Trail: false 389 | Value: true 390 | bl_caster_rotation_link: 391 | Alpha: 1 392 | Show Axes: false 393 | Show Trail: false 394 | Value: true 395 | br_caster_l_wheel_link: 396 | Alpha: 1 397 | Show Axes: false 398 | Show Trail: false 399 | Value: true 400 | br_caster_r_wheel_link: 401 | Alpha: 1 402 | Show Axes: false 403 | Show Trail: false 404 | Value: true 405 | br_caster_rotation_link: 406 | Alpha: 1 407 | Show Axes: false 408 | Show Trail: false 409 | Value: true 410 | double_stereo_link: 411 | Alpha: 1 412 | Show Axes: false 413 | Show Trail: false 414 | Value: true 415 | fl_caster_l_wheel_link: 416 | Alpha: 1 417 | Show Axes: false 418 | Show Trail: false 419 | Value: true 420 | fl_caster_r_wheel_link: 421 | Alpha: 1 422 | Show Axes: false 423 | Show Trail: false 424 | Value: true 425 | fl_caster_rotation_link: 426 | Alpha: 1 427 | Show Axes: false 428 | Show Trail: false 429 | Value: true 430 | fr_caster_l_wheel_link: 431 | Alpha: 1 432 | Show Axes: false 433 | Show Trail: false 434 | Value: true 435 | fr_caster_r_wheel_link: 436 | Alpha: 1 437 | Show Axes: false 438 | Show Trail: false 439 | Value: true 440 | fr_caster_rotation_link: 441 | Alpha: 1 442 | Show Axes: false 443 | Show Trail: false 444 | Value: true 445 | head_mount_kinect_ir_link: 446 | Alpha: 1 447 | Show Axes: false 448 | Show Trail: false 449 | Value: true 450 | head_mount_kinect_rgb_link: 451 | Alpha: 1 452 | Show Axes: false 453 | Show Trail: false 454 | Value: true 455 | head_mount_link: 456 | Alpha: 1 457 | Show Axes: false 458 | Show Trail: false 459 | Value: true 460 | head_mount_prosilica_link: 461 | Alpha: 1 462 | Show Axes: false 463 | Show Trail: false 464 | Value: true 465 | head_pan_link: 466 | Alpha: 1 467 | Show Axes: false 468 | Show Trail: false 469 | Value: true 470 | head_plate_frame: 471 | Alpha: 1 472 | Show Axes: false 473 | Show Trail: false 474 | Value: true 475 | head_tilt_link: 476 | Alpha: 1 477 | Show Axes: false 478 | Show Trail: false 479 | Value: true 480 | l_elbow_flex_link: 481 | Alpha: 1 482 | Show Axes: false 483 | Show Trail: false 484 | Value: true 485 | l_forearm_link: 486 | Alpha: 1 487 | Show Axes: false 488 | Show Trail: false 489 | Value: true 490 | l_forearm_roll_link: 491 | Alpha: 1 492 | Show Axes: false 493 | Show Trail: false 494 | Value: true 495 | l_gripper_l_finger_link: 496 | Alpha: 1 497 | Show Axes: false 498 | Show Trail: false 499 | Value: true 500 | l_gripper_l_finger_tip_link: 501 | Alpha: 1 502 | Show Axes: false 503 | Show Trail: false 504 | Value: true 505 | l_gripper_motor_accelerometer_link: 506 | Alpha: 1 507 | Show Axes: false 508 | Show Trail: false 509 | Value: true 510 | l_gripper_palm_link: 511 | Alpha: 1 512 | Show Axes: false 513 | Show Trail: false 514 | Value: true 515 | l_gripper_r_finger_link: 516 | Alpha: 1 517 | Show Axes: false 518 | Show Trail: false 519 | Value: true 520 | l_gripper_r_finger_tip_link: 521 | Alpha: 1 522 | Show Axes: false 523 | Show Trail: false 524 | Value: true 525 | l_shoulder_lift_link: 526 | Alpha: 1 527 | Show Axes: false 528 | Show Trail: false 529 | Value: true 530 | l_shoulder_pan_link: 531 | Alpha: 1 532 | Show Axes: false 533 | Show Trail: false 534 | Value: true 535 | l_upper_arm_link: 536 | Alpha: 1 537 | Show Axes: false 538 | Show Trail: false 539 | Value: true 540 | l_upper_arm_roll_link: 541 | Alpha: 1 542 | Show Axes: false 543 | Show Trail: false 544 | Value: true 545 | l_wrist_flex_link: 546 | Alpha: 1 547 | Show Axes: false 548 | Show Trail: false 549 | Value: true 550 | l_wrist_roll_link: 551 | Alpha: 1 552 | Show Axes: false 553 | Show Trail: false 554 | Value: true 555 | laser_tilt_mount_link: 556 | Alpha: 1 557 | Show Axes: false 558 | Show Trail: false 559 | Value: true 560 | r_elbow_flex_link: 561 | Alpha: 1 562 | Show Axes: false 563 | Show Trail: false 564 | Value: true 565 | r_forearm_link: 566 | Alpha: 1 567 | Show Axes: false 568 | Show Trail: false 569 | Value: true 570 | r_forearm_roll_link: 571 | Alpha: 1 572 | Show Axes: false 573 | Show Trail: false 574 | Value: true 575 | r_gripper_l_finger_link: 576 | Alpha: 1 577 | Show Axes: false 578 | Show Trail: false 579 | Value: true 580 | r_gripper_l_finger_tip_link: 581 | Alpha: 1 582 | Show Axes: false 583 | Show Trail: false 584 | Value: true 585 | r_gripper_motor_accelerometer_link: 586 | Alpha: 1 587 | Show Axes: false 588 | Show Trail: false 589 | Value: true 590 | r_gripper_palm_link: 591 | Alpha: 1 592 | Show Axes: false 593 | Show Trail: false 594 | Value: true 595 | r_gripper_r_finger_link: 596 | Alpha: 1 597 | Show Axes: false 598 | Show Trail: false 599 | Value: true 600 | r_gripper_r_finger_tip_link: 601 | Alpha: 1 602 | Show Axes: false 603 | Show Trail: false 604 | Value: true 605 | r_shoulder_lift_link: 606 | Alpha: 1 607 | Show Axes: false 608 | Show Trail: false 609 | Value: true 610 | r_shoulder_pan_link: 611 | Alpha: 1 612 | Show Axes: false 613 | Show Trail: false 614 | Value: true 615 | r_upper_arm_link: 616 | Alpha: 1 617 | Show Axes: false 618 | Show Trail: false 619 | Value: true 620 | r_upper_arm_roll_link: 621 | Alpha: 1 622 | Show Axes: false 623 | Show Trail: false 624 | Value: true 625 | r_wrist_flex_link: 626 | Alpha: 1 627 | Show Axes: false 628 | Show Trail: false 629 | Value: true 630 | r_wrist_roll_link: 631 | Alpha: 1 632 | Show Axes: false 633 | Show Trail: false 634 | Value: true 635 | sensor_mount_link: 636 | Alpha: 1 637 | Show Axes: false 638 | Show Trail: false 639 | Value: true 640 | torso_lift_link: 641 | Alpha: 1 642 | Show Axes: false 643 | Show Trail: false 644 | Value: true 645 | Robot Alpha: 0.5 646 | Show Scene Robot: true 647 | Value: true 648 | Enabled: true 649 | Global Options: 650 | Background Color: 48; 48; 48 651 | Fixed Frame: /world_frame 652 | Name: root 653 | Tools: 654 | - Class: rviz/Interact 655 | Hide Inactive Objects: true 656 | - Class: rviz/MoveCamera 657 | - Class: rviz/Select 658 | Value: true 659 | Views: 660 | Current: 661 | Class: rviz/XYOrbit 662 | Distance: 2.9965 663 | Focal Point: 664 | X: 0.113567 665 | Y: 0.10592 666 | Z: 2.23518e-07 667 | Name: Current View 668 | Near Clip Distance: 0.01 669 | Pitch: 0.509797 670 | Target Frame: /world_frame 671 | Value: XYOrbit (rviz) 672 | Yaw: 5.65995 673 | Saved: ~ 674 | Window Geometry: 675 | Displays: 676 | collapsed: false 677 | Height: 1337 678 | Help: 679 | collapsed: false 680 | Hide Left Dock: false 681 | Hide Right Dock: false 682 | Motion Planning: 683 | collapsed: false 684 | QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 685 | Views: 686 | collapsed: false 687 | Width: 1828 688 | X: 459 689 | Y: -243 690 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/planning_context.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/snp_prbt_description_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/snp_prbt_description_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /snp_prbt_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | snp_prbt_moveit_config 4 | 0.0.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the snp_prbt_description with the MoveIt! Motion Planning Framework 7 | 8 | 9 | Harsh Deshpande 10 | Jonathan Hechtbauer 11 | 12 | Harsh Deshpande 13 | 14 | Apache License 2.0 15 | 16 | http://moveit.ros.org/ 17 | https://github.com/ros-planning/moveit/issues 18 | https://github.com/ros-planning/moveit 19 | 20 | catkin 21 | 22 | joint_state_publisher 23 | moveit_ros_move_group 24 | moveit_fake_controller_manager 25 | moveit_kinematics 26 | moveit_planners_ompl 27 | moveit_ros_visualization 28 | pilz_trajectory_generation 29 | prbt_ikfast_manipulator_plugin 30 | robot_state_publisher 31 | snp_prbt_description 32 | xacro 33 | 34 | 35 | -------------------------------------------------------------------------------- /snp_prbt_support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(snp_prbt_support) 3 | add_definitions(-std=c++11) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | actionlib 7 | control_msgs 8 | godel_msgs 9 | industrial_robot_simulator_service 10 | roscpp 11 | trajectory_msgs 12 | ) 13 | 14 | find_package(PkgConfig) 15 | pkg_check_modules(PC_MODBUS QUIET libmodbus) 16 | 17 | find_path(MODBUS_INCLUDE_DIR modbus/modbus.h 18 | HINTS ${PC_MODBUS_INCLUDEDIR} ${PC_MODBUS_INCLUDE_DIRS} 19 | ) 20 | find_library(MODBUS_LIBRARY NAMES modbus libmodbus 21 | HINTS ${PC_MODBUS_LIBDIR} ${PC_MODBUS_LIBRARY_DIRS} ) 22 | 23 | mark_as_advanced(MODBUS_INCLUDE_DIR MODBUS_LIBRARY ) 24 | 25 | if(NOT MODBUS_INCLUDE_DIR OR NOT MODBUS_LIBRARY) 26 | message(FATAL_ERROR "modbus library not found") 27 | endif() 28 | 29 | catkin_package() 30 | 31 | include_directories(${catkin_INCLUDE_DIRS} ${MODBUS_INCLUDE_DIR} 32 | ) 33 | 34 | add_executable(prbt_blend_process_service_node 35 | src/prbt_blend_process_service_node.cpp 36 | ) 37 | add_dependencies(prbt_blend_process_service_node ${catkin_EXPORTED_TARGETS}) 38 | 39 | target_link_libraries(prbt_blend_process_service_node 40 | ${catkin_LIBRARIES} 41 | ${MODBUS_LIBRARY} 42 | ) 43 | 44 | add_executable(spindle_sim 45 | src/spindle_sim.cpp 46 | ) 47 | 48 | target_link_libraries(spindle_sim 49 | ${MODBUS_LIBRARY} 50 | ) 51 | 52 | install(TARGETS prbt_blend_process_service_node spindle_sim 53 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 54 | 55 | install(PROGRAMS scripts/ensenso_interface scripts/set_region_of_interest 56 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 57 | ) 58 | -------------------------------------------------------------------------------- /snp_prbt_support/launch/setup_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | $(arg roi_lower) 16 | $(arg roi_lower) 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /snp_prbt_support/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | snp_prbt_support 4 | 0.0.0 5 | The snp_prbt_support package 6 | 7 | Harsh Deshpande 8 | Jonathan Hechtbauer 9 | 10 | Ludovic Delval 11 | Mathias Ludtke 12 | Qiyaoe Shi 13 | 14 | Apache License 2.0 15 | 16 | catkin 17 | 18 | actionlib 19 | control_msgs 20 | godel_msgs 21 | industrial_robot_simulator_service 22 | libmodbus-dev 23 | roscpp 24 | trajectory_msgs 25 | 26 | actionlib 27 | control_msgs 28 | ensenso_camera 29 | ensenso_camera_msgs 30 | godel_msgs 31 | industrial_robot_simulator_service 32 | libmodbus-dev 33 | roscpp 34 | rospy 35 | std_srvs 36 | trajectory_msgs 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /snp_prbt_support/scripts/ensenso_interface: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2018, Fraunhofer IPA 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | import actionlib 19 | from actionlib_msgs.msg import GoalStatus 20 | 21 | from ensenso_camera_msgs.msg import RequestDataAction, RequestDataGoal 22 | from std_srvs.srv import Trigger, TriggerResponse 23 | 24 | 25 | class scan_interface(): 26 | def __init__(self): 27 | self.trigger_service = rospy.Service("point_cloud_workspace_request",Trigger,self.get_last_scan) 28 | self.request_scan = actionlib.SimpleActionClient("request_data", RequestDataAction) 29 | 30 | def get_last_scan(self, req): 31 | goal = RequestDataGoal() 32 | goal.request_point_cloud = True 33 | state = self.request_scan.send_goal_and_wait(goal,rospy.Duration(1.0)) 34 | print state 35 | if(state == GoalStatus.SUCCEEDED): 36 | return TriggerResponse (True, "Requested scan succeed !") 37 | else: 38 | return TriggerResponse (False, "Requested scan failed: %s"%(self.request_scan.get_result().error.message)) 39 | 40 | if __name__ == "__main__": 41 | rospy.init_node("ensenso_scan_interface") 42 | SI = scan_interface() 43 | rospy.spin() 44 | -------------------------------------------------------------------------------- /snp_prbt_support/scripts/set_region_of_interest: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2018, Fraunhofer IPA 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import rospy 18 | import actionlib 19 | 20 | #from ensenso_camera_msgs.msg import GetParameterAction, GetParameterGoal 21 | from ensenso_camera_msgs.msg import SetParameterAction, SetParameterGoal 22 | from ensenso_camera_msgs.msg import GetParameterAction, GetParameterGoal 23 | from ensenso_camera_msgs.msg import Parameter 24 | 25 | def main(): 26 | timeout = rospy.get_param("~timeout", 60) 27 | lower_point = rospy.get_param("~lower_point",{'x': 0.0, 'y': 0.0, 'z': 0.0}) 28 | upper_point = rospy.get_param("~upper_point",{'x': 0.0, 'y': 0.0, 'z': 0.0}) 29 | 30 | set_parameter = actionlib.SimpleActionClient("set_parameter", SetParameterAction) 31 | get_parameter = actionlib.SimpleActionClient("get_parameter", GetParameterAction) 32 | if not set_parameter.wait_for_server(rospy.Duration(timeout)): 33 | rospy.logerr("The camera node is not running!") 34 | sys.exit() 35 | 36 | #upper:LeftTop; lower:RightBottom 37 | roi_message = Parameter(key=Parameter.REGION_OF_INTEREST) 38 | roi_message.region_of_interest_value.lower.x=lower_point["x"] 39 | roi_message.region_of_interest_value.lower.y=lower_point["y"] 40 | roi_message.region_of_interest_value.lower.z=lower_point["z"] 41 | roi_message.region_of_interest_value.upper.x=upper_point["x"] 42 | roi_message.region_of_interest_value.upper.y=upper_point["y"] 43 | roi_message.region_of_interest_value.upper.z=upper_point["z"] 44 | 45 | set_parameter.send_goal(SetParameterGoal( 46 | parameters=[Parameter(key=Parameter.REGION_OF_INTEREST, region_of_interest_value=roi_message.region_of_interest_value)] 47 | )) 48 | set_parameter.wait_for_result() 49 | 50 | #check the roi 51 | get_parameter.send_goal(GetParameterGoal(keys=[Parameter.REGION_OF_INTEREST])) 52 | get_parameter.wait_for_result() 53 | roi = get_parameter.get_result().results[0].region_of_interest_value 54 | print ("lower point of roi: /n", roi.lower) 55 | print ("upper point of roi: /n", roi.upper) 56 | 57 | if __name__ == "__main__": 58 | try: 59 | rospy.init_node("ensenso_camera_set_parameter") 60 | main() 61 | except rospy.ROSInterruptException: 62 | pass 63 | -------------------------------------------------------------------------------- /snp_prbt_support/src/prbt_blend_process_service_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | void appendTrajectory(trajectory_msgs::JointTrajectory& target, const trajectory_msgs::JointTrajectory& extend) { 30 | const ros::Duration offset = target.points.empty() ? ros::Duration(0.0) : target.points.back().time_from_start; 31 | std::transform(extend.points.begin(), extend.points.end(), std::back_inserter(target.points), 32 | [offset](trajectory_msgs::JointTrajectoryPoint pt){ pt.time_from_start += offset; return pt;}); 33 | } 34 | 35 | class PsirBlendProcess { 36 | ros::NodeHandle nh_; 37 | actionlib::SimpleActionServer as_; 38 | actionlib::SimpleActionClient ac_; 39 | ros::ServiceClient sim_client_; 40 | modbus_t *modbus_ {nullptr}; 41 | int spindle_coil_{1}; 42 | 43 | bool move(const trajectory_msgs::JointTrajectory &trajectory) { 44 | control_msgs::FollowJointTrajectoryGoal goal; 45 | goal.trajectory = trajectory; 46 | 47 | goal.trajectory.header.stamp = ros::Time(0); 48 | for (auto &point: goal.trajectory.points) { 49 | point.accelerations.assign(point.accelerations.size(), 0.0); 50 | } 51 | return ac_.sendGoalAndWait(goal, ros::Duration(trajectory.points.back().time_from_start*1.1)) == actionlib::SimpleClientGoalState::SUCCEEDED; 52 | } 53 | void execute(const godel_msgs::ProcessExecutionGoalConstPtr &goal) { 54 | godel_msgs::ProcessExecutionResult res; 55 | res.success = goal->simulate? simulateProcess(goal) : executeProcess(goal); 56 | if(res.success) { 57 | as_.setSucceeded(res); 58 | }else{ 59 | as_.setAborted(res); 60 | } 61 | } 62 | 63 | bool simulateProcess(const godel_msgs::ProcessExecutionGoalConstPtr &goal) { 64 | industrial_robot_simulator_service::SimulateTrajectory srv; 65 | srv.request.trajectory = goal->trajectory_approach; 66 | srv.request.trajectory.header.stamp = ros::Time(0); 67 | appendTrajectory(srv.request.trajectory, goal->trajectory_process); 68 | appendTrajectory(srv.request.trajectory, goal->trajectory_depart); 69 | srv.request.wait_for_execution = goal->wait_for_execution; 70 | ROS_INFO_STREAM(srv.request); 71 | return sim_client_.call(srv); 72 | } 73 | bool setSpindle(bool on) { 74 | return modbus_ && modbus_write_bit(modbus_, spindle_coil_, on?1:0) == 1; 75 | } 76 | bool executeProcess(const godel_msgs::ProcessExecutionGoalConstPtr &goal) { 77 | if(!move(goal->trajectory_approach) || !setSpindle(true)) return false; 78 | bool ok = move(goal->trajectory_process); 79 | return setSpindle(false) && ok && move(goal->trajectory_depart); 80 | } 81 | 82 | public: 83 | PsirBlendProcess() 84 | : as_(nh_,"blend_process_execution_as",boost::bind(&PsirBlendProcess::execute, this, _1), false), 85 | ac_("joint_trajectory_action", true), 86 | sim_client_(nh_.serviceClient("simulate_path")) 87 | { 88 | } 89 | 90 | ~PsirBlendProcess() { 91 | if(modbus_) { 92 | modbus_close(modbus_); 93 | modbus_free(modbus_); 94 | } 95 | } 96 | bool start() { 97 | 98 | if (!ac_.waitForServer(ros::Duration(30.0))) return false; 99 | ROS_INFO("WAITED"); 100 | 101 | if(!modbus_){ 102 | ros::NodeHandle priv("~"); 103 | std::string spindle_host("169.254.60.1"); 104 | priv.getParam("spindle_host", spindle_host); 105 | priv.getParam("spindle_coil", spindle_coil_); 106 | modbus_ = modbus_new_tcp(spindle_host.c_str(), priv.param("spindle_port", 502)); 107 | if (modbus_connect(modbus_) != 0) { 108 | modbus_free(modbus_); 109 | modbus_ = nullptr; 110 | return false; 111 | } 112 | } 113 | 114 | as_.start(); 115 | ROS_INFO("Starting PRBT blend process service"); 116 | return true; 117 | } 118 | }; 119 | 120 | int main(int argc, char** argv) 121 | { 122 | ros::init(argc, argv, "prbt_blend_process_service_node"); 123 | 124 | ros::NodeHandle nh; 125 | PsirBlendProcess proc; 126 | if(!proc.start()) return 1; 127 | ros::spin(); 128 | return 0; 129 | } 130 | -------------------------------------------------------------------------------- /snp_prbt_support/src/spindle_sim.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | int main(int argc, char** argv) 19 | { 20 | modbus_t * ctx = modbus_new_tcp("127.0.0.1", 1502); 21 | modbus_set_debug(ctx, TRUE); 22 | int s = modbus_tcp_listen(ctx, 1); 23 | modbus_tcp_accept(ctx, &s); 24 | 25 | modbus_mapping_t *mapping = modbus_mapping_new(2, 0, 0, 0); 26 | int rc = 0; 27 | 28 | uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH] = {}; 29 | 30 | do{ 31 | rc = modbus_receive(ctx, query); 32 | if(rc > 0) rc = modbus_reply(ctx, query, rc, mapping); 33 | } while(rc > 0); 34 | 35 | modbus_mapping_free(mapping); 36 | close(s); 37 | modbus_free(ctx); 38 | return 0; 39 | } 40 | --------------------------------------------------------------------------------