├── .gitignore ├── .travis.yml ├── LICENSE ├── README.md ├── open_manipulator_ar_markers ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ └── ar_pose.launch ├── package.xml └── rviz │ └── ar_marker.rviz ├── open_manipulator_camera ├── CMakeLists.txt ├── camera_info │ ├── astra_pro.yaml │ └── raspicam.yaml ├── launch │ ├── astra_pro.launch │ ├── raspicam.launch │ └── realsense_d435.launch └── package.xml ├── open_manipulator_perceptions ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml └── open_manipulator_pick_and_place ├── CMakeLists.txt ├── include └── open_manipulator_pick_and_place │ └── open_manipulator_pick_and_place.h ├── launch └── open_manipulator_pick_and_place.launch ├── package.xml └── src └── open_manipulator_pick_and_place.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes ros-industrial/industrial_ci package. 2 | # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst 3 | 4 | sudo: required 5 | dist: trusty 6 | services: 7 | - docker 8 | language: generic 9 | python: 10 | - "2.7" 11 | compiler: 12 | - gcc 13 | notifications: 14 | email: 15 | on_success: change 16 | on_failure: always 17 | recipients: 18 | - jhshim@robotis.com 19 | # - pyo@robotis.com 20 | env: 21 | matrix: 22 | - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian 23 | # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie 24 | branches: 25 | only: 26 | - master 27 | - develop 28 | - kinetic-devel 29 | install: 30 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 31 | script: 32 | - source .ci_config/travis.sh 33 | -------------------------------------------------------------------------------- /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 | # OpenManipulator 2 | 3 | 4 | 5 | - Active Branches: noetic, main 6 | - Legacy Branches: *-devel 7 | 8 | ## ROBOTIS e-Manual for OpenManipulator 9 | - [ROBOTIS e-Manual for OpenManipulator](http://emanual.robotis.com/docs/en/platform/openmanipulator/) 10 | 11 | ## Wiki for open_manipulator_perceptions Packages 12 | - http://wiki.ros.org/open_manipulator_perceptions (metapackage) 13 | - http://wiki.ros.org/open_manipulator_ar_markers 14 | 15 | ## Open Source related to OpenManipulator 16 | - [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) 17 | - [open_manipulator_msgs](https://github.com/ROBOTIS-GIT/open_manipulator_msgs) 18 | - [open_manipulator_simulations](https://github.com/ROBOTIS-GIT/open_manipulator_simulations) 19 | - [open_manipulator_perceptions](https://github.com/ROBOTIS-GIT/open_manipulator_perceptions) 20 | - [open_manipulator_with_tb3](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3) 21 | - [open_manipulator_with_tb3_msgs](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3_msgs) 22 | - [open_manipulator_with_tb3_simulations](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3_simulations) 23 | - [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) 24 | - [turtlebot3_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_msgs) 25 | - [turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations) 26 | - [turtlebot3_applications](https://github.com/ROBOTIS-GIT/turtlebot3_applications) 27 | - [turtlebot3_applications_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_applications_msgs) 28 | - [turtlebot3_autorace](https://github.com/ROBOTIS-GIT/turtlebot3_autorace) 29 | - [turtlebot3_deliver](https://github.com/ROBOTIS-GIT/turtlebot3_deliver) 30 | - [hls_lfcd_lds_driver](https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver) 31 | - [manipulator_h](https://github.com/ROBOTIS-GIT/ROBOTIS-MANIPULATOR-H) 32 | - [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) 33 | - [dynamixel_workbench](https://github.com/ROBOTIS-GIT/dynamixel-workbench) 34 | - [robotis_math](https://github.com/ROBOTIS-GIT/ROBOTIS-Math) 35 | - [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) 36 | - [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) 37 | 38 | ## Documents and Videos related to OpenManipulator 39 | - [ROBOTIS e-Manual for OpenManipulator](http://emanual.robotis.com/docs/en/platform/openmanipulator/) 40 | - [ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/) 41 | - [ROBOTIS e-Manual for ROBOTIS MANIPULATOR-H](http://emanual.robotis.com/docs/en/platform/manipulator_h/introduction/) 42 | - [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) 43 | - [ROBOTIS e-Manual for Dynamixel Workbench](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/) 44 | - [e-Book for TurtleBot3 and OpenManipulator](https://community.robotsource.org/t/download-the-ros-robot-programming-book-for-free/51/) 45 | - [Videos for OpenManipulator](https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb) 46 | - [Videos for TurtleBot3 and OpenManipulator](https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU) 47 | -------------------------------------------------------------------------------- /open_manipulator_ar_markers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package open_manipulator_ar_markers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2018-06-01) 6 | ------------------ 7 | * added new open_manipulator_perceptions package 8 | * added perception packages as AR marker, Object recognition and so on 9 | * Contributors: Darby Lim 10 | -------------------------------------------------------------------------------- /open_manipulator_ar_markers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(open_manipulator_ar_markers) 6 | 7 | add_compile_options(-std=c++11) 8 | 9 | ################################################################################ 10 | # Find catkin packages and libraries for catkin and system dependencies 11 | ################################################################################ 12 | find_package(catkin REQUIRED COMPONENTS 13 | ar_track_alvar 14 | ar_track_alvar_msgs 15 | image_transport 16 | image_proc 17 | ) 18 | 19 | ################################################################################ 20 | # Setup for python modules and scripts 21 | ################################################################################ 22 | 23 | ################################################################################ 24 | # Declare ROS messages, services and actions 25 | ################################################################################ 26 | 27 | ################################################################################ 28 | # Declare ROS dynamic reconfigure parameters 29 | ################################################################################ 30 | 31 | ################################################################################ 32 | # Declare catkin specific configuration to be passed to dependent projects 33 | ################################################################################ 34 | catkin_package( 35 | CATKIN_DEPENDS ar_track_alvar ar_track_alvar_msgs image_transport image_proc 36 | ) 37 | 38 | ################################################################################ 39 | # Build 40 | ################################################################################ 41 | include_directories( 42 | ${catkin_INCLUDE_DIRS} 43 | ) 44 | 45 | ################################################################################ 46 | # Install 47 | ################################################################################ 48 | install(DIRECTORY launch 49 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 50 | ) 51 | 52 | ################################################################################ 53 | # Test 54 | ################################################################################ 55 | -------------------------------------------------------------------------------- /open_manipulator_ar_markers/launch/ar_pose.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | ["joint_states"] 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 67 | 68 | 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 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /open_manipulator_ar_markers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | open_manipulator_ar_markers 4 | 1.0.0 5 | 6 | To find AR marker, it depends ar_track_alvar packages 7 | 8 | Apache 2.0 9 | Darby Lim 10 | Pyo 11 | http://wiki.ros.org/open_manipulator_ar_markers 12 | http://emanual.robotis.com/docs/en/platform/openmanipulator 13 | https://github.com/ROBOTIS-GIT/open_manipulator_perceptions 14 | https://github.com/ROBOTIS-GIT/open_manipulator_perceptions/issues 15 | catkin 16 | ar_track_alvar 17 | ar_track_alvar_msgs 18 | image_transport 19 | image_proc 20 | 21 | -------------------------------------------------------------------------------- /open_manipulator_ar_markers/rviz/ar_marker.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.5 9 | Tree Height: 408 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Camera 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/TF 55 | Enabled: true 56 | Frame Timeout: 15 57 | Frames: 58 | All Enabled: false 59 | camera_color_frame: 60 | Value: true 61 | camera_color_optical_frame: 62 | Value: false 63 | camera_depth_frame: 64 | Value: false 65 | camera_depth_optical_frame: 66 | Value: false 67 | camera_infra1_frame: 68 | Value: false 69 | camera_infra1_optical_frame: 70 | Value: false 71 | camera_infra2_frame: 72 | Value: false 73 | camera_infra2_optical_frame: 74 | Value: false 75 | camera_link: 76 | Value: true 77 | end_effector_link: 78 | Value: false 79 | gripper_link: 80 | Value: false 81 | gripper_link_sub: 82 | Value: false 83 | link1: 84 | Value: false 85 | link2: 86 | Value: false 87 | link3: 88 | Value: false 89 | link4: 90 | Value: false 91 | link5: 92 | Value: false 93 | world: 94 | Value: false 95 | Marker Scale: 0.20000000298023224 96 | Name: TF 97 | Show Arrows: true 98 | Show Axes: true 99 | Show Names: true 100 | Tree: 101 | world: 102 | link1: 103 | link2: 104 | link3: 105 | link4: 106 | link5: 107 | camera_link: 108 | camera_color_frame: 109 | camera_color_optical_frame: 110 | {} 111 | camera_depth_frame: 112 | camera_depth_optical_frame: 113 | {} 114 | camera_infra1_frame: 115 | camera_infra1_optical_frame: 116 | {} 117 | camera_infra2_frame: 118 | camera_infra2_optical_frame: 119 | {} 120 | end_effector_link: 121 | {} 122 | gripper_link: 123 | {} 124 | gripper_link_sub: 125 | {} 126 | Update Interval: 0 127 | Value: true 128 | - Class: rviz/Camera 129 | Enabled: true 130 | Image Rendering: background and overlay 131 | Image Topic: /camera/color/image_raw 132 | Name: Camera 133 | Overlay Alpha: 0.5 134 | Queue Size: 10 135 | Transport Hint: raw 136 | Unreliable: false 137 | Value: true 138 | Visibility: 139 | Grid: true 140 | Image: true 141 | Marker: true 142 | PointCloud2: true 143 | RobotModel: true 144 | TF: true 145 | Value: true 146 | Zoom Factor: 1 147 | - Class: rviz/Image 148 | Enabled: false 149 | Image Topic: /raspicam_node/image_raw 150 | Max Value: 1 151 | Median window: 5 152 | Min Value: 0 153 | Name: Image 154 | Normalize Range: true 155 | Queue Size: 2 156 | Transport Hint: raw 157 | Unreliable: false 158 | Value: false 159 | - Alpha: 1 160 | Autocompute Intensity Bounds: true 161 | Autocompute Value Bounds: 162 | Max Value: 10 163 | Min Value: -10 164 | Value: true 165 | Axis: Z 166 | Channel Name: intensity 167 | Class: rviz/PointCloud2 168 | Color: 255; 255; 255 169 | Color Transformer: Intensity 170 | Decay Time: 0 171 | Enabled: false 172 | Invert Rainbow: false 173 | Max Color: 255; 255; 255 174 | Max Intensity: 4096 175 | Min Color: 0; 0; 0 176 | Min Intensity: 0 177 | Name: PointCloud2 178 | Position Transformer: XYZ 179 | Queue Size: 10 180 | Selectable: true 181 | Size (Pixels): 3 182 | Size (m): 0.009999999776482582 183 | Style: Flat Squares 184 | Topic: /camera/depth/points 185 | Unreliable: false 186 | Use Fixed Frame: true 187 | Use rainbow: true 188 | Value: false 189 | - Class: rviz/Marker 190 | Enabled: true 191 | Marker Topic: /visualization_marker 192 | Name: Marker 193 | Namespaces: 194 | {} 195 | Queue Size: 100 196 | Value: true 197 | - Alpha: 1 198 | Class: rviz/RobotModel 199 | Collision Enabled: false 200 | Enabled: true 201 | Links: 202 | All Links Enabled: true 203 | Expand Joint Details: false 204 | Expand Link Details: false 205 | Expand Tree: false 206 | Link Tree Style: Links in Alphabetic Order 207 | end_effector_link: 208 | Alpha: 1 209 | Show Axes: false 210 | Show Trail: false 211 | Value: true 212 | gripper_link: 213 | Alpha: 1 214 | Show Axes: false 215 | Show Trail: false 216 | Value: true 217 | gripper_link_sub: 218 | Alpha: 1 219 | Show Axes: false 220 | Show Trail: false 221 | Value: true 222 | link1: 223 | Alpha: 1 224 | Show Axes: false 225 | Show Trail: false 226 | Value: true 227 | link2: 228 | Alpha: 1 229 | Show Axes: false 230 | Show Trail: false 231 | Value: true 232 | link3: 233 | Alpha: 1 234 | Show Axes: false 235 | Show Trail: false 236 | Value: true 237 | link4: 238 | Alpha: 1 239 | Show Axes: false 240 | Show Trail: false 241 | Value: true 242 | link5: 243 | Alpha: 1 244 | Show Axes: false 245 | Show Trail: false 246 | Value: true 247 | world: 248 | Alpha: 1 249 | Show Axes: false 250 | Show Trail: false 251 | Name: RobotModel 252 | Robot Description: robot_description 253 | TF Prefix: "" 254 | Update Interval: 0 255 | Value: true 256 | Visual Enabled: true 257 | Enabled: true 258 | Global Options: 259 | Background Color: 48; 48; 48 260 | Default Light: true 261 | Fixed Frame: world 262 | Frame Rate: 30 263 | Name: root 264 | Tools: 265 | - Class: rviz/Interact 266 | Hide Inactive Objects: true 267 | - Class: rviz/MoveCamera 268 | - Class: rviz/Select 269 | - Class: rviz/FocusCamera 270 | - Class: rviz/Measure 271 | - Class: rviz/SetInitialPose 272 | Theta std deviation: 0.2617993950843811 273 | Topic: /initialpose 274 | X std deviation: 0.5 275 | Y std deviation: 0.5 276 | - Class: rviz/SetGoal 277 | Topic: /move_base_simple/goal 278 | - Class: rviz/PublishPoint 279 | Single click: true 280 | Topic: /clicked_point 281 | Value: true 282 | Views: 283 | Current: 284 | Class: rviz/Orbit 285 | Distance: 0.16678020358085632 286 | Enable Stereo Rendering: 287 | Stereo Eye Separation: 0.05999999865889549 288 | Stereo Focal Distance: 1 289 | Swap Stereo Eyes: false 290 | Value: false 291 | Focal Point: 292 | X: 0.16570918262004852 293 | Y: 0.025928165763616562 294 | Z: 0.2826584279537201 295 | Focal Shape Fixed Size: true 296 | Focal Shape Size: 0.05000000074505806 297 | Invert Z Axis: false 298 | Name: Current View 299 | Near Clip Distance: 0.009999999776482582 300 | Pitch: 0.4397975206375122 301 | Target Frame: 302 | Value: Orbit (rviz) 303 | Yaw: 0.6253852844238281 304 | Saved: ~ 305 | Window Geometry: 306 | Camera: 307 | collapsed: false 308 | Displays: 309 | collapsed: false 310 | Height: 996 311 | Hide Left Dock: false 312 | Hide Right Dock: true 313 | Image: 314 | collapsed: false 315 | QMainWindow State: 000000ff00000000fd0000000400000000000001db00000346fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001d5000000c900fffffffb0000000c00430061006d00650072006101000002180000016b0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650200000f2d0000014b000001db0000020e000000010000014a0000037afc0200000003fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fe0000003efc0100000002fb0000000800540069006d00650100000000000005fe000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000041d0000034600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 316 | Selection: 317 | collapsed: false 318 | Time: 319 | collapsed: false 320 | Tool Properties: 321 | collapsed: false 322 | Views: 323 | collapsed: true 324 | Width: 1534 325 | X: 2285 326 | Y: 58 327 | -------------------------------------------------------------------------------- /open_manipulator_camera/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # CMake 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(open_manipulator_camera) 6 | 7 | add_compile_options(-std=c++11) 8 | 9 | ################################################################################ 10 | # Packages 11 | ################################################################################ 12 | find_package(catkin REQUIRED COMPONENTS 13 | image_transport 14 | image_proc 15 | ) 16 | 17 | ################################################################################ 18 | # Declare ROS messages, services and actions 19 | ################################################################################ 20 | 21 | ################################################################################ 22 | # Declare ROS dynamic reconfigure parameters 23 | ################################################################################ 24 | 25 | ################################################################################ 26 | # Catkin specific configuration 27 | ################################################################################ 28 | catkin_package( 29 | CATKIN_DEPENDS image_transport image_proc 30 | ) 31 | 32 | ################################################################################ 33 | # Build 34 | ################################################################################ 35 | include_directories( 36 | ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | ################################################################################ 40 | # Install 41 | ################################################################################ 42 | 43 | install(DIRECTORY launch 44 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 45 | ) 46 | 47 | ################################################################################ 48 | # Test 49 | ################################################################################ 50 | -------------------------------------------------------------------------------- /open_manipulator_camera/camera_info/astra_pro.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: rgb_Astra_Orbbec 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [638.5487821295111, 0, 352.1179468111886, 0, 633.6917202957546, 233.0611548320362, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.2066256521967435, -0.1773042991130427, 0.002233198741188607, 0.02290147252523488, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [667.1920166015625, 0, 362.5443404081379, 0, 0, 675.0497436523438, 233.9594635486083, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /open_manipulator_camera/camera_info/raspicam.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: raspicam 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [499.7530783571702, 0, 316.587604963782, 0, 497.1254098843589, 244.4671143837159, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.1875667734616699, -0.308355301175031, -0.001445054342809013, -0.003001227280700795, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [512.7666015625, 0, 314.6203608421492, 0, 0, 512.2265625, 243.9360354014207, 0, 0, 0, 1, 0] 21 | -------------------------------------------------------------------------------- /open_manipulator_camera/launch/astra_pro.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /open_manipulator_camera/launch/raspicam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /open_manipulator_camera/launch/realsense_d435.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /open_manipulator_camera/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | open_manipulator_camera 4 | 0.1.1 5 | 6 | This packages includes some camera launch files 7 | 8 | Apache 2.0 9 | Yong-Ho Na 10 | Darby Lim 11 | Pyo 12 | https://github.com/ROBOTIS-GIT/open_manipulator_perceptions/issues 13 | https://github.com/ROBOTIS-GIT/open_manipulator_perceptions 14 | http://emanual.robotis.com/docs/en/platform/openmanipulator 15 | catkin 16 | image_transport 17 | image_proc 18 | 19 | -------------------------------------------------------------------------------- /open_manipulator_perceptions/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package open_manipulator_perceptions 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2018-06-01) 6 | ------------------ 7 | * added new open_manipulator_perceptions package 8 | * added perception packages as AR marker, Object recognition and so on 9 | * Contributors: Darby Lim 10 | -------------------------------------------------------------------------------- /open_manipulator_perceptions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(open_manipulator_perceptions) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /open_manipulator_perceptions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | open_manipulator_perceptions 4 | 1.0.0 5 | 6 | This packages are configured to related perception packages as AR marker, Object recognition and so on. 7 | 8 | Apache 2.0 9 | Darby Lim 10 | Hye-Jong KIM 11 | Ryan Shim 12 | Yong-Ho Na 13 | Pyo 14 | http://wiki.ros.org/open_manipulator_perceptions 15 | http://emanual.robotis.com/docs/en/platform/openmanipulator 16 | https://github.com/ROBOTIS-GIT/open_manipulator_perceptions 17 | https://github.com/ROBOTIS-GIT/open_manipulator_perceptions/issues 18 | catkin 19 | open_manipulator_ar_markers 20 | open_manipulator_camera 21 | open_manipulator_pick_and_place 22 | 23 | 24 | -------------------------------------------------------------------------------- /open_manipulator_pick_and_place/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(open_manipulator_pick_and_place) 6 | 7 | add_compile_options(-std=c++11) 8 | 9 | ################################################################################ 10 | # Find catkin packages and libraries for catkin and system dependencies 11 | ################################################################################ 12 | find_package(catkin REQUIRED 13 | COMPONENTS 14 | roscpp 15 | sensor_msgs 16 | open_manipulator_msgs 17 | ar_track_alvar_msgs 18 | ) 19 | 20 | ################################################################################ 21 | # Setup for python modules and scripts 22 | ################################################################################ 23 | 24 | ################################################################################ 25 | # Declare ROS messages, services and actions 26 | ################################################################################ 27 | 28 | ################################################################################ 29 | ## Declare ROS dynamic reconfigure parameters 30 | ################################################################################ 31 | 32 | ################################################################################ 33 | # Declare catkin specific configuration to be passed to dependent projects 34 | ################################################################################ 35 | catkin_package( 36 | INCLUDE_DIRS include 37 | CATKIN_DEPENDS 38 | roscpp 39 | sensor_msgs 40 | open_manipulator_msgs 41 | ar_track_alvar_msgs 42 | ) 43 | 44 | ################################################################################ 45 | # Build 46 | ################################################################################ 47 | include_directories( 48 | include 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | 52 | add_executable(open_manipulator_pick_and_place src/open_manipulator_pick_and_place.cpp) 53 | add_dependencies(open_manipulator_pick_and_place ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 54 | target_link_libraries(open_manipulator_pick_and_place ${catkin_LIBRARIES} ) 55 | 56 | ################################################################################ 57 | # Install 58 | ################################################################################ 59 | install(TARGETS open_manipulator_pick_and_place 60 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 61 | ) 62 | 63 | install(DIRECTORY include/${PROJECT_NAME}/ 64 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 65 | ) 66 | 67 | install(DIRECTORY launch 68 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 69 | ) 70 | 71 | ################################################################################ 72 | # Test 73 | ################################################################################ 74 | -------------------------------------------------------------------------------- /open_manipulator_pick_and_place/include/open_manipulator_pick_and_place/open_manipulator_pick_and_place.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ 18 | 19 | #ifndef OPEN_MANIPULATOR_PICK_AND_PLACE_H 20 | #define OPEN_MANIPULATOR_PICK_AND_PLACE_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "open_manipulator_msgs/OpenManipulatorState.h" 27 | #include "open_manipulator_msgs/KinematicsPose.h" 28 | #include "open_manipulator_msgs/SetJointPosition.h" 29 | #include "open_manipulator_msgs/SetKinematicsPose.h" 30 | 31 | #include "ar_track_alvar_msgs/AlvarMarkers.h" 32 | #include "sensor_msgs/JointState.h" 33 | 34 | #define NUM_OF_JOINT_AND_TOOL 5 35 | #define HOME_POSE 1 36 | #define DEMO_START 2 37 | #define DEMO_STOP 3 38 | 39 | typedef struct _ArMarker 40 | { 41 | uint32_t id; 42 | double position[3]; 43 | } ArMarker; 44 | 45 | class OpenManipulatorPickandPlace 46 | { 47 | private: 48 | // ROS NodeHandle 49 | ros::NodeHandle node_handle_; 50 | ros::NodeHandle priv_node_handle_; 51 | ros::ServiceClient goal_joint_space_path_client_; 52 | ros::ServiceClient goal_tool_control_client_; 53 | ros::ServiceClient goal_task_space_path_client_; 54 | 55 | ros::Subscriber open_manipulator_states_sub_; 56 | ros::Subscriber open_manipulator_joint_states_sub_; 57 | ros::Subscriber open_manipulator_kinematics_pose_sub_; 58 | ros::Subscriber ar_pose_marker_sub_; 59 | 60 | std::vector present_joint_angle_; 61 | std::vector present_kinematic_position_; 62 | std::vector joint_name_; 63 | bool open_manipulator_is_moving_; 64 | std::vector ar_marker_pose; 65 | 66 | uint8_t mode_state_; 67 | uint8_t demo_count_; 68 | uint8_t pick_ar_id_; 69 | 70 | public: 71 | OpenManipulatorPickandPlace(); 72 | ~OpenManipulatorPickandPlace(); 73 | 74 | void initServiceClient(); 75 | void initSubscribe(); 76 | 77 | void manipulatorStatesCallback(const open_manipulator_msgs::OpenManipulatorState::ConstPtr &msg); 78 | void kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg); 79 | void jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); 80 | void arPoseMarkerCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr &msg); 81 | 82 | bool setJointSpacePath(std::vector joint_name, std::vector joint_angle, double path_time); 83 | bool setToolControl(std::vector joint_angle); 84 | bool setTaskSpacePath(std::vector kinematics_pose, std::vector kienmatics_orientation, double path_time); 85 | 86 | void publishCallback(const ros::TimerEvent&); 87 | void setModeState(char ch); 88 | void demoSequence(); 89 | 90 | void printText(); 91 | bool kbhit(); 92 | }; 93 | 94 | #endif //OPEN_MANIPULATOR_PICK_AND_PLACE_H 95 | -------------------------------------------------------------------------------- /open_manipulator_pick_and_place/launch/open_manipulator_pick_and_place.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /open_manipulator_pick_and_place/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | open_manipulator_pick_and_place 4 | 1.0.0 5 | 6 | Example of OpenManipulator 7 | 8 | Apache 2.0 9 | Darby Lim 10 | Hye-Jong KIM 11 | Ryan Shim 12 | Yong-Ho Na 13 | Pyo 14 | http://wiki.ros.org/open_manipulator_description 15 | http://emanual.robotis.com/docs/en/platform/openmanipulator 16 | https://github.com/ROBOTIS-GIT/open_manipulator 17 | https://github.com/ROBOTIS-GIT/open_manipulator/issues 18 | catkin 19 | roscpp 20 | sensor_msgs 21 | open_manipulator_msgs 22 | ar_track_alvar_msgs 23 | 24 | -------------------------------------------------------------------------------- /open_manipulator_pick_and_place/src/open_manipulator_pick_and_place.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ 18 | 19 | #include "open_manipulator_pick_and_place/open_manipulator_pick_and_place.h" 20 | 21 | OpenManipulatorPickandPlace::OpenManipulatorPickandPlace() 22 | : node_handle_(""), 23 | priv_node_handle_("~"), 24 | mode_state_(0), 25 | demo_count_(0), 26 | pick_ar_id_(0) 27 | { 28 | present_joint_angle_.resize(NUM_OF_JOINT_AND_TOOL, 0.0); 29 | present_kinematic_position_.resize(3, 0.0); 30 | 31 | joint_name_.push_back("joint1"); 32 | joint_name_.push_back("joint2"); 33 | joint_name_.push_back("joint3"); 34 | joint_name_.push_back("joint4"); 35 | 36 | initServiceClient(); 37 | initSubscribe(); 38 | } 39 | 40 | OpenManipulatorPickandPlace::~OpenManipulatorPickandPlace() 41 | { 42 | if (ros::isStarted()) 43 | { 44 | ros::shutdown(); 45 | ros::waitForShutdown(); 46 | } 47 | } 48 | 49 | void OpenManipulatorPickandPlace::initServiceClient() 50 | { 51 | goal_joint_space_path_client_ = node_handle_.serviceClient("goal_joint_space_path"); 52 | goal_tool_control_client_ = node_handle_.serviceClient("goal_tool_control"); 53 | goal_task_space_path_client_ = node_handle_.serviceClient("goal_task_space_path"); 54 | } 55 | 56 | void OpenManipulatorPickandPlace::initSubscribe() 57 | { 58 | open_manipulator_states_sub_ = node_handle_.subscribe("states", 10, &OpenManipulatorPickandPlace::manipulatorStatesCallback, this); 59 | open_manipulator_joint_states_sub_ = node_handle_.subscribe("joint_states", 10, &OpenManipulatorPickandPlace::jointStatesCallback, this); 60 | open_manipulator_kinematics_pose_sub_ = node_handle_.subscribe("gripper/kinematics_pose", 10, &OpenManipulatorPickandPlace::kinematicsPoseCallback, this); 61 | ar_pose_marker_sub_ = node_handle_.subscribe("/ar_pose_marker", 10, &OpenManipulatorPickandPlace::arPoseMarkerCallback, this); 62 | } 63 | 64 | bool OpenManipulatorPickandPlace::setJointSpacePath(std::vector joint_name, std::vector joint_angle, double path_time) 65 | { 66 | open_manipulator_msgs::SetJointPosition srv; 67 | srv.request.joint_position.joint_name = joint_name; 68 | srv.request.joint_position.position = joint_angle; 69 | srv.request.path_time = path_time; 70 | 71 | if (goal_joint_space_path_client_.call(srv)) 72 | { 73 | return srv.response.is_planned; 74 | } 75 | return false; 76 | } 77 | 78 | bool OpenManipulatorPickandPlace::setToolControl(std::vector joint_angle) 79 | { 80 | open_manipulator_msgs::SetJointPosition srv; 81 | srv.request.joint_position.joint_name.push_back("gripper"); 82 | srv.request.joint_position.position = joint_angle; 83 | 84 | if (goal_tool_control_client_.call(srv)) 85 | { 86 | return srv.response.is_planned; 87 | } 88 | return false; 89 | } 90 | 91 | bool OpenManipulatorPickandPlace::setTaskSpacePath(std::vector kinematics_pose,std::vector kienmatics_orientation, double path_time) 92 | { 93 | open_manipulator_msgs::SetKinematicsPose srv; 94 | 95 | srv.request.end_effector_name = "gripper"; 96 | 97 | srv.request.kinematics_pose.pose.position.x = kinematics_pose.at(0); 98 | srv.request.kinematics_pose.pose.position.y = kinematics_pose.at(1); 99 | srv.request.kinematics_pose.pose.position.z = kinematics_pose.at(2); 100 | 101 | srv.request.kinematics_pose.pose.orientation.w = kienmatics_orientation.at(0); 102 | srv.request.kinematics_pose.pose.orientation.x = kienmatics_orientation.at(1); 103 | srv.request.kinematics_pose.pose.orientation.y = kienmatics_orientation.at(2); 104 | srv.request.kinematics_pose.pose.orientation.z = kienmatics_orientation.at(3); 105 | 106 | srv.request.path_time = path_time; 107 | 108 | if (goal_task_space_path_client_.call(srv)) 109 | { 110 | return srv.response.is_planned; 111 | } 112 | return false; 113 | } 114 | 115 | void OpenManipulatorPickandPlace::manipulatorStatesCallback(const open_manipulator_msgs::OpenManipulatorState::ConstPtr &msg) 116 | { 117 | if (msg->open_manipulator_moving_state == msg->IS_MOVING) 118 | open_manipulator_is_moving_ = true; 119 | else 120 | open_manipulator_is_moving_ = false; 121 | } 122 | 123 | void OpenManipulatorPickandPlace::jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) 124 | { 125 | std::vector temp_angle; 126 | temp_angle.resize(NUM_OF_JOINT_AND_TOOL); 127 | for (int i = 0; i < msg->name.size(); i ++) 128 | { 129 | if (!msg->name.at(i).compare("joint1")) temp_angle.at(0) = (msg->position.at(i)); 130 | else if (!msg->name.at(i).compare("joint2")) temp_angle.at(1) = (msg->position.at(i)); 131 | else if (!msg->name.at(i).compare("joint3")) temp_angle.at(2) = (msg->position.at(i)); 132 | else if (!msg->name.at(i).compare("joint4")) temp_angle.at(3) = (msg->position.at(i)); 133 | else if (!msg->name.at(i).compare("gripper")) temp_angle.at(4) = (msg->position.at(i)); 134 | } 135 | present_joint_angle_ = temp_angle; 136 | } 137 | 138 | void OpenManipulatorPickandPlace::kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg) 139 | { 140 | std::vector temp_position; 141 | temp_position.push_back(msg->pose.position.x); 142 | temp_position.push_back(msg->pose.position.y); 143 | temp_position.push_back(msg->pose.position.z); 144 | 145 | present_kinematic_position_ = temp_position; 146 | } 147 | 148 | void OpenManipulatorPickandPlace::arPoseMarkerCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr &msg) 149 | { 150 | std::vector temp_buffer; 151 | for (int i = 0; i < msg->markers.size(); i ++) 152 | { 153 | ArMarker temp; 154 | temp.id = msg->markers.at(i).id; 155 | temp.position[0] = msg->markers.at(i).pose.pose.position.x; 156 | temp.position[1] = msg->markers.at(i).pose.pose.position.y; 157 | temp.position[2] = msg->markers.at(i).pose.pose.position.z; 158 | 159 | temp_buffer.push_back(temp); 160 | } 161 | 162 | ar_marker_pose = temp_buffer; 163 | } 164 | 165 | void OpenManipulatorPickandPlace::publishCallback(const ros::TimerEvent&) 166 | { 167 | printText(); 168 | if (kbhit()) setModeState(std::getchar()); 169 | 170 | if (mode_state_ == HOME_POSE) 171 | { 172 | std::vector joint_angle; 173 | 174 | joint_angle.push_back( 0.00); 175 | joint_angle.push_back(-1.05); 176 | joint_angle.push_back( 0.35); 177 | joint_angle.push_back( 0.70); 178 | setJointSpacePath(joint_name_, joint_angle, 2.0); 179 | 180 | std::vector gripper_value; 181 | gripper_value.push_back(0.0); 182 | setToolControl(gripper_value); 183 | mode_state_ = 0; 184 | } 185 | else if (mode_state_ == DEMO_START) 186 | { 187 | if (!open_manipulator_is_moving_) demoSequence(); 188 | } 189 | else if (mode_state_ == DEMO_STOP) 190 | { 191 | 192 | } 193 | } 194 | void OpenManipulatorPickandPlace::setModeState(char ch) 195 | { 196 | if (ch == '1') 197 | mode_state_ = HOME_POSE; 198 | else if (ch == '2') 199 | { 200 | mode_state_ = DEMO_START; 201 | demo_count_ = 0; 202 | } 203 | else if (ch == '3') 204 | mode_state_ = DEMO_STOP; 205 | } 206 | 207 | void OpenManipulatorPickandPlace::demoSequence() 208 | { 209 | std::vector joint_angle; 210 | std::vector kinematics_position; 211 | std::vector kinematics_orientation; 212 | std::vector gripper_value; 213 | 214 | switch (demo_count_) 215 | { 216 | case 0: // home pose 217 | joint_angle.push_back( 0.00); 218 | joint_angle.push_back(-1.05); 219 | joint_angle.push_back( 0.35); 220 | joint_angle.push_back( 0.70); 221 | setJointSpacePath(joint_name_, joint_angle, 1.5); 222 | demo_count_ ++; 223 | break; 224 | case 1: // initial pose 225 | joint_angle.push_back( 0.01); 226 | joint_angle.push_back(-0.80); 227 | joint_angle.push_back( 0.00); 228 | joint_angle.push_back( 1.90); 229 | setJointSpacePath(joint_name_, joint_angle, 1.0); 230 | demo_count_ ++; 231 | break; 232 | case 2: // wait & open the gripper 233 | setJointSpacePath(joint_name_, present_joint_angle_, 3.0); 234 | gripper_value.push_back(0.010); 235 | setToolControl(gripper_value); 236 | demo_count_ ++; 237 | break; 238 | case 3: // pick the box 239 | for (int i = 0; i < ar_marker_pose.size(); i ++) 240 | { 241 | if (ar_marker_pose.at(i).id == pick_ar_id_) 242 | { 243 | kinematics_position.push_back(ar_marker_pose.at(i).position[0]); 244 | kinematics_position.push_back(ar_marker_pose.at(i).position[1]); 245 | kinematics_position.push_back(0.05); 246 | kinematics_orientation.push_back(0.74); 247 | kinematics_orientation.push_back(0.00); 248 | kinematics_orientation.push_back(0.66); 249 | kinematics_orientation.push_back(0.00); 250 | setTaskSpacePath(kinematics_position, kinematics_orientation, 2.0); 251 | demo_count_ ++; 252 | return; 253 | } 254 | } 255 | demo_count_ = 2; // If the detection fails. 256 | break; 257 | case 4: // wait & grip 258 | setJointSpacePath(joint_name_, present_joint_angle_, 1.0); 259 | gripper_value.push_back(-0.002); 260 | setToolControl(gripper_value); 261 | demo_count_ ++; 262 | break; 263 | case 5: // initial pose 264 | joint_angle.push_back( 0.01); 265 | joint_angle.push_back(-0.80); 266 | joint_angle.push_back( 0.00); 267 | joint_angle.push_back( 1.90); 268 | setJointSpacePath(joint_name_, joint_angle, 1.0); 269 | demo_count_ ++; 270 | break; 271 | case 6: // place pose 272 | joint_angle.push_back( 1.57); 273 | joint_angle.push_back(-0.21); 274 | joint_angle.push_back(-0.15); 275 | joint_angle.push_back( 1.89); 276 | setJointSpacePath(joint_name_, joint_angle, 1.0); 277 | demo_count_ ++; 278 | break; 279 | case 7: // place the box 280 | kinematics_position.push_back(present_kinematic_position_.at(0)); 281 | kinematics_position.push_back(present_kinematic_position_.at(1)); 282 | if (pick_ar_id_ == 0) kinematics_position.push_back(present_kinematic_position_.at(2)-0.076); 283 | else if (pick_ar_id_ == 1) kinematics_position.push_back(present_kinematic_position_.at(2)-0.041); 284 | else if (pick_ar_id_ == 2) kinematics_position.push_back(present_kinematic_position_.at(2)-0.006); 285 | kinematics_orientation.push_back(0.74); 286 | kinematics_orientation.push_back(0.00); 287 | kinematics_orientation.push_back(0.66); 288 | kinematics_orientation.push_back(0.00); 289 | setTaskSpacePath(kinematics_position, kinematics_orientation, 2.0); 290 | demo_count_ ++; 291 | break; 292 | case 8: // wait & place 293 | setJointSpacePath(joint_name_, present_joint_angle_, 1.0); 294 | gripper_value.push_back(0.010); 295 | setToolControl(gripper_value); 296 | demo_count_ ++; 297 | break; 298 | case 9: // move up after place the box 299 | kinematics_position.push_back(present_kinematic_position_.at(0)); 300 | kinematics_position.push_back(present_kinematic_position_.at(1)); 301 | kinematics_position.push_back(0.135); 302 | kinematics_orientation.push_back(0.74); 303 | kinematics_orientation.push_back(0.00); 304 | kinematics_orientation.push_back(0.66); 305 | kinematics_orientation.push_back(0.00); 306 | setTaskSpacePath(kinematics_position, kinematics_orientation, 2.0); 307 | demo_count_ ++; 308 | break; 309 | case 10: // home pose 310 | joint_angle.push_back( 0.00); 311 | joint_angle.push_back(-1.05); 312 | joint_angle.push_back( 0.35); 313 | joint_angle.push_back( 0.70); 314 | setJointSpacePath(joint_name_, joint_angle, 1.5); 315 | demo_count_ = 1; 316 | if (pick_ar_id_ == 0) pick_ar_id_ = 1; 317 | else if (pick_ar_id_ == 1) pick_ar_id_ = 2; 318 | else if (pick_ar_id_ == 2) 319 | { 320 | pick_ar_id_ = 0; 321 | demo_count_ = 0; 322 | mode_state_ = DEMO_STOP; 323 | } 324 | break; 325 | } // end of switch-case 326 | } 327 | 328 | 329 | void OpenManipulatorPickandPlace::printText() 330 | { 331 | system("clear"); 332 | 333 | printf("\n"); 334 | printf("-----------------------------\n"); 335 | printf("Pick and Place demonstration!\n"); 336 | printf("-----------------------------\n"); 337 | 338 | printf("1 : Home pose\n"); 339 | printf("2 : Pick and Place demo. start\n"); 340 | printf("3 : Pick and Place demo. Stop\n"); 341 | 342 | printf("-----------------------------\n"); 343 | 344 | if (mode_state_ == DEMO_START) 345 | { 346 | switch(demo_count_) 347 | { 348 | case 1: // home pose 349 | printf("Move home pose\n"); 350 | break; 351 | case 2: // initial pose 352 | printf("Move initial pose\n"); 353 | break; 354 | case 3: 355 | printf("Detecting...\n"); 356 | break; 357 | case 4: 358 | case 5: 359 | case 6: 360 | printf("Pick the box\n"); 361 | break; 362 | case 7: 363 | case 8: 364 | case 9: 365 | case 10: 366 | printf("Place the box \n"); 367 | break; 368 | } 369 | } 370 | else if (mode_state_ == DEMO_STOP) 371 | { 372 | printf("The end of demo\n"); 373 | } 374 | 375 | printf("-----------------------------\n"); 376 | printf("Present Joint Angle J1: %.3lf J2: %.3lf J3: %.3lf J4: %.3lf\n", 377 | present_joint_angle_.at(0), 378 | present_joint_angle_.at(1), 379 | present_joint_angle_.at(2), 380 | present_joint_angle_.at(3)); 381 | printf("Present Tool Position: %.3lf\n", present_joint_angle_.at(4)); 382 | printf("Present Kinematics Position X: %.3lf Y: %.3lf Z: %.3lf\n", 383 | present_kinematic_position_.at(0), 384 | present_kinematic_position_.at(1), 385 | present_kinematic_position_.at(2)); 386 | printf("-----------------------------\n"); 387 | 388 | if (ar_marker_pose.size()) printf("AR marker detected.\n"); 389 | for (int i = 0; i < ar_marker_pose.size(); i ++) 390 | { 391 | printf("ID: %d --> X: %.3lf\tY: %.3lf\tZ: %.3lf\n", 392 | ar_marker_pose.at(i).id, 393 | ar_marker_pose.at(i).position[0], 394 | ar_marker_pose.at(i).position[1], 395 | ar_marker_pose.at(i).position[2]); 396 | } 397 | } 398 | 399 | bool OpenManipulatorPickandPlace::kbhit() 400 | { 401 | termios term; 402 | tcgetattr(0, &term); 403 | 404 | termios term2 = term; 405 | term2.c_lflag &= ~ICANON; 406 | tcsetattr(0, TCSANOW, &term2); 407 | 408 | int byteswaiting; 409 | ioctl(0, FIONREAD, &byteswaiting); 410 | tcsetattr(0, TCSANOW, &term); 411 | return byteswaiting > 0; 412 | } 413 | 414 | int main(int argc, char **argv) 415 | { 416 | // Init ROS node 417 | ros::init(argc, argv, "open_manipulator_pick_and_place"); 418 | ros::NodeHandle node_handle(""); 419 | 420 | OpenManipulatorPickandPlace open_manipulator_pick_and_place; 421 | 422 | ros::Timer publish_timer = node_handle.createTimer(ros::Duration(0.100)/*100ms*/, &OpenManipulatorPickandPlace::publishCallback, &open_manipulator_pick_and_place); 423 | 424 | while (ros::ok()) 425 | { 426 | ros::spinOnce(); 427 | } 428 | return 0; 429 | } 430 | --------------------------------------------------------------------------------