├── .github └── workflows │ └── ci.yaml ├── .gitignore ├── Dockerfile ├── LICENSE ├── README.md ├── autodock_core ├── CMakeLists.txt ├── action │ └── AutoDocking.action ├── launch │ └── autodock_server.launch ├── package.xml ├── scripts │ ├── autodock_core │ │ ├── __init__.py │ │ ├── autodock_server.py │ │ └── autodock_utils.py │ └── simple_autodock.py ├── setup.py └── src │ └── ObstacleObserver.cpp ├── autodock_examples ├── CMakeLists.txt ├── configs │ ├── mock_robot.yaml │ └── turtlebot3.yaml ├── package.xml ├── rviz │ ├── default.rviz │ └── tb3_nav.rviz └── scripts │ ├── dock_robot_test.py │ └── dock_sim_test.py ├── autodock_sim ├── CMakeLists.txt ├── launch │ ├── dock_sim.launch │ ├── tb3_dock_sim.launch │ └── tb3_nav_dock_sim.launch ├── maps │ ├── map.pgm │ └── map.yaml ├── models │ ├── Fiducial10 │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── Fiducial10.material │ │ │ └── textures │ │ │ │ └── 6x6_1000-10.png │ │ ├── model.config │ │ └── model.sdf │ ├── Fiducial11 │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── Fiducial11.material │ │ │ └── textures │ │ │ │ └── 6x6_1000-11.png │ │ ├── model.config │ │ └── model.sdf │ ├── Fiducial20 │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── Fiducial20.material │ │ │ └── textures │ │ │ │ └── 6x6_1000-20.png │ │ ├── model.config │ │ └── model.sdf │ ├── MiniMockCharger │ │ ├── model.config │ │ └── model.sdf │ ├── MockCharger │ │ ├── model.config │ │ └── model.sdf │ └── MockRobot │ │ ├── meshes │ │ ├── FatWheel.mtl │ │ ├── FatWheel.obj │ │ ├── MockRobot.mtl │ │ ├── MockRobot.obj │ │ ├── MockRobot_Diffuse.png │ │ ├── SmallWheel.mtl │ │ └── SmallWheel.obj │ │ ├── model.config │ │ ├── model.sdf │ │ └── thumbnails │ │ ├── 1.png │ │ ├── 2.png │ │ ├── 3.png │ │ ├── 4.png │ │ └── 5.png ├── package.xml ├── src │ └── ChargingStationPlugin.cpp └── worlds │ ├── charging.world │ └── tb3_charging.world └── docs ├── architecture.png ├── dock_gz_sim.gif ├── state_diagram.png └── tb3_dock_sim.gif /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: ci 2 | on: 3 | push: 4 | branches: [ main, develop ] 5 | pull_request: 6 | branches: [ main, develop ] 7 | jobs: 8 | build-docker-image: 9 | name: build-docker-image 10 | runs-on: ubuntu-latest 11 | steps: 12 | - uses: actions/checkout@v2 13 | - name: Build docker image 14 | uses: docker/build-push-action@v2 15 | with: 16 | file: ./Dockerfile 17 | push: false 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | *.py[cod] 3 | *$py.class 4 | .vscode/* 5 | .log -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | # only copy autodock_core and autodock_examples repo 4 | COPY autodock_core /root/catkin_ws/src/autodock_core 5 | COPY autodock_examples /root/catkin_ws/src/autodock_examples 6 | 7 | SHELL ["bash", "-c"] 8 | 9 | # add install deps 10 | RUN apt-get update && apt-get install \ 11 | git -y 12 | 13 | # install ros fiducial repo 14 | RUN cd /root/catkin_ws/src && \ 15 | git clone https://github.com/UbiquityRobotics/fiducials.git 16 | 17 | # install dependencies 18 | # Note: force return as true, as fiducial has some non python3 deps 19 | # https://github.com/UbiquityRobotics/fiducials/issues/252 20 | RUN cd /root/catkin_ws && \ 21 | apt-get update && \ 22 | rosdep update && \ 23 | rosdep install --from-paths src --ignore-src -yr || true 24 | 25 | # build repo 26 | RUN . /ros_entrypoint.sh && cd /root/catkin_ws && \ 27 | catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release && \ 28 | sed -i '$isource "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh 29 | 30 | ENTRYPOINT ["/ros_entrypoint.sh"] 31 | CMD ["bash"] 32 | -------------------------------------------------------------------------------- /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 | # autodock 2 | 3 | ![](https://github.com/osrf/autodock/workflows/ci/badge.svg) 4 | 5 | ROS packages for automatic docking 6 | 7 | `autodock` a state machine based auto docking solution for differential-drive robot, 8 | allows accurate and reliable docking. It utilizes 3 fiducial markers to locate the 9 | position of the docking station. Hence, the Robot should equip with a camera input 10 | for fiducial marker detection. Note that, this package works on top of the navigation 11 | stack, not making any alteration to the robot's nav stack. The solution is fully 12 | tested on multiple simulated and actual robots. 13 | 14 | ![](docs/dock_gz_sim.gif) ![](docs/tb3_dock_sim.gif) 15 | 16 | Packages: 17 | - `autodock_core`: Core autodock scripts and lib 18 | - `autodock_examples`: Examples for autodock 19 | - `autodock_sim`: Autodock Simulation 20 | 21 | ## Installation 22 | 23 | [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) is used during development. 24 | 25 | Dependencies 26 | - [Fiducial](https://github.com/UbiquityRobotics/fiducials) 27 | - [Turtlebot Simulation](http://wiki.ros.org/turtlebot3_simulations) ** Optional 28 | 29 | ```bash 30 | # First, clone repos and deps to 'catkin_ws/src', then install 31 | cd catkin_ws 32 | rosdep update && rosdep install --from-paths src --ignore-src -yr 33 | catkin_make 34 | ``` 35 | ## Architecture Diagram 36 | 37 | ![](docs/architecture.png) 38 | 39 | ## State Machine Diagram 40 | 41 | ![](docs/state_diagram.png) 42 | 43 | --- 44 | 45 | ## Run Examples on Simulation with MockRobot 46 | 47 | Run `MockRobot` in gazebo world 48 | ```bash 49 | roslaunch autodock_sim dock_sim.launch 50 | ``` 51 | 52 | try to send an action goal request 53 | ```bash 54 | rostopic pub /autodock_action/goal autodock_core/AutoDockingActionGoal {} --once 55 | ``` 56 | 57 | Now you will see the robot starts to execute a series of autodocking sequence. 58 | 59 | > To explain..... when an action goal `AutoDockingActionGoal` is received by the robot, 60 | the robot will start to docking sequence. When it manages to reach the targer 61 | docking station, it will send a `std_srvs/Trigger` to the `MockCharger` charging 62 | station, to activate the charger. The charger will validate if the robot is docked, 63 | then send a "Success" back to the robot if all goes well. 64 | Subsequently, end the entire docking action. 65 | 66 | ```bash 67 | # To remote control the robot: 68 | rosrun teleop_twist_keyboard teleop_twist_keyboard.py 69 | # To cancel an active docking action: 70 | rostopic pub /autodock_action/cancel actionlib_msgs/GoalID {} --once 71 | # To pause an active docking action: 72 | rostopic pub /pause_dock std_msgs/Bool True --once 73 | ``` 74 | 75 | ### More Tests 76 | 77 | This simple smoke test script will randomly move the robot to some random position 78 | in the gazebo world, and initiate the docking sequence. 79 | 80 | ```bash 81 | # indicate the number of "spawn" with the -c arg 82 | rosrun autodock_examples dock_sim_test.py -c 10 83 | ``` 84 | 85 | --- 86 | 87 | ## Run with turtlebot3 88 | 89 | This demonstrates front dock with turtlebot3, attached with a front camera. 90 | A smaller docking station is used here. 91 | 92 | - dependency: `sudo apt install ros-noetic-turtlebot3-gazebo` 93 | 94 | ```bash 95 | roslaunch autodock_sim tb3_dock_sim.launch 96 | rostopic pub /autodock_action/goal autodock_core/AutoDockingActionGoal {} --once 97 | ``` 98 | 99 | --- 100 | 101 | ## Run Turtlebot3 with navigation 102 | 103 | This demo depends on [turtlebot3_simulation](https://github.com/ROBOTIS-GIT/turtlebot3_simulations) 104 | and [move_base](http://wiki.ros.org/move_base). 105 | Please ensures that you have all dependencies are fully installed. 106 | 107 | > Note: You can install all turtlebot simulation related packages by 108 | `sudo apt install ros-noetic-turtlebot3*`, or follow the setup steps in 109 | [Robotis docs](https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/). 110 | Once done, continue to follow the steps to launch and map the world with a `burger` robot. 111 | 112 | Please ensures that entire pipeline of the turtlebot works before proceeding with the 113 | autodock tb3 demo below. Once done with mapping, you can start with the `autodock` simulation: 114 | 115 | ```bash 116 | # 1. launch turtlebot3 gazebo world, and also autodock_server 117 | roslaunch autodock_sim tb3_nav_dock_sim.launch 118 | 119 | # 2. launch navigation stack, with provided map. You can also remap the environment by following the turtlebot sim tutorial 120 | ## New Terminal 121 | export TURTLEBOT3_MODEL=burger 122 | roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/catkin_ws/src/autodock/autodock_sim/maps/map.yaml open_rviz:=0 123 | ``` 124 | 125 | Now, localize the robot and move the robot with rviz. 126 | Move the robot as such that the camera is facing the charging station. 127 | ```bash 128 | # 3. send a dock action. make sure that the robot's camera is facing the charger 129 | ## New Terminal 130 | rostopic pub /autodock_action/goal autodock_core/AutoDockingActionGoal {} --once 131 | ``` 132 | 133 | This will also demonstrate how the simple `obstacle_observer` works. 134 | Try place an obstacle near the robot during docking and see if it pauses. 135 | 136 | > Note: the current `obstacle_observer` will only `pause` if it is 137 | in `predock`, `steer_dock`, or `parralel_correction` state. 138 | 139 | --- 140 | 141 | ### Docker Container 142 | 143 | A docker file is provieded to buid a container for autodock. Follow these steps: 144 | 145 | ```bash 146 | cd catkin_ws/src/autodock 147 | docker build -t osrf/autodock:v1 . 148 | docker run -it --network host osrf/autodock:v1 bash -c "$COMMAND" 149 | ``` 150 | 151 | --- 152 | 153 | ## Notes 154 | 155 | > The initial development of this code was graciously supported by [Kabam Robotics (aka Cognicept)](https://github.com/cognicept-admin). 156 | 157 | ### Future Work 158 | - Advance `ObstacleObserver`: filter occupancy of charging station on costmap. 159 | 160 | ### Stray Commands 161 | 162 | Debug by launching autodock node on a seperate terminal 163 | ```bash 164 | # Terminal 1 165 | roslaunch autodock_sim dock_sim.launch autodock_server:=false 166 | 167 | # Terminal 2, specify config.yaml with the 'autodock_config' arg 168 | roslaunch autodock_core autodock_server.launch \ 169 | autodock_config:=src/autodock/autodock_examples/configs/mock_robot.yaml 170 | ``` 171 | -------------------------------------------------------------------------------- /autodock_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(autodock_core) 3 | 4 | ## Find catkin dependencies 5 | find_package(catkin REQUIRED COMPONENTS actionlib message_generation) 6 | 7 | catkin_python_setup() 8 | 9 | ## Add actions 10 | add_action_files(DIRECTORY action FILES AutoDocking.action) 11 | 12 | ## Generate messages 13 | generate_messages(DEPENDENCIES std_msgs actionlib_msgs) 14 | 15 | ## Define catkin exports 16 | catkin_package( 17 | CATKIN_DEPENDS message_runtime roscpp actionlib 18 | ) 19 | 20 | catkin_install_python(PROGRAMS 21 | scripts/simple_autodock.py 22 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 23 | 24 | install(DIRECTORY launch/ 25 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 26 | ) 27 | 28 | 29 | ## autodock obstacle observer 30 | include_directories(include ${catkin_INCLUDE_DIRS}) 31 | add_executable(obstacle_observer src/ObstacleObserver.cpp) 32 | target_link_libraries(obstacle_observer ${catkin_LIBRARIES}) 33 | add_dependencies( obstacle_observer 34 | ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 35 | 36 | install(TARGETS obstacle_observer 37 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 40 | ) -------------------------------------------------------------------------------- /autodock_core/action/AutoDocking.action: -------------------------------------------------------------------------------- 1 | ###################################################################### 2 | # GOAL 3 | ## docking waypoint heading to the charging station 4 | string docking_waypoint 5 | 6 | --- 7 | ###################################################################### 8 | # RESULT 9 | 10 | ## if docking succeed 11 | bool is_success 12 | 13 | ## verbose description of the result 14 | string status 15 | 16 | --- 17 | ###################################################################### 18 | # FEEDBACK 19 | 20 | ## progress approx, 0 - 1.0 21 | float32 progress 22 | 23 | ## verbose description of the progress 24 | string status 25 | 26 | ## state 27 | uint8 STATE_INVALID = 0 28 | uint8 STATE_IDLE = 1 29 | uint8 STATE_PREDOCK = 2 30 | uint8 STATE_PARALLEL_CORRECTION = 3 31 | uint8 STATE_STEER_DOCK = 4 32 | uint8 STATE_LAST_MILE = 5 33 | uint8 STATE_ACTIVATE_CHARGER = 6 34 | uint8 STATE_RETRY = 7 35 | uint8 STATE_PAUSE = 8 36 | 37 | uint8 state 38 | -------------------------------------------------------------------------------- /autodock_core/launch/autodock_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /autodock_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autodock_core 4 | 0.2.0 5 | Auto docking package 6 | You Liang 7 | BSD 8 | 9 | You Liang 10 | 11 | catkin 12 | 13 | roscpp 14 | actionlib 15 | message_generation 16 | std_msgs 17 | actionlib_msgs 18 | 19 | roscpp 20 | tf2_ros 21 | aruco_detect 22 | actionlib 23 | message_runtime 24 | 25 | 26 | -------------------------------------------------------------------------------- /autodock_core/scripts/autodock_core/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_core/scripts/autodock_core/__init__.py -------------------------------------------------------------------------------- /autodock_core/scripts/autodock_core/autodock_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2021 Open Source Robotics Foundation, Inc. 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 | 18 | import rospy 19 | import tf2_ros 20 | import actionlib 21 | 22 | from autodock_core.autodock_utils import DockState, Pose2D 23 | import autodock_core.autodock_utils as utils 24 | 25 | import numpy as np 26 | from typing import Tuple 27 | 28 | # rosmsgs 29 | from visualization_msgs.msg import Marker 30 | from geometry_msgs.msg import Twist 31 | from nav_msgs.msg import Odometry 32 | from std_msgs.msg import Bool 33 | from autodock_core.msg import AutoDockingAction, AutoDockingFeedback 34 | from autodock_core.msg import AutoDockingGoal, AutoDockingResult 35 | from std_srvs.srv import SetBool 36 | 37 | 38 | ############################################################################## 39 | 40 | 41 | class AutoDockConfig: 42 | """ 43 | Default AutoDockConfig 44 | """ 45 | # General configs 46 | tf_expiry: float 47 | dock_timeout: float 48 | controller_rate: float 49 | # frames and topics 50 | base_link: str 51 | left_marker: str 52 | right_marker: str 53 | # vel profile 54 | linear_vel_range: Tuple[float, float] 55 | angular_vel_range = Tuple[float, float] 56 | max_linear_vel: float # m/s, for parallel.c and steer 57 | min_linear_vel: float # m/s, for lastmile 58 | max_angular_vel: float # rad/s 59 | min_angular_vel: float # rad/s 60 | # stop thresh 61 | stop_yaw_diff: float # radian 62 | stop_trans_diff: float # meters 63 | # debug state 64 | debug_mode: bool 65 | 66 | ############################################################################## 67 | ############################################################################## 68 | 69 | class AutoDockServer: 70 | """ 71 | This AutoDock server is the base class for the AutoDockServer. Here, we 72 | have abstracted all ROS Interfaces. The user is only required to derived 73 | from this class and implement his/her own custom implementation. 74 | """ 75 | 76 | def __init__(self, config: AutoDockConfig, run_server: bool): 77 | rospy.loginfo("Starting AutoDockServer Node") 78 | self.cfg = config 79 | self.run_server = run_server 80 | 81 | # param check 82 | assert (len(self.cfg.linear_vel_range) == 2 and 83 | len(self.cfg.linear_vel_range) == 2 84 | ), "linear and angular vel range should have size of 2" 85 | assert (self.cfg.linear_vel_range[0] < self.cfg.linear_vel_range[1] 86 | ), "linear vel min should be larger than max" 87 | assert (self.cfg.angular_vel_range[0] < self.cfg.angular_vel_range[1] 88 | ), "linear vel min should be larger than max" 89 | 90 | # create_subscriber to tf broadcaster 91 | self.__tfBuffer = tf2_ros.Buffer(cache_time=rospy.Duration(5.0)) 92 | self.__tf_listener = tf2_ros.TransformListener(self.__tfBuffer) 93 | 94 | # create_publisher to cmd_vel 95 | self.__cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 96 | 97 | # create_subscriber to pause dock 98 | rospy.Subscriber("/pause_dock", Bool, self.__pause_dock_cb) 99 | 100 | # debug timer for state machine marker 101 | if self.cfg.debug_mode: 102 | self.__marker_pub = rospy.Publisher('/sm_maker', Marker, queue_size=1) 103 | self.__timer = rospy.Timer(rospy.Duration(0.5), self.__timer_cb) 104 | self.is_pause = False # TODO 105 | 106 | self.dock_state = DockState.INVALID 107 | self.start_time = rospy.Time.now() 108 | self.sleep_period = rospy.Duration(1/self.cfg.controller_rate) 109 | 110 | # create service client to 111 | # 1. enable aruco detections only when action is started 112 | # 2. disable aruco detections when action is idle 113 | # disable on initialize 114 | self.set_aruco_detections(detection_state=False) 115 | if run_server: 116 | self.__as = actionlib.SimpleActionServer( 117 | "autodock_action", 118 | AutoDockingAction, 119 | execute_cb=self.__execute_cb, 120 | auto_start=False) 121 | self.__as.start() 122 | self.feedback_msg = AutoDockingFeedback() 123 | rospy.loginfo("~ Start AutodockServer with ROS Action Server") 124 | 125 | def start(self) -> bool: 126 | """ 127 | Virtual function. This function will be triggered when autodock request 128 | is requested 129 | :return : if action succeeded 130 | """ 131 | rospy.logwarn("Server implementation has not been specified. " 132 | "Do overload the start() function") 133 | return False 134 | 135 | def set_state(self, state: DockState, printout=""): 136 | """ 137 | set state of the auto dock server 138 | :param state: Current DockState 139 | :param printout: Verbose description of the state 140 | """ 141 | state_str = DockState.to_string(state) 142 | rospy.logwarn(f" State: [{state_str}] | {printout}") 143 | self.dock_state = state 144 | if self.run_server: 145 | self.feedback_msg.state = state 146 | self.feedback_msg.progress = DockState.to_percent(state) 147 | self.feedback_msg.status = f"{state_str} | {printout}" 148 | self.__as.publish_feedback(self.feedback_msg) 149 | 150 | def check_cancel(self) -> bool: 151 | """ 152 | Check if to cancel this docking action. This will happen if a 153 | preempt is requested during server mode. or if a timeout is reached. 154 | :return : true if cancel is requested. false as default 155 | """ 156 | if self.run_server and self.__as.is_preempt_requested(): 157 | rospy.logwarn('Preempted Requested!') 158 | return True 159 | 160 | # check if dock_timeout reaches 161 | if (rospy.Time.now() - self.start_time).secs > self.cfg.dock_timeout: 162 | rospy.logwarn('Timeout reaches!') 163 | self.set_state(self.dock_state, "Reach Timeout") 164 | return True 165 | return False 166 | 167 | def publish_cmd(self, linear_vel=0.0, angular_vel=0.0): 168 | """ 169 | Command the robot to move, default param is STOP! 170 | """ 171 | msg = Twist() 172 | msg.linear.x = linear_vel 173 | msg.angular.z = angular_vel 174 | 175 | if (msg.linear.x > self.cfg.linear_vel_range[1]): 176 | msg.linear.x = self.cfg.linear_vel_range[1] 177 | elif(msg.linear.x < self.cfg.linear_vel_range[0]): 178 | msg.linear.x = self.cfg.linear_vel_range[0] 179 | 180 | if (msg.linear.x > self.cfg.angular_vel_range[1]): 181 | msg.linear.x = self.cfg.angular_vel_range[1] 182 | elif(msg.linear.x < self.cfg.angular_vel_range[0]): 183 | msg.linear.x = self.cfg.angular_vel_range[0] 184 | 185 | # print(f" cmd_vel: [{msg.linear.x:.3f}, {msg.angular.z :.3f}]") 186 | self.__cmd_vel_pub.publish(msg) 187 | 188 | def get_odom(self) -> np.ndarray: 189 | """ 190 | Get the current odom of the robot 191 | :return : 4x4 homogenous matrix, None if not avail 192 | """ 193 | try: 194 | return utils.get_mat_from_odom_msg( 195 | rospy.wait_for_message( 196 | "/odom", Odometry, timeout=self.cfg.tf_expiry) 197 | ) 198 | except rospy.exceptions.ROSException: 199 | rospy.logerr(f"Failed to get odom") 200 | return None 201 | 202 | def get_tf(self, 203 | target_link: str, 204 | ref_link=None, 205 | target_time=None) -> np.ndarray: 206 | """ 207 | This will provide the transformation of the marker, 208 | if ref_link is not provided, we will use robot's base_link as ref 209 | :param now : this is a hack fix 210 | :return : 4x4 homogenous matrix, None if not avail 211 | """ 212 | if ref_link is None: 213 | ref_link = self.cfg.base_link 214 | if target_time is None: 215 | target_time = rospy.Time.now() 216 | 217 | try: 218 | return utils.get_mat_from_transfrom_msg( 219 | self.__tfBuffer.lookup_transform( 220 | ref_link, target_link, target_time, 221 | rospy.Duration(self.cfg.tf_expiry)) 222 | ) 223 | except (tf2_ros.LookupException, 224 | tf2_ros.ConnectivityException, 225 | tf2_ros.ExtrapolationException): 226 | rospy.logwarn(f"Failed lookup: {target_link}, from {ref_link}") 227 | return None 228 | 229 | def get_centre_of_side_markers(self, offset=0.0) -> Pose2D: 230 | """ 231 | Get centre tf of both side markers, reference to base_link 232 | :return: tf of the centre [x, y, yaw] 233 | """ 234 | now = rospy.Time.now() 235 | left_tf = self.get_tf(self.cfg.left_marker, target_time=now) 236 | right_tf = self.get_tf(self.cfg.right_marker, target_time=now) 237 | 238 | if (left_tf is None or right_tf is None): 239 | return None 240 | 241 | # Offset is normal to the line, direction is outward to the 242 | # the camera. this is to create a point in front of the goal 243 | return utils.get_centre_tf(left_tf, right_tf, offset) 244 | 245 | def do_pause(self) -> bool: 246 | """ 247 | Blocking function which will pause the action server. 248 | """ 249 | prev_state = self.dock_state 250 | self.set_state(DockState.PAUSE, f"pause action from {prev_state}") 251 | self.publish_cmd() 252 | while not rospy.is_shutdown(): 253 | if self.check_cancel(): 254 | return False 255 | 256 | if not self.is_pause: 257 | self.set_state(prev_state, f"resume action from pause") 258 | return True 259 | 260 | rospy.sleep(self.sleep_period) 261 | exit(0) 262 | 263 | def move_with_odom(self, forward: float) -> bool: 264 | """ 265 | Move robot in linear motion with Odom. Blocking function 266 | :return : success 267 | """ 268 | self.set_state(self.dock_state, f"move robot: {forward:.2f} m") 269 | 270 | _initial_tf = self.get_odom() 271 | if _initial_tf is None: 272 | return False 273 | 274 | # get the tf mat from odom to goal frame 275 | _goal_tf = utils.apply_2d_transform(_initial_tf, (forward, 0, 0)) 276 | 277 | # second mat 278 | while not rospy.is_shutdown(): 279 | if self.check_cancel(): 280 | return False 281 | 282 | if self.is_pause: 283 | if not self.do_pause(): 284 | return False 285 | 286 | _curr_tf = self.get_odom() 287 | if _curr_tf is None: 288 | return False 289 | 290 | dx, dy, dyaw = utils.compute_tf_diff(_curr_tf, _goal_tf) 291 | print(f" current x, y, yaw diff: {dx:.3f} | {dy:.2f} | {dyaw:.2f}") 292 | 293 | if abs(dx) < self.cfg.stop_trans_diff: 294 | rospy.logwarn("Done with move robot") 295 | return True 296 | 297 | # This makes sure that the robot is actually moving linearly 298 | ang_vel = utils.sat_proportional_filter( 299 | dyaw, abs_max=self.cfg.min_angular_vel, factor=0.2) 300 | l_vel = utils.bin_filter(dx, self.cfg.min_linear_vel) 301 | 302 | self.publish_cmd(linear_vel=l_vel, angular_vel=ang_vel) 303 | rospy.sleep(self.sleep_period) 304 | exit(0) 305 | 306 | def rotate_with_odom(self, rotate: float) -> bool: 307 | """ 308 | Spot Rotate the robot with odom. Blocking function 309 | :return : success 310 | """ 311 | self.set_state(self.dock_state, f"Turn robot: {rotate:.2f} rad") 312 | 313 | _initial_tf = self.get_odom() 314 | if _initial_tf is None: 315 | return False 316 | 317 | # get the tf mat from odom to goal frame 318 | _goal_tf = utils.apply_2d_transform(_initial_tf, (0, 0, rotate)) 319 | 320 | while not rospy.is_shutdown(): 321 | if self.check_cancel(): 322 | return False 323 | 324 | if self.is_pause: 325 | if not self.do_pause(): 326 | return False 327 | 328 | _curr_tf = self.get_odom() 329 | if _curr_tf is None: 330 | return False 331 | 332 | dx, dy, dyaw = utils.compute_tf_diff(_curr_tf, _goal_tf) 333 | print(f"current x, y, yaw diff: {dx:.2f} | {dy:.2f} | {dyaw:.2f}") 334 | 335 | if abs(dyaw) < self.cfg.stop_yaw_diff: 336 | rospy.logwarn("Done with rotate robot") 337 | return True 338 | 339 | # publish rotate 340 | ang_vel = utils.sat_proportional_filter( 341 | dyaw, 342 | abs_min=self.cfg.min_angular_vel, 343 | abs_max=self.cfg.max_angular_vel) 344 | self.publish_cmd(angular_vel=ang_vel) 345 | rospy.sleep(self.sleep_period) 346 | exit(0) 347 | 348 | def set_aruco_detections(self, detection_state) -> bool: 349 | """ 350 | Set aruco detections to True or False 351 | :return : success 352 | """ 353 | if self.cfg.debug_mode: 354 | return True 355 | else: 356 | try: 357 | detection_srv_name = "/enable_detections" 358 | rospy.wait_for_service(detection_srv_name, timeout=3.0) 359 | enable_detections_srv = rospy.ServiceProxy(detection_srv_name, SetBool) 360 | resp = enable_detections_srv(detection_state) 361 | rospy.loginfo("Enable detections response: " + resp.message) 362 | return resp.success 363 | except rospy.ServiceException as e: 364 | rospy.logerr("Service call failed: " + str(e)) 365 | 366 | def __pause_dock_cb(self, msg): 367 | self.is_pause = msg.data 368 | 369 | def __execute_cb(self, goal: AutoDockingGoal): 370 | self.start_time = rospy.Time.now() 371 | _result = AutoDockingResult() 372 | _result.is_success = self.start() 373 | _prev_state = DockState.to_string(self.feedback_msg.state) 374 | if _result.is_success: 375 | _duration = rospy.Time.now() - self.start_time 376 | _result.status = f"Succeeded! Took {_duration.secs}s" 377 | self.__as.set_succeeded(_result) 378 | elif self.__as.is_preempt_requested(): 379 | _result.is_success = False 380 | _result.status = f"Cancel during [{_prev_state}], " \ 381 | f"with status: {self.feedback_msg.status}" 382 | self.__as.set_preempted(_result) 383 | self.set_state(DockState.IDLE, "Dock Action is canceled") 384 | else: 385 | _result.is_success = False 386 | _result.status = f"Failed during [{_prev_state}], " \ 387 | f"with status: {self.feedback_msg.status}" 388 | self.__as.set_aborted(_result) 389 | self.set_state(DockState.IDLE, "Failed execute Dock Action") 390 | 391 | def __timer_cb(self, timer): 392 | # This is mainly for debuging 393 | marker = Marker() 394 | marker.header.stamp = rospy.Time.now() 395 | marker.header.frame_id = self.cfg.base_link 396 | marker.type = Marker.TEXT_VIEW_FACING 397 | marker.pose.position.z = 1.1 398 | marker.scale.x = 0.2 399 | marker.scale.y = 0.2 400 | marker.scale.z = 0.2 401 | marker.color.r = 1 402 | marker.color.b = 1 403 | marker.color.a = 1 404 | marker.text = DockState.to_string(self.dock_state) 405 | self.__marker_pub.publish(marker) 406 | # print(" now: ", rospy.Time.now().secs) 407 | -------------------------------------------------------------------------------- /autodock_core/scripts/autodock_core/autodock_utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2021 Open Source Robotics Foundation, Inc. 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 | 18 | import math 19 | from typing import Tuple, List 20 | import numpy as np 21 | 22 | import tf 23 | from tf import transformations as ts 24 | 25 | from geometry_msgs.msg import TransformStamped 26 | from nav_msgs.msg import Odometry 27 | from autodock_core.msg import AutoDockingFeedback 28 | 29 | 30 | ############################################################################## 31 | 32 | Pose2D = Tuple[float, float, float] 33 | 34 | 35 | class DockState: 36 | INVALID = AutoDockingFeedback.STATE_INVALID 37 | IDLE = AutoDockingFeedback.STATE_IDLE 38 | PREDOCK = AutoDockingFeedback.STATE_PREDOCK 39 | PARALLEL_CORRECTION = AutoDockingFeedback.STATE_PARALLEL_CORRECTION 40 | STEER_DOCK = AutoDockingFeedback.STATE_STEER_DOCK 41 | LAST_MILE = AutoDockingFeedback.STATE_LAST_MILE 42 | ACTIVATE_CHARGER = AutoDockingFeedback.STATE_ACTIVATE_CHARGER 43 | RETRY = AutoDockingFeedback.STATE_RETRY 44 | PAUSE = AutoDockingFeedback.STATE_PAUSE 45 | 46 | def to_string(input): 47 | _map = { 48 | DockState.INVALID: 'INVALID', 49 | DockState.IDLE: 'IDLE', 50 | DockState.PREDOCK: 'PREDOCK', 51 | DockState.PARALLEL_CORRECTION: 'PARALLEL_CORRECTION', 52 | DockState.STEER_DOCK: 'STEER_DOCK', 53 | DockState.LAST_MILE: 'LAST_MILE', 54 | DockState.ACTIVATE_CHARGER: 'ACTIVATE_CHARGER', 55 | DockState.RETRY: 'RETRY', 56 | DockState.PAUSE: 'PAUSE' 57 | } 58 | return _map[input] 59 | 60 | def to_percent(input): 61 | """ 62 | Simple util to convert DockState to percent representation, 63 | use in publishing feedback in dock server 64 | """ 65 | _map = { 66 | DockState.IDLE: 0.0, 67 | DockState.PREDOCK: 0.15, 68 | DockState.PARALLEL_CORRECTION: 0.35, 69 | DockState.STEER_DOCK: 0.50, 70 | DockState.LAST_MILE: 0.8, 71 | DockState.ACTIVATE_CHARGER: 0.9, 72 | DockState.RETRY: 0.1, 73 | DockState.PAUSE: 0.1 # TODO 74 | } 75 | return _map[input] 76 | 77 | ############################################################################## 78 | 79 | 80 | def get_2d_inverse(pose_2d: Pose2D) -> Pose2D: 81 | """ 82 | Inverse a 2d transformation, mainly to switch the frame of reference 83 | :param tf1, tf2: 2d transformation 84 | :return: 2d tf of [x, y, yaw] 85 | """ 86 | _tr = (pose_2d[0], pose_2d[1], 0) 87 | _q = ts.quaternion_from_euler(0, 0, pose_2d[2]) 88 | _tf_mat = ts.concatenate_matrices( 89 | ts.translation_matrix(_tr), ts.quaternion_matrix(_q)) 90 | _i_tf_mat = ts.inverse_matrix(_tf_mat) 91 | trans = ts.translation_from_matrix(_i_tf_mat) 92 | euler = ts.euler_from_matrix(_i_tf_mat) 93 | return trans[0], trans[1], euler[2] 94 | 95 | 96 | def get_centre_tf(tf1: np.ndarray, tf2: np.ndarray, offset=0.0) -> Pose2D: 97 | """ 98 | Get centre point of both tf1 and tf2, Note that this output new frame 99 | has a different orientation as the marker, which it's frame is x axis 100 | (pointing out of the marker) is directed towards the robot's camera. 101 | :param tf1, tf2: 4x4 homogenous matrix from tf1 and tf2 102 | :param offset: (optional) offset target point to the normal 103 | :return: 2d tf of the centre [x, y, yaw] 104 | """ 105 | x1, y1, _ = get_2d_pose(tf1) 106 | x2, y2, _ = get_2d_pose(tf2) 107 | _x = (x1 + x2)/2 108 | _y = (y1 + y2)/2 109 | _yaw = -math.atan2((x2 - x1), (y2 - y1)) 110 | _x += math.cos(_yaw)*offset 111 | _y += math.sin(_yaw)*offset 112 | # print("Inverse! ", get_2d_inverse((_x, _y, _yaw))) # for sanity check 113 | return _x, _y, _yaw 114 | 115 | 116 | def get_mat_from_transfrom_msg(msg: TransformStamped) -> np.ndarray: 117 | """ 118 | This will return a homogenous transformation of transform msg 119 | :param : input transform msg 120 | :return : homogenous transformation matrix 121 | """ 122 | _rot = msg.transform.rotation 123 | _q = (_rot.x, _rot.y, _rot.z, _rot.w) 124 | 125 | _trans = msg.transform.translation 126 | _tr = (_trans.x, _trans.y, _trans.z) 127 | _tf_mat = ts.concatenate_matrices( 128 | ts.translation_matrix(_tr), ts.quaternion_matrix(_q)) 129 | return _tf_mat 130 | 131 | 132 | def get_mat_from_odom_msg(msg: Odometry) -> np.ndarray: 133 | """ 134 | This will return a homogenous transformation of odom pose msg 135 | :param : input odom msg 136 | :return : homogenous transformation matrix 137 | """ 138 | _rot = msg.pose.pose.orientation 139 | _q = (_rot.x, _rot.y, _rot.z, _rot.w) 140 | _trans = msg.pose.pose.position 141 | _tr = (_trans.x, _trans.y, _trans.z) 142 | _tf_mat = ts.concatenate_matrices( 143 | ts.translation_matrix(_tr), ts.quaternion_matrix(_q)) 144 | return _tf_mat 145 | 146 | 147 | def get_2d_pose(_tf: np.ndarray) -> Pose2D: 148 | """ 149 | :param: input homogenous matrix 150 | :return : 2dPose in x, y, yaw format 151 | """ 152 | trans = ts.translation_from_matrix(_tf) 153 | euler = ts.euler_from_matrix(_tf) 154 | return trans[0], trans[1], euler[2] 155 | 156 | 157 | def apply_2d_transform(mat: np.ndarray, transform: Pose2D) -> np.ndarray: 158 | """ 159 | Apply a 2d transform to a homogenous matrix 160 | :param mat: the input 4x4 homogenous matrix 161 | :param transform : 2d transform which to apply to the mat 162 | :return : transformed homogenous transformation matrix 163 | """ 164 | # req target transformation from base 165 | q = tf.transformations.quaternion_from_euler(0, 0, transform[2]) 166 | tf_mat = ts.concatenate_matrices( 167 | ts.translation_matrix( 168 | (transform[0], transform[1], 0)), ts.quaternion_matrix(q)) 169 | return np.matmul(mat, tf_mat) 170 | 171 | 172 | def compute_tf_diff(current_tf: np.ndarray, ref_tf: np.ndarray) -> Pose2D: 173 | """ 174 | Find the diff of two transformation matrix 175 | :param : homogenous transformation of 2 matrices 176 | :return : the 2d planer trans fo the 2 inputs; [x, y, yaw] 177 | """ 178 | tf_diff = np.matmul(ts.inverse_matrix(current_tf), ref_tf) 179 | trans = ts.translation_from_matrix(tf_diff) 180 | euler = ts.euler_from_matrix(tf_diff) 181 | return trans[0], trans[1], euler[2] 182 | 183 | 184 | def avg_2d_poses(poses: List[Pose2D]) -> Pose2D: 185 | """ 186 | Provide the average of a list of 2D poses 187 | :param poses : a list of 2d poses 188 | :return : output avg Pose2D 189 | """ 190 | _l = len(poses) 191 | if (_l == 0): 192 | return None 193 | _x = 0 194 | _y = 0 195 | _yaw = 0 196 | for pose in poses: 197 | _x += pose[0] 198 | _y += pose[1] 199 | _yaw += pose[2] 200 | return _x/_l, _y/_l, _yaw/_l 201 | 202 | 203 | def sat_proportional_filter( 204 | input: float, abs_min=0.0, abs_max=10.0, factor=1) -> float: 205 | """ 206 | Simple saturated proportional filter 207 | :param input : input value 208 | :param abs_min and abs_max : upper and lower bound, abs value 209 | :param factor : multiplier factor for the input value 210 | :return : output filtered value, within boundary 211 | """ 212 | output = 0.0 213 | input *= factor 214 | if abs(input) < abs_min: 215 | if (input < 0): 216 | output = -abs_min 217 | else: 218 | output = abs_min 219 | elif abs(input) > abs_max: 220 | if (input > 0): 221 | output = abs_max 222 | else: 223 | output = -abs_max 224 | else: 225 | output = input 226 | return output 227 | 228 | 229 | def bin_filter(input: float, abs_boundary: float) -> float: 230 | """ 231 | Simple binary filter, will provide abs_ceiling as a binary output, 232 | according to the 'negativity' of the input value 233 | :param input : input value 234 | :param abs_boundary : abs boundary value 235 | :return : output binary value 236 | """ 237 | output = abs(abs_boundary) 238 | if input < 0: 239 | output = -abs(abs_boundary) 240 | return output 241 | 242 | 243 | def flip_yaw(yaw: float) -> float: 244 | """ 245 | Flip yaw angle by 180 degree, input yaw range should be within 246 | [-pi, pi] radian. Else use set_angle() fn to fix the convention. 247 | Output will also be within the same range of [-pi, pi] radian. 248 | """ 249 | if yaw >= 0: 250 | return yaw - math.pi 251 | else: 252 | return yaw + math.pi 253 | 254 | 255 | def set_angle(angle: float) -> float: 256 | """ 257 | Ensure the angle is within the range of [-pi, pi] radian convention 258 | """ 259 | return math.atan2(math.sin(angle), math.cos(angle)) 260 | 261 | 262 | def flip_base_frame(input: Pose2D): 263 | """ 264 | Flip the current reference frame by 180 degree. As such, the negativity 265 | of translation is flipped, and the yaw angle is located at the opposite 266 | quadrant. Currently is used to flip from 'back dock' to 'front dock' 267 | """ 268 | return -input[0], -input[1], flip_yaw(input[2]) 269 | -------------------------------------------------------------------------------- /autodock_core/scripts/simple_autodock.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2021 Open Source Robotics Foundation, Inc. 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 | 18 | import math 19 | import argparse 20 | 21 | import rospy 22 | from std_srvs.srv import Trigger 23 | from sensor_msgs.msg import BatteryState 24 | 25 | from autodock_core.autodock_utils import DockState 26 | import autodock_core.autodock_utils as utils 27 | from autodock_core.autodock_server import AutoDockServer, AutoDockConfig 28 | 29 | ############################################################################## 30 | 31 | 32 | class DefaultAutoDockConfig(AutoDockConfig): 33 | """ 34 | Default config is used in simulation 35 | # Note: all these tf computations are reference to robot base_link. Hence, 36 | """ 37 | # General Configs 38 | cam_offset = 0.35 # camera to base link, for edge2edge distance 39 | front_dock = False 40 | tf_expiry: float = 1.0 # sec 41 | dock_timeout = 220 # sec 42 | controller_rate = 4.0 # hz 43 | 44 | # goal thresh 45 | stop_yaw_diff = 0.03 # radian 46 | stop_trans_diff = 0.02 # meters 47 | 48 | # frames and topics 49 | base_link = "base_link" 50 | left_marker = "fiducial_10" 51 | right_marker = "fiducial_11" 52 | centre_marker = "fiducial_20" 53 | 54 | # velocity profiles 55 | linear_vel_range = (-0.6, 0.6) 56 | angular_vel_range = (-0.4, 0.4) 57 | 58 | max_linear_vel = 0.5 # m/s, for parallel.c and steer 59 | min_linear_vel = 0.25 # m/s, for lastmile 60 | max_angular_vel = 0.35 # rad/s 61 | min_angular_vel = 0.20 # rad/s 62 | 63 | # predock state 64 | max_parallel_offset = 0.16 # m, will move to parallel.c if exceeded 65 | predock_tf_samples = 5 # tf samples to avg, parallel.c validation 66 | 67 | # steer dock state 68 | offset_to_angular_vel = 2 # factor to convert y-offset to ang vel 69 | to_last_mile_dis = 0.50 # edge2edge distance where transition to LM 70 | to_last_mile_tol = 0.25 # transition tolerance from SD to LM 71 | 72 | # last mile 73 | stop_distance = 0.10 # edge2edge distance to stop from charger 74 | max_last_mile_odom = 0.20 # max last mile odom move without using marker 75 | 76 | # activate charger state 77 | enable_charger_srv = True 78 | check_battery_status = False 79 | check_battery_timeout = 1.0 80 | charger_srv_name = "trigger_charger_srv" 81 | battery_status_topic = "battery_state" 82 | 83 | # retry state 84 | retry_count = 1 # how many times to retry 85 | retry_retreat_dis = 0.4 # meters, distance retreat during retry 86 | 87 | # debug state 88 | debug_mode = True # when False selectively turns on aruco detections only 89 | # a valid action is in progress. 90 | 91 | 92 | class AutoDockStateMachine(AutoDockServer): 93 | """ 94 | Implementation of the AutoDock Server with a Simple State Machine. Also 95 | this describes the logic of which the control loop of each state. 96 | """ 97 | 98 | def __init__(self, 99 | config: DefaultAutoDockConfig, 100 | run_server=False, 101 | load_rosparam=False, 102 | fake_clock=False): 103 | # /Note: This ugly fix is to solve time sync issue when nodes are 104 | # running on a different PC, which time in not sync. As we will only 105 | # wish to enable sim time on 'AutoDockServer Node', please ensures 106 | # this node is not starting along with other nodes. 107 | if fake_clock: 108 | rospy.logwarn("WARNING!!!! fake clock is in used. " 109 | "Temporary set use_sim_time to true") 110 | rospy.set_param("/use_sim_time", True) 111 | 112 | rospy.init_node('auto_dock_node') 113 | 114 | if fake_clock: 115 | rospy.logwarn( 116 | "WARNING!!!! fake clock enabled! now disable use_sim_time") 117 | rospy.set_param("/use_sim_time", False) 118 | 119 | self.cfg = config 120 | if load_rosparam: 121 | self.init_params() 122 | 123 | super().__init__(self.cfg, run_server) 124 | self.dock_state = DockState.IDLE 125 | 126 | def init_params(self): 127 | rospy.loginfo("Getting AutoDockServer params from rosparams server") 128 | param_names = [attr for attr in dir(self.cfg) if not callable( 129 | getattr(self.cfg, attr)) and not attr.startswith("__")] 130 | 131 | # get private rosparam, if none use default 132 | for param_name in param_names: 133 | param_val = rospy.get_param( 134 | "~" + param_name, getattr(self.cfg, param_name)) 135 | print(f" set param [{param_name}] to [{param_val}]") 136 | setattr(self.cfg, param_name, param_val) 137 | 138 | # brute check to ensure numerical config is positive 139 | if isinstance(param_val, (int, float)): 140 | assert param_val >= 0, f"[{param_name}] param should be +ve" 141 | 142 | def start(self) -> bool: 143 | """ 144 | Start Docking Sequence 145 | """ 146 | rospy.loginfo("Start Docking Action Now") 147 | self.init_params() 148 | self.set_aruco_detections(detection_state=True) 149 | rospy.sleep(self.sleep_period) 150 | 151 | current_retry_count = 0 152 | rospy.loginfo(f"Will attempt with {self.cfg.retry_count} retry") 153 | 154 | while(True): 155 | 156 | if( 157 | self.do_predock() 158 | and self.do_steer_dock() 159 | and self.do_last_mile() 160 | and self.do_activate_charger() 161 | ): 162 | self.publish_cmd() 163 | self.set_state(DockState.IDLE, "All Success!") 164 | self.set_aruco_detections(detection_state=False) 165 | return True 166 | 167 | # If Dock failed 168 | self.publish_cmd() 169 | rospy.logwarn("Failed to Dock attempt") 170 | 171 | # Break from a retry 172 | if(current_retry_count >= self.cfg.retry_count): 173 | self.set_aruco_detections(detection_state=False) 174 | break 175 | 176 | # check again if it failed because of canceled 177 | if self.check_cancel(): 178 | self.set_aruco_detections(detection_state=False) 179 | break 180 | 181 | # Attempt a Retry 182 | current_retry_count += 1 183 | rospy.logwarn("Attempting retry: " 184 | f"{current_retry_count}/{self.cfg.retry_count}") 185 | 186 | if not self.do_retry(): 187 | rospy.logwarn("Not able to retry") 188 | self.set_aruco_detections(detection_state=False) 189 | break 190 | 191 | # Note: will not set_state IDLE here since we will wish to capture 192 | # the state which failed in the action result status 193 | self.publish_cmd() 194 | return False 195 | 196 | def do_retry(self) -> bool: 197 | """ 198 | Attempt to retry the docking again, only retry by retreating if it 199 | failed in activate charger, last mile. if Failed with steerDock, 200 | will straightaway attempt predock without retreating. 201 | """ 202 | if self.check_cancel(): 203 | return False 204 | 205 | if self.dock_state in [DockState.ACTIVATE_CHARGER, 206 | DockState.LAST_MILE]: 207 | self.set_state(DockState.RETRY, "Retry by retreating") 208 | _dis = self.cfg.retry_retreat_dis 209 | if self.cfg.front_dock: 210 | _dis *= -1 211 | return self.move_with_odom(_dis) 212 | elif self.dock_state == DockState.STEER_DOCK: 213 | self.set_state(DockState.RETRY, "skip retreat and try predock") 214 | return True 215 | else: 216 | rospy.loginfo("State is not retry-able") 217 | return False 218 | 219 | def do_parallel_correction(self, offset: float) -> bool: 220 | """ 221 | A parallel "parking" correction will be executed if the y offset 222 | exceeds the allowable threshold. Note this is an open loop operation. 223 | :return: success 224 | """ 225 | self.set_state(DockState.PARALLEL_CORRECTION, 226 | f"activate parallel correction with {offset:.2f}m") 227 | return ( 228 | # Step 1: turn -90 degrees respective to odom 229 | self.rotate_with_odom(-math.pi/2) 230 | # Step 2: parallel correction respective to odom 231 | and self.move_with_odom(offset) 232 | # Step 3: turn 90 degrees respective to odom 233 | and self.rotate_with_odom(math.pi/2) 234 | ) 235 | 236 | def do_single_side_marker_rotate(self) -> bool: 237 | """ 238 | This predock's substate which handles single side marker detection, 239 | adjust the yaw according to the pose estimation of one single marker 240 | :return : success 241 | """ 242 | self.set_state(DockState.PREDOCK, "Try single marker yaw adjustment") 243 | 244 | left_tf = self.get_tf(self.cfg.left_marker) 245 | if left_tf is not None: 246 | rospy.logwarn(f"Rotate with left marker: {self.cfg.left_marker}") 247 | yaw = utils.get_2d_pose(left_tf)[2] 248 | if self.cfg.front_dock: 249 | yaw = utils.flip_yaw(yaw) 250 | return self.rotate_with_odom(yaw - math.pi/2) 251 | 252 | right_tf = self.get_tf(self.cfg.right_marker) 253 | if right_tf is not None: 254 | rospy.logwarn(f"Rotate with right marker: {self.cfg.right_marker}") 255 | yaw = utils.get_2d_pose(right_tf)[2] 256 | if self.cfg.front_dock: 257 | yaw = utils.flip_yaw(yaw) 258 | return self.rotate_with_odom(yaw - math.pi/2) 259 | 260 | rospy.logerr("Not detecting two side markers, exit state") 261 | return False 262 | 263 | def do_predock(self) -> bool: 264 | """ 265 | This is the predock phase, which will simply check if the the robot 266 | is located in the allowable zone, and rotate the robot base as such as 267 | the robot orientation is normal to the side markers. 268 | @return: success 269 | """ 270 | self.set_state(DockState.PREDOCK, "start docking") 271 | 272 | # initial check if both markers are detected, 273 | # if only one single side marker detected, will purely depends to that 274 | # marker to rotate till facing the charger. then will check if both 275 | # markers exist again. 276 | if self.get_centre_of_side_markers() is None: 277 | if not self.do_single_side_marker_rotate(): 278 | return False 279 | 280 | # start predock loop 281 | _pose_list = [] 282 | rospy.loginfo("Both Markers are detected, running predock loop") 283 | while not rospy.is_shutdown(): 284 | if self.check_cancel(): 285 | return False 286 | 287 | if self.is_pause: 288 | if not self.do_pause(): 289 | return False 290 | 291 | centre_tf = self.get_centre_of_side_markers() 292 | 293 | if centre_tf is None: 294 | rospy.logerr("Not detecting two side markers, exit state") 295 | return False 296 | 297 | if self.cfg.front_dock: 298 | centre_tf = utils.flip_base_frame(centre_tf) 299 | _, _, yaw = centre_tf 300 | print(f"current yaw diff: {yaw:.3f}") 301 | 302 | # Check if the robot is now normal to the charging station 303 | if abs(yaw) < self.cfg.stop_yaw_diff: 304 | rospy.logwarn(f"Done with yaw correction, tf: {centre_tf}") 305 | self.publish_cmd() 306 | 307 | # switch the reference frame to charging station. 308 | marker_to_base_link_tf = utils.get_2d_inverse(centre_tf) 309 | if len(_pose_list) < self.cfg.predock_tf_samples: 310 | remainings = self.cfg.predock_tf_samples - len(_pose_list) 311 | print(f"Getting {remainings} more samples for averaging") 312 | _pose_list.append(marker_to_base_link_tf) 313 | continue 314 | 315 | y_offset = utils.avg_2d_poses(_pose_list)[1] 316 | if self.cfg.front_dock: # TODO 317 | y_offset *= -1 318 | _pose_list = [] 319 | 320 | # if robot y axis is way off, we will do parallel correction 321 | # after this, will repeat predock 322 | if abs(y_offset) > self.cfg.max_parallel_offset: 323 | if not self.do_parallel_correction(y_offset): 324 | return False 325 | 326 | self.publish_cmd() 327 | rospy.sleep(rospy.Duration(self.cfg.tf_expiry)) 328 | self.set_state(DockState.PREDOCK, "try predock again") 329 | continue 330 | 331 | return True 332 | 333 | # publish rotate 334 | ang_vel = utils.bin_filter(yaw, self.cfg.min_angular_vel) 335 | self.publish_cmd(angular_vel=ang_vel) 336 | 337 | rospy.sleep(self.sleep_period) 338 | exit(0) 339 | 340 | def do_steer_dock(self) -> bool: 341 | """ 342 | This will utilize the side markers to steer dock the robot 343 | closer to the target charging station 344 | @return: success 345 | """ 346 | self.set_state(DockState.STEER_DOCK) 347 | offset_from_charger = \ 348 | self.cfg.cam_offset + self.cfg.to_last_mile_dis 349 | transition_dis_with_tol = \ 350 | offset_from_charger + self.cfg.to_last_mile_tol 351 | _dir = 1 if self.cfg.front_dock else -1 352 | 353 | while not rospy.is_shutdown(): 354 | if self.check_cancel(): 355 | return False 356 | 357 | if self.is_pause: 358 | if not self.do_pause(): 359 | return False 360 | 361 | # Note that centre_tf is consists of a front_offset 362 | centre_tf = self.get_centre_of_side_markers(offset_from_charger) 363 | 364 | # check if both markers are not detected 365 | if centre_tf is None: 366 | rospy.logwarn("Not detecting two sides marker") 367 | centre_tf_mat = self.get_tf(self.cfg.centre_marker) 368 | 369 | if centre_tf_mat is None: 370 | rospy.logerr("Not able to locate centre marker too") 371 | return False 372 | 373 | dis, _, _ = utils.get_2d_pose(centre_tf_mat) 374 | # check centre marker is near, with safety tol 375 | if (abs(dis) > transition_dis_with_tol): 376 | rospy.logerr( 377 | f"Centre marker is {dis:.3f} m away, too far, exit") 378 | return False 379 | 380 | rospy.logwarn( 381 | f"Centre marker {dis}m away, transition to last_mile") 382 | return True 383 | 384 | if self.cfg.front_dock: 385 | centre_tf = utils.flip_base_frame(centre_tf) 386 | dis, offset, _ = centre_tf 387 | 388 | # transition distance to last mile 389 | # since we offset with offset_from_charger, This should be 0, 390 | if (dis > 0): 391 | rospy.logwarn("Transition to lastmile state") 392 | self.publish_cmd() 393 | return True 394 | 395 | print(f" DETECTED both side markers!! " 396 | f"[d: {dis:.2f}, offset: {offset:.2f}]") 397 | 398 | ang_vel = utils.sat_proportional_filter( 399 | -offset, factor=self.cfg.offset_to_angular_vel) 400 | self.publish_cmd(linear_vel=_dir*self.cfg.max_linear_vel, 401 | angular_vel=ang_vel) 402 | rospy.sleep(self.sleep_period) 403 | exit(0) 404 | 405 | def do_last_mile(self) -> bool: 406 | """ 407 | This will utilize the single centre marker to conduct a final last 408 | mile docking to the target charging station 409 | @return: success 410 | """ 411 | self.set_state(DockState.LAST_MILE) 412 | 413 | remaining_dis = self.cfg.to_last_mile_dis 414 | _dir = 1 if self.cfg.front_dock else -1 415 | 416 | while not rospy.is_shutdown(): 417 | if self.check_cancel(): 418 | return False 419 | 420 | if self.is_pause: 421 | if not self.do_pause(): 422 | return False 423 | 424 | centre_tf_mat = self.get_tf(self.cfg.centre_marker) 425 | 426 | # Final Dock based on odom if centre marker is getting to close 427 | if centre_tf_mat is None: 428 | rospy.logwarn("Not detecting centre marker") 429 | if remaining_dis < self.cfg.max_last_mile_odom: 430 | rospy.logwarn(f"move {remaining_dis}m with odom") 431 | return self.move_with_odom(_dir*remaining_dis) 432 | else: 433 | rospy.logerr("exceeded max_last_mile_odom with " 434 | "remaining dis of {remaining_dis}, exit!") 435 | return False 436 | 437 | centre_tf = utils.get_2d_pose(centre_tf_mat) 438 | if self.cfg.front_dock: 439 | centre_tf = utils.flip_base_frame(centre_tf) 440 | dis, _, yaw = centre_tf 441 | 442 | yaw -= math.pi/2 443 | remaining_dis = - dis - self.cfg.stop_distance - self.cfg.cam_offset 444 | print(f" Approaching Charger -> d: {dis:.3f}, yaw: {yaw:.2f}" 445 | f", remaining dis: {remaining_dis:.3f}") 446 | 447 | if (remaining_dis <= 0): 448 | rospy.loginfo(" ~ STOP!! Reach destination! ~") 449 | self.publish_cmd() 450 | return True 451 | 452 | ang_vel = utils.sat_proportional_filter( 453 | yaw, abs_min=0.0, abs_max=self.cfg.min_angular_vel, factor=1.2) 454 | self.publish_cmd(linear_vel=_dir*self.cfg.min_linear_vel, 455 | angular_vel=ang_vel) 456 | rospy.sleep(self.sleep_period) 457 | exit(0) 458 | 459 | def do_activate_charger(self) -> bool: 460 | self.set_state(DockState.ACTIVATE_CHARGER, "Docked, activate charger!") 461 | rospy.sleep(rospy.Duration(2.0)) 462 | # Charger trigger service 463 | if self.cfg.enable_charger_srv: 464 | try: 465 | _trigger = rospy.ServiceProxy( 466 | self.cfg.charger_srv_name, Trigger) 467 | res = _trigger() 468 | rospy.loginfo(f"Trigger charger srv, " 469 | f"success [{res.success}] | msg: {res.message}") 470 | if not res.success: 471 | rospy.logerr("trigger charger failed") 472 | return False 473 | except rospy.ServiceException as e: 474 | rospy.logerr(f"Charger srv call failed: {e}") 475 | return False 476 | 477 | # BatteryCharge status Validation 478 | if self.cfg.check_battery_status: 479 | try: 480 | # check status every 2s 481 | start = rospy.Time.now() 482 | while (True): 483 | battery_status_msg = rospy.wait_for_message( 484 | self.cfg.battery_status_topic, BatteryState, timeout=2.0) 485 | charge_status = battery_status_msg.power_supply_status 486 | 487 | if (charge_status == BatteryState.POWER_SUPPLY_STATUS_CHARGING): 488 | break 489 | 490 | print(f"battery state is {charge_status}, Not Charging!") 491 | rospy.sleep(rospy.Duration(1.0)) 492 | if ((rospy.Time.now() - start).secs > 493 | self.cfg.check_battery_timeout): 494 | rospy.logerr("time out for check battery!") 495 | return False 496 | rospy.loginfo("Battery is now Charging!") 497 | except rospy.exceptions.ROSException: 498 | rospy.logerr(f"Failed to get battery state") 499 | return False 500 | self.set_state(DockState.ACTIVATE_CHARGER, "Celebration!!") 501 | return True 502 | 503 | 504 | ############################################################################## 505 | if __name__ == "__main__": 506 | parser = argparse.ArgumentParser(description='Simple AutoDockServer') 507 | parser.add_argument('--server', dest='run_server', 508 | action='store_true', help="run action server") 509 | parser.add_argument('--fake_clock', dest='fake_clock', 510 | action='store_true', 511 | help="Danger! use this to solve time sync issue") 512 | parser.add_argument('--rosparam', dest='load_rosparam', 513 | action='store_true', help="load rosparam for configs") 514 | args, unknown = parser.parse_known_args() 515 | 516 | # Default Default Configuration 517 | config = DefaultAutoDockConfig() 518 | 519 | if args.run_server: 520 | node = AutoDockStateMachine( 521 | config, 522 | run_server=True, 523 | load_rosparam=args.load_rosparam, 524 | fake_clock=args.fake_clock) 525 | rospy.spin() 526 | else: 527 | node = AutoDockStateMachine( 528 | config, 529 | run_server=False, 530 | load_rosparam=args.load_rosparam, 531 | fake_clock=args.fake_clock) 532 | node.start() 533 | -------------------------------------------------------------------------------- /autodock_core/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | # fetch values from package.xml 5 | setup_args = generate_distutils_setup( 6 | packages=['autodock_core'], 7 | package_dir={'': 'scripts'}) 8 | 9 | setup(**setup_args) 10 | -------------------------------------------------------------------------------- /autodock_core/src/ObstacleObserver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2021 Open Source Robotics Foundation 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 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | using AutoDockFeedbackMsg = autodock_core::AutoDockingActionFeedback; 27 | using FeedbackStateMsg = autodock_core::AutoDockingFeedback; 28 | 29 | class ObstacleObserver 30 | { 31 | // This simple obstacle observer node checks if any obstacle is near to the 32 | // vicinity of the robot. This mainly serves as a safety node for the current 33 | // autodock action. Essentially, it subscribes to the robot's local_costmap 34 | // and publish a pause to the /pause_dock topic. also, only certain state 35 | // we will activate the pausing capability, so prevent robot stoping when it 36 | // is near the charging station. 37 | public: 38 | ObstacleObserver(ros::NodeHandle nh) : nh_(nh) 39 | { 40 | pause_dock_pub_ = nh_.advertise("/pause_dock", 3000); 41 | 42 | pause_vicinity_pub_ = nh_.advertise( 43 | "/pause_dock_vicinity", 100); 44 | 45 | costmap_sub_ = nh_.subscribe( 46 | "/move_base/global_costmap/local_costmap", 100, 47 | &ObstacleObserver::costmap_cb, this); 48 | 49 | autodock_feedback_sub_ = nh_.subscribe( 50 | "/autodock_action/feedback", 100, 51 | &ObstacleObserver::autodock_feedback_cb, this); 52 | 53 | /// Default rate: 3.33 hz 54 | ros_timer_ = nh_.createTimer( 55 | ros::Duration(0.3), &ObstacleObserver::periodic_pub_cb, this); 56 | 57 | /// Get all rosparams 58 | /// Pause when occupied pixels are within the vicinity radius (meters) 59 | nh.param("vicinity_radius", vicinity_radius_, 0.3); 60 | ROS_INFO("param: [vicinity_radius]: %f", vicinity_radius_); 61 | 62 | /// Pause when occupied pixel is above coverage_percent, ( 0.0 - 1.0 ) 63 | nh.param("coverage_percent", coverage_percent_thresh_, 0.10); 64 | ROS_INFO("param: [coverage_percent]: %f", coverage_percent_thresh_); 65 | 66 | /// Consider a pixel as occupied if above occupancy_prob ( 0 - 100 ) 67 | nh.param("occupancy_prob", occupancy_prob_thresh_, 60); 68 | ROS_INFO("param: [occupancy_prob]: %d", occupancy_prob_thresh_); 69 | 70 | // frame_id of the the vicinity_msg (footprint), mainly for debug 71 | nh.param( 72 | "base_link_name", vicinity_msg_.header.frame_id, "base_link"); 73 | 74 | geometry_msgs::Point32 point; 75 | for (float rad = 0; rad <= 2 * M_PI; rad += 0.2) 76 | { 77 | point.x = vicinity_radius_ * cos(rad); 78 | point.y = vicinity_radius_ * sin(rad); 79 | vicinity_msg_.polygon.points.push_back(point); 80 | } 81 | ROS_INFO("Done Initialize Obstacle Observer Node"); 82 | } 83 | 84 | void costmap_cb( 85 | const nav_msgs::OccupancyGrid::ConstPtr &msg) 86 | { 87 | const auto info = msg->info; 88 | int checked_pixels = 0; 89 | int occluded_pixels = 0; 90 | 91 | const float pixel_radius = vicinity_radius_ / info.resolution; 92 | // TODO: add debug flag 93 | // ROS_INFO("heard robot costmap %dx%d, pixel_radius: %f", 94 | // info.height, info.width, pixel_radius ); 95 | 96 | const int a = info.width / 2; 97 | const int b = info.height / 2; 98 | const int r_sq = pow(pixel_radius, 2); 99 | 100 | const int x_max = a + pixel_radius; 101 | const int x_min = a - pixel_radius; 102 | const int y_max = b + pixel_radius; 103 | const int y_min = b - pixel_radius; 104 | 105 | for (int y = y_min; y <= y_max; y++) 106 | { 107 | for (int x = x_min; x <= x_max; x++) 108 | { 109 | // use foot print radius 110 | if (pow(x - a, 2) + pow(y - b, 2) > r_sq) 111 | continue; 112 | 113 | checked_pixels++; 114 | u_int i = x + (info.height - y - 1) * info.width; 115 | 116 | // std::cout << (int)msg->data[i] << '|'; 117 | if (msg->data[i] > occupancy_prob_thresh_) 118 | occluded_pixels++; 119 | } 120 | } 121 | 122 | // std::cout << std::endl; 123 | // ROS_INFO(" Occlusion pixels count is %d / %d, from %d", 124 | // occluded_pixels, checked_pixels, info.height * info.width); 125 | 126 | occupancy_percent_ = occluded_pixels / (float)checked_pixels; 127 | } 128 | 129 | void autodock_feedback_cb( 130 | const AutoDockFeedbackMsg::ConstPtr &msg) 131 | { 132 | ROS_INFO("ObstacleObserver received autodock state: %d", 133 | msg->feedback.state); 134 | dock_state_ = msg->feedback.state; 135 | publish_msg_fn(); 136 | } 137 | 138 | void periodic_pub_cb(const ros::TimerEvent &) 139 | { 140 | publish_msg_fn(); 141 | } 142 | 143 | void publish_msg_fn() 144 | { 145 | std_msgs::Bool msg; 146 | msg.data = false; 147 | 148 | // These states are pause - able 149 | if (dock_state_ == FeedbackStateMsg::STATE_PREDOCK || 150 | dock_state_ == FeedbackStateMsg::STATE_STEER_DOCK || 151 | dock_state_ == FeedbackStateMsg::STATE_PAUSE || 152 | dock_state_ == FeedbackStateMsg::STATE_PARALLEL_CORRECTION) 153 | { 154 | // ROS_INFO("Checking State occupancy percent: %0.3f of State: %d", 155 | // occupancy_percent_, dock_state_); 156 | 157 | if (occupancy_percent_ > coverage_percent_thresh_) 158 | { 159 | msg.data = true; // is occupied 160 | ROS_INFO("PAUSE!, ocupancy percent %0.3f > %0.3f thresh", 161 | occupancy_percent_, coverage_percent_thresh_); 162 | } 163 | } 164 | pause_dock_pub_.publish(msg); 165 | vicinity_msg_.header.stamp = ros::Time::now(); 166 | pause_vicinity_pub_.publish(vicinity_msg_); 167 | } 168 | 169 | private: 170 | ros::NodeHandle nh_; 171 | uint8_t dock_state_ = FeedbackStateMsg::STATE_INVALID; 172 | 173 | ros::Subscriber costmap_sub_; 174 | ros::Subscriber autodock_feedback_sub_; 175 | ros::Publisher pause_dock_pub_; 176 | ros::Publisher pause_vicinity_pub_; //for debug 177 | ros::Timer ros_timer_; 178 | geometry_msgs::PolygonStamped vicinity_msg_; 179 | 180 | float occupancy_percent_ = 0.0; 181 | 182 | // rosparams 183 | float coverage_percent_thresh_; 184 | float vicinity_radius_; 185 | int occupancy_prob_thresh_; 186 | }; 187 | 188 | int main(int argc, char **argv) 189 | { 190 | ros::init(argc, argv, "obstacle_observer"); 191 | std::cout << "Running Obstatcle Observer Node" << std::endl; 192 | ros::NodeHandle nh("~"); 193 | ObstacleObserver dock_server(nh); 194 | ros::spin(); 195 | } 196 | -------------------------------------------------------------------------------- /autodock_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(autodock_examples) 3 | 4 | ## Find catkin dependencies 5 | find_package(catkin REQUIRED) 6 | 7 | ## Define catkin exports 8 | catkin_package( 9 | CATKIN_DEPENDS message_runtime rospy) 10 | 11 | catkin_install_python(PROGRAMS 12 | scripts/dock_sim_test.py 13 | scripts/dock_robot_test.py 14 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 15 | ) 16 | 17 | install(DIRECTORY launch/ 18 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 19 | ) 20 | -------------------------------------------------------------------------------- /autodock_examples/configs/mock_robot.yaml: -------------------------------------------------------------------------------- 1 | # Gazebo Simulation robot 2 | 3 | ############################################################################## 4 | # Frame Ids 5 | 6 | base_link : "base_footprint" 7 | left_marker: "fiducial_10" 8 | centre_marker: "fiducial_20" 9 | right_marker: "fiducial_11" 10 | 11 | ############################################################################## 12 | # Vel profiles 13 | 14 | # the upper lower limit of the cmd_vel output 15 | linear_vel_range: [-0.5, 0.5] 16 | angular_vel_range: [-0.35, 0.35] 17 | 18 | # Absolute vel profile 19 | # min (ang and linear) vel here means the min cmd_vel before the robot stalls 20 | max_linear_vel: 0.5 # m/s, for parallel.c and steer 21 | min_linear_vel: 0.25 # m/s, for lastmile 22 | max_angular_vel: 0.35 # rad/s 23 | min_angular_vel: 0.20 # rad/s 24 | 25 | ############################################################################## 26 | # General Configs 27 | 28 | cam_offset: 0.35 # camera to base link 29 | stop_yaw_diff: 0.03 # radian 30 | stop_trans_diff: 0.02 # meters 31 | tf_expiry: 1.0 # sec 32 | controller_rate: 4.0 # hz 33 | dock_timeout: 220 # sec 34 | front_dock: false 35 | 36 | ############################################################################## 37 | # Predock State 38 | 39 | max_parallel_offset: 0.16 # m, will move to parallel.c if exceeded 40 | predock_tf_samples: 5 # tf samples to avg, parallel.c validation 41 | 42 | ############################################################################## 43 | # Steer Dock State 44 | 45 | to_last_mile_dis: 0.50 # edge2edge distance where transition to LM 46 | to_last_mile_tol: 0.20 # transition tolerance from SD to LM 47 | # Determine how "rigorous" we want the robot to correct itself during steer dock 48 | offset_to_angular_vel: 2 # multiplier factor to convert y-offset to ang vel 49 | 50 | ############################################################################## 51 | # Last mile State 52 | 53 | max_last_mile_odom: 0.20 # max last mile odom move without using marker 54 | stop_distance: 0.10 # edge2edge distance to stop from charger 55 | 56 | ############################################################################## 57 | # Activate Charger 58 | 59 | enable_charger_srv: True # whether to activate charger after last mile 60 | check_battery_status: False # check if battery status is POWER_SUPPLY_STATUS_CHARGING 61 | 62 | ############################################################################## 63 | # Retry State 64 | 65 | retry_count: 2 # how many times to retry 66 | retry_retreat_dis: 0.5 # meters, distance retreat during retry 67 | -------------------------------------------------------------------------------- /autodock_examples/configs/turtlebot3.yaml: -------------------------------------------------------------------------------- 1 | # Gazebo Simulation robot 2 | 3 | ############################################################################## 4 | # Frame Ids 5 | 6 | base_link : "base_footprint" 7 | left_marker: "fiducial_10" 8 | centre_marker: "fiducial_20" 9 | right_marker: "fiducial_11" 10 | 11 | ############################################################################## 12 | # Vel profiles 13 | 14 | # the upper lower limit of the cmd_vel output 15 | linear_vel_range: [-0.8, 0.8] 16 | angular_vel_range: [-0.13, 0.13] 17 | 18 | # Absolute vel profile 19 | # min (ang and linear) vel here means the min cmd_vel before the robot stalls 20 | max_linear_vel: 0.05 # m/s, for parallel.c and steer 21 | min_linear_vel: 0.02 # m/s, for lastmile 22 | max_angular_vel: 0.13 # rad/s 23 | min_angular_vel: 0.05 # rad/s 24 | 25 | ############################################################################## 26 | # General Configs 27 | 28 | cam_offset: 0.04 # camera to base link 29 | stop_yaw_diff: 0.03 # radian 30 | stop_trans_diff: 0.02 # meters 31 | tf_expiry: 0.5 # sec 32 | controller_rate: 4.0 # hz 33 | dock_timeout: 220 # sec 34 | front_dock: true # turtlebot "charging point" is located in front 35 | 36 | ############################################################################## 37 | # Predock State 38 | 39 | max_parallel_offset: 0.10 # m, will move to parallel.c if exceeded 40 | predock_tf_samples: 3 # tf samples to avg, parallel.c validation 41 | 42 | ############################################################################## 43 | # Steer Dock State 44 | 45 | to_last_mile_dis: 0.3 # edge2edge distance where transition to LM 46 | to_last_mile_tol: 0.1 # transition tolerance from SD to LM 47 | # Determine how "rigorous" we want the robot to correct itself during steer dock 48 | offset_to_angular_vel: 1.1 # multiplier factor to convert y-offset to ang vel 49 | 50 | ############################################################################## 51 | # Last mile State 52 | 53 | max_last_mile_odom: 0.20 # max last mile odom move without using marker 54 | stop_distance: 0.08 # edge2edge distance to stop from charger 55 | 56 | ############################################################################## 57 | # Activate Charger 58 | 59 | enable_charger_srv: False # whether to activate charger after last mile 60 | check_battery_status: False # check if battery status is POWER_SUPPLY_STATUS_CHARGING 61 | -------------------------------------------------------------------------------- /autodock_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autodock_examples 4 | 0.2.0 5 | Autodock examples 6 | You Liang 7 | BSD 8 | 9 | You Liang 10 | 11 | catkin 12 | 13 | roscpp 14 | actionlib 15 | message_generation 16 | std_msgs 17 | actionlib_msgs 18 | 19 | roscpp 20 | rospy 21 | tf2_ros 22 | aruco_detect 23 | actionlib 24 | message_runtime 25 | autodock_core 26 | 27 | 28 | -------------------------------------------------------------------------------- /autodock_examples/rviz/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | Splitter Ratio: 0.5 11 | Tree Height: 130 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/TF 57 | Enabled: true 58 | Frame Timeout: 3 59 | Frames: 60 | All Enabled: true 61 | base_footprint: 62 | Value: true 63 | camera_link: 64 | Value: true 65 | Marker Alpha: 1 66 | Marker Scale: 1 67 | Name: TF 68 | Show Arrows: true 69 | Show Axes: true 70 | Show Names: true 71 | Tree: 72 | {} 73 | Update Interval: 0 74 | Value: true 75 | - Class: rviz/Image 76 | Enabled: true 77 | Image Topic: /fiducial_images 78 | Max Value: 1 79 | Median window: 5 80 | Min Value: 0 81 | Name: Image 82 | Normalize Range: true 83 | Queue Size: 2 84 | Transport Hint: raw 85 | Unreliable: false 86 | Value: true 87 | - Class: rviz/Marker 88 | Enabled: true 89 | Marker Topic: /sm_maker 90 | Name: Marker 91 | Namespaces: 92 | {} 93 | Queue Size: 100 94 | Value: true 95 | Enabled: true 96 | Global Options: 97 | Background Color: 48; 48; 48 98 | Default Light: true 99 | Fixed Frame: odom 100 | Frame Rate: 30 101 | Name: root 102 | Tools: 103 | - Class: rviz/Interact 104 | Hide Inactive Objects: true 105 | - Class: rviz/MoveCamera 106 | - Class: rviz/Select 107 | - Class: rviz/FocusCamera 108 | - Class: rviz/Measure 109 | - Class: rviz/SetInitialPose 110 | Theta std deviation: 0.2617993950843811 111 | Topic: /initialpose 112 | X std deviation: 0.5 113 | Y std deviation: 0.5 114 | - Class: rviz/SetGoal 115 | Topic: /move_base_simple/goal 116 | - Class: rviz/PublishPoint 117 | Single click: true 118 | Topic: /clicked_point 119 | Value: true 120 | Views: 121 | Current: 122 | Class: rviz/Orbit 123 | Distance: 5.887696266174316 124 | Enable Stereo Rendering: 125 | Stereo Eye Separation: 0.05999999865889549 126 | Stereo Focal Distance: 1 127 | Swap Stereo Eyes: false 128 | Value: false 129 | Field of View: 0.7853981852531433 130 | Focal Point: 131 | X: 0.6806231141090393 132 | Y: 0.5094268321990967 133 | Z: -0.229132279753685 134 | Focal Shape Fixed Size: true 135 | Focal Shape Size: 0.05000000074505806 136 | Invert Z Axis: false 137 | Name: Current View 138 | Near Clip Distance: 0.009999999776482582 139 | Pitch: 0.9803977012634277 140 | Target Frame: 141 | Yaw: 5.1485772132873535 142 | Saved: ~ 143 | Window Geometry: 144 | Displays: 145 | collapsed: false 146 | Height: 846 147 | Hide Left Dock: false 148 | Hide Right Dock: false 149 | Image: 150 | collapsed: false 151 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000029efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000118000000d400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000161000001800000001b00ffffff000000010000010f0000029efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000430000029e000000b500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000044fc0100000002fb0000000800540069006d00650100000000000004b00000028900fffffffb0000000800540069006d00650100000000000004500000000000000000000003540000029e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 152 | Selection: 153 | collapsed: false 154 | Time: 155 | collapsed: false 156 | Tool Properties: 157 | collapsed: false 158 | Views: 159 | collapsed: false 160 | Width: 1200 161 | X: 92 162 | Y: 409 163 | -------------------------------------------------------------------------------- /autodock_examples/rviz/tb3_nav.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 359 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: Image 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz/TF 54 | Enabled: true 55 | Frame Timeout: 3 56 | Frames: 57 | All Enabled: false 58 | base_footprint: 59 | Value: true 60 | base_link: 61 | Value: true 62 | base_scan: 63 | Value: true 64 | camera_rgb_optical_frame: 65 | Value: true 66 | caster_back_link: 67 | Value: false 68 | fiducial_10: 69 | Value: true 70 | fiducial_11: 71 | Value: true 72 | fiducial_20: 73 | Value: true 74 | imu_link: 75 | Value: false 76 | map: 77 | Value: true 78 | odom: 79 | Value: true 80 | wheel_left_link: 81 | Value: false 82 | wheel_right_link: 83 | Value: false 84 | Marker Alpha: 1 85 | Marker Scale: 0.5 86 | Name: TF 87 | Show Arrows: true 88 | Show Axes: true 89 | Show Names: true 90 | Tree: 91 | map: 92 | {} 93 | odom: 94 | base_footprint: 95 | base_link: 96 | base_scan: 97 | {} 98 | caster_back_link: 99 | {} 100 | imu_link: 101 | {} 102 | wheel_left_link: 103 | {} 104 | wheel_right_link: 105 | {} 106 | camera_rgb_optical_frame: 107 | fiducial_10: 108 | {} 109 | fiducial_11: 110 | {} 111 | fiducial_20: 112 | {} 113 | Update Interval: 0 114 | Value: true 115 | - Class: rviz/Image 116 | Enabled: true 117 | Image Topic: /fiducial_images 118 | Max Value: 1 119 | Median window: 5 120 | Min Value: 0 121 | Name: Image 122 | Normalize Range: true 123 | Queue Size: 2 124 | Transport Hint: raw 125 | Unreliable: false 126 | Value: true 127 | - Class: rviz/Marker 128 | Enabled: true 129 | Marker Topic: /sm_maker 130 | Name: Marker 131 | Namespaces: 132 | "": true 133 | Queue Size: 100 134 | Value: true 135 | - Alpha: 0.699999988079071 136 | Class: rviz/Map 137 | Color Scheme: map 138 | Draw Behind: false 139 | Enabled: true 140 | Name: Map 141 | Topic: /map 142 | Unreliable: false 143 | Use Timestamp: false 144 | Value: true 145 | - Alpha: 0.4000000059604645 146 | Class: rviz/Map 147 | Color Scheme: costmap 148 | Draw Behind: false 149 | Enabled: true 150 | Name: Map 151 | Topic: /move_base/local_costmap/costmap 152 | Unreliable: false 153 | Use Timestamp: false 154 | Value: true 155 | - Alpha: 1 156 | Class: rviz/RobotModel 157 | Collision Enabled: false 158 | Enabled: true 159 | Links: 160 | All Links Enabled: true 161 | Expand Joint Details: false 162 | Expand Link Details: false 163 | Expand Tree: false 164 | Link Tree Style: Links in Alphabetic Order 165 | base_footprint: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | base_link: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | base_scan: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | caster_back_link: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | Value: true 184 | imu_link: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | wheel_left_link: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Value: true 193 | wheel_right_link: 194 | Alpha: 1 195 | Show Axes: false 196 | Show Trail: false 197 | Value: true 198 | Name: RobotModel 199 | Robot Description: robot_description 200 | TF Prefix: "" 201 | Update Interval: 0 202 | Value: true 203 | Visual Enabled: true 204 | - Alpha: 1 205 | Buffer Length: 1 206 | Class: rviz/Path 207 | Color: 25; 255; 0 208 | Enabled: true 209 | Head Diameter: 0.30000001192092896 210 | Head Length: 0.20000000298023224 211 | Length: 0.30000001192092896 212 | Line Style: Lines 213 | Line Width: 0.029999999329447746 214 | Name: Path 215 | Offset: 216 | X: 0 217 | Y: 0 218 | Z: 0 219 | Pose Color: 255; 85; 255 220 | Pose Style: None 221 | Queue Size: 10 222 | Radius: 0.029999999329447746 223 | Shaft Diameter: 0.10000000149011612 224 | Shaft Length: 0.10000000149011612 225 | Topic: /move_base/NavfnROS/plan 226 | Unreliable: false 227 | Value: true 228 | - Alpha: 1 229 | Class: rviz/Polygon 230 | Color: 25; 255; 0 231 | Enabled: true 232 | Name: Polygon 233 | Queue Size: 10 234 | Topic: /pause_dock_vicinity 235 | Unreliable: false 236 | Value: true 237 | Enabled: true 238 | Global Options: 239 | Background Color: 48; 48; 48 240 | Default Light: true 241 | Fixed Frame: map 242 | Frame Rate: 30 243 | Name: root 244 | Tools: 245 | - Class: rviz/Interact 246 | Hide Inactive Objects: true 247 | - Class: rviz/MoveCamera 248 | - Class: rviz/Select 249 | - Class: rviz/FocusCamera 250 | - Class: rviz/Measure 251 | - Class: rviz/SetInitialPose 252 | Theta std deviation: 0.2617993950843811 253 | Topic: /initialpose 254 | X std deviation: 0.5 255 | Y std deviation: 0.5 256 | - Class: rviz/SetGoal 257 | Topic: /move_base_simple/goal 258 | - Class: rviz/PublishPoint 259 | Single click: true 260 | Topic: /clicked_point 261 | Value: true 262 | Views: 263 | Current: 264 | Class: rviz/Orbit 265 | Distance: 12.993084907531738 266 | Enable Stereo Rendering: 267 | Stereo Eye Separation: 0.05999999865889549 268 | Stereo Focal Distance: 1 269 | Swap Stereo Eyes: false 270 | Value: false 271 | Field of View: 0.7853981852531433 272 | Focal Point: 273 | X: 0.3537686765193939 274 | Y: 0.6861514449119568 275 | Z: 0.2940061092376709 276 | Focal Shape Fixed Size: true 277 | Focal Shape Size: 0.05000000074505806 278 | Invert Z Axis: false 279 | Name: Current View 280 | Near Clip Distance: 0.009999999776482582 281 | Pitch: 1.5697963237762451 282 | Target Frame: 283 | Yaw: 3.1135942935943604 284 | Saved: ~ 285 | Window Geometry: 286 | Displays: 287 | collapsed: false 288 | Height: 1147 289 | Hide Left Dock: false 290 | Hide Right Dock: false 291 | Image: 292 | collapsed: false 293 | QMainWindow State: 000000ff00000000fd00000004000000000000018a000003d7fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001f2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000235000001df0000001600ffffff000000010000010f0000029efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000430000029e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000066a00000044fc0100000002fb0000000800540069006d006501000000000000066a000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004da000003d700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 294 | Selection: 295 | collapsed: false 296 | Time: 297 | collapsed: false 298 | Tool Properties: 299 | collapsed: false 300 | Views: 301 | collapsed: false 302 | Width: 1642 303 | X: 190 304 | Y: 109 305 | -------------------------------------------------------------------------------- /autodock_examples/scripts/dock_robot_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2021 Open Source Robotics Foundation, Inc. 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 | 18 | import random 19 | import argparse 20 | 21 | # ros 22 | import rospy 23 | import actionlib 24 | from geometry_msgs.msg import Twist 25 | from std_srvs.srv import Trigger 26 | from autodock_core.msg import AutoDockingAction, AutoDockingGoal 27 | 28 | 29 | ############################################################################## 30 | 31 | 32 | def autodock_client(): 33 | # Creates the SimpleActionClient, passing the type of the action 34 | client = actionlib.SimpleActionClient( 35 | 'autodock_action', AutoDockingAction) 36 | client.wait_for_server() 37 | 38 | # Sends the goal to the action server. 39 | goal_msg = AutoDockingGoal() 40 | client.send_goal(goal_msg) 41 | 42 | # Waits for the server to finish performing the action. 43 | client.wait_for_result() 44 | return client.get_result() 45 | 46 | 47 | def stop_charging(charger_srv_name): 48 | if not charger_srv_name: 49 | return False 50 | 51 | print(f"calling stop charging srv: ", charger_srv_name) 52 | try: 53 | _trigger = rospy.ServiceProxy(charger_srv_name, Trigger) 54 | res = _trigger() 55 | print(f"stop charger srv, " 56 | f"success [{res.success}] | msg: {res.message}") 57 | if not res.success: 58 | print("stop charger failed") 59 | return False 60 | except rospy.ServiceException as e: 61 | print(f"Stop Charger srv call failed: {e}") 62 | return False 63 | 64 | 65 | def move_robot_randomly(move_duration, linear_range, angular_range): 66 | """ 67 | Move the robot to a predocking position from the docking station, by 68 | using `/cmd_vel` topic. 69 | :arg move_duration: How long to move the robot in secs (Float) 70 | :arg linear_range: Set with upper and lower bound of the linear vel 71 | :arg angular_range: Set wiht upper and lower bound of the angular vel 72 | """ 73 | print(f"Move Robot with CMD VEL!") 74 | pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) 75 | 76 | sleep_duration = 0.2 77 | linear_vel = random.uniform(linear_range[0], linear_range[1]) 78 | angular_vel = random.uniform(angular_range[0], angular_range[1]) 79 | 80 | total_rotation = move_duration*angular_vel * 180/3.14 81 | print(f" => cmd_vel: [{linear_vel:.3f}, {angular_vel :.3f}]") 82 | print(f" => to travel: {move_duration*linear_vel}m, " 83 | f"turn {total_rotation} degree") 84 | msg = Twist() 85 | msg.linear.x = linear_vel 86 | msg.angular.z = angular_vel 87 | 88 | _loop_count = int(move_duration/sleep_duration) 89 | for _ in range(_loop_count): 90 | pub.publish(msg) 91 | rospy.sleep(rospy.Duration(sleep_duration)) 92 | # stop 93 | print(f"Reached random position, STOP!!") 94 | 95 | msg.linear.x = 0 96 | msg.angular.z = 0 97 | pub.publish(msg) 98 | 99 | 100 | ############################################################################## 101 | 102 | if __name__ == "__main__": 103 | parser = argparse.ArgumentParser() 104 | parser.add_argument("-c", "--count", type=int, default=1, 105 | help="loop count, default 1") 106 | parser.add_argument("-r", "--retreat_duration", type=float, default=6.0, 107 | help="Duration to retreat after one docking cycle") 108 | parser.add_argument("-ll", "--lower_linear", type=float, default=0.12, 109 | help="Lower bound of the linear vel during retreat") 110 | parser.add_argument("-ul", "--upper_linear", type=float, default=0.20, 111 | help="Lower bound of the linear vel during retreat") 112 | parser.add_argument("-la", "--lower_angular", type=float, default=-0.06, 113 | help="Lower bound of the angular vel during retreat") 114 | parser.add_argument("-ua", "--upper_angular", type=float, default=0.06, 115 | help="Upper bound of the angular vel during retreat") 116 | parser.add_argument("-cs", "--charger_srv_name", type=str, default=None, 117 | help="Charging station srv name, default None") 118 | args = parser.parse_args() 119 | 120 | # This is to indicate the range of the robot vel during retreat after 121 | # one docking cycle. 122 | linear_range = (args.lower_linear, args.upper_linear) 123 | angular_range = (args.lower_angular, args.upper_angular) 124 | 125 | print(f"Start Docking Test Script, with {args.count} random requests") 126 | rospy.init_node('test_dock_client') 127 | 128 | for i in range(args.count): 129 | try: 130 | print("\n" + "-----"*5 + f"Start {i}" + "-----"*5) 131 | stop_charging(args.charger_srv_name) 132 | move_robot_randomly( 133 | args.retreat_duration, linear_range, angular_range) 134 | 135 | print("-----"*5 + "Start Dock" + "-----"*5) 136 | result = autodock_client() 137 | 138 | print("-----"*5 + "Done" + "-----"*5) 139 | print("Action response: ", result) 140 | assert result.is_success, "Failed to Dock" 141 | 142 | except rospy.ROSInterruptException: 143 | print("program interrupted before completion") 144 | -------------------------------------------------------------------------------- /autodock_examples/scripts/dock_sim_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2021 Open Source Robotics Foundation, Inc. 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 | 18 | import os 19 | import subprocess 20 | import random 21 | import argparse 22 | from typing import List 23 | 24 | # ros 25 | import rospy 26 | import actionlib 27 | from autodock_core.msg import AutoDockingAction, AutoDockingGoal 28 | 29 | 30 | # Params 31 | model_name = "mock_docking_robot" 32 | cam_offset = 0.35 # offset from base_link to cam_link 33 | stop_distance = 0.10 # prevent collission with the charging station 34 | operating_area = { 35 | 'x': (1.5, 2.1), 36 | 'y': (-0.3, 0.3), 37 | 'z': (-0.4, 0.4) 38 | } 39 | 40 | 41 | ############################################################################## 42 | 43 | 44 | def autodock_client(): 45 | # Creates the SimpleActionClient, passing the type of the action 46 | client = actionlib.SimpleActionClient( 47 | 'autodock_action', AutoDockingAction) 48 | client.wait_for_server() 49 | 50 | # Sends the goal to the action server. 51 | goal_msg = AutoDockingGoal() 52 | client.send_goal(goal_msg) 53 | 54 | # Waits for the server to finish performing the action. 55 | client.wait_for_result() 56 | return client.get_result() 57 | 58 | 59 | def move_robot_randomly(): 60 | # change robot pose 61 | _x = random.uniform(operating_area['x'][0], operating_area['x'][1]) 62 | _y = random.uniform(operating_area['y'][0], operating_area['y'][1]) 63 | _yaw = random.uniform(operating_area['z'][0], operating_area['z'][1]) 64 | pose_cmd = (f"gz model -m {model_name} " 65 | f"-x {_x:.2f} -y {_y:.2f} -Y {_yaw:.2f}") 66 | print(f"Move robot with command: [{pose_cmd}]") 67 | os.system(pose_cmd) 68 | 69 | 70 | def get_robot_pose(): 71 | get_pose_cmd = (f"gz model -p -m {model_name}") 72 | 73 | print(f"Get Pose with command: [{get_pose_cmd}]") 74 | ret = subprocess.check_output(get_pose_cmd, shell=True).decode("utf-8") 75 | pose = ret.strip().split(" ") 76 | return float(pose[0]), float(pose[1]), float(pose[5]) 77 | 78 | 79 | def check_result(pose, result, allowance=(0.06, 0.06, 0.06)): 80 | x_allowance = allowance[0] 81 | y_allowance = allowance[1] 82 | yaw_allowance = allowance[2] 83 | if result is None or not result.is_success: 84 | print("invalid action result") 85 | return False 86 | 87 | # compare robot edge to charging station edge distance, with safety tol 88 | _x = abs(pose[0]) - cam_offset - stop_distance 89 | if _x > x_allowance: 90 | print(f"Err: exceed x {_x} allowance of {x_allowance}") 91 | return False 92 | 93 | if abs(pose[1]) > y_allowance: 94 | print(f"Err: exceed y {pose[1]} allowance of {y_allowance}") 95 | return False 96 | if abs(pose[2]) > yaw_allowance: 97 | print(f"Err: exceed yaw {pose[2]} allowance of {yaw_allowance}") 98 | return False 99 | return True 100 | 101 | 102 | ############################################################################## 103 | 104 | 105 | if __name__ == "__main__": 106 | parser = argparse.ArgumentParser() 107 | parser.add_argument("-c", "--count", type=int, default=1, 108 | help="loop count, default 1") 109 | parser.add_argument("-x", "--x_allowance", type=float, default=0.03, 110 | help="x allowance, default 0.03") 111 | parser.add_argument("-y", "--y_allowance", type=float, default=0.03, 112 | help="y allowance, default 0.03") 113 | parser.add_argument("-Y", "--yaw_allowance", type=float, default=0.03, 114 | help="yaw allowance, default 0.03") 115 | args = parser.parse_args() 116 | 117 | _allowance = (args.x_allowance, args.y_allowance, args.yaw_allowance) 118 | print(f"Start Docking Test Script, with {args.count} random requests") 119 | print(f"Operating Area is {operating_area}") 120 | print(f"Allowance, (x, y, yaw) is {_allowance}") 121 | 122 | rospy.init_node('test_dock_client') 123 | 124 | for i in range(args.count): 125 | try: 126 | print("\n" + "-----"*5 + f"Start {i}" + "-----"*5) 127 | move_robot_randomly() 128 | result = autodock_client() 129 | print("-----"*5 + "Done" + "-----"*5) 130 | print("Action response: ", result) 131 | robot_pose = get_robot_pose() 132 | print("Robot is located at: ", robot_pose) 133 | print("-----"*5 + "Check" + "-----"*5) 134 | check = check_result(robot_pose, result, allowance=_allowance) 135 | print("Check result: ", check) 136 | assert check, "Failed to Dock" 137 | except rospy.ROSInterruptException: 138 | print("program interrupted before completion") 139 | -------------------------------------------------------------------------------- /autodock_sim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autodock_sim) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | gazebo_dev 6 | cmake_modules 7 | roslib 8 | roscpp 9 | geometry_msgs 10 | std_srvs 11 | tf 12 | rosgraph_msgs 13 | dynamic_reconfigure 14 | std_msgs 15 | gazebo_msgs 16 | ) 17 | 18 | catkin_package( 19 | LIBRARIES 20 | ChargingStationPlugin 21 | 22 | CATKIN_DEPENDS 23 | roslib 24 | roscpp 25 | geometry_msgs 26 | std_srvs 27 | tf 28 | rosgraph_msgs 29 | dynamic_reconfigure 30 | std_msgs 31 | gazebo_msgs 32 | ) 33 | 34 | include_directories( 35 | include 36 | ${Boost_INCLUDE_DIRS} 37 | ${catkin_INCLUDE_DIRS} 38 | ${TinyXML_INCLUDE_DIRS}) 39 | 40 | link_directories(${catkin_LIBRARY_DIRS}) 41 | 42 | set(cxx_flags) 43 | foreach (item ${GAZEBO_CFLAGS}) 44 | set(cxx_flags "${cxx_flags} ${item}") 45 | endforeach () 46 | 47 | set(ld_flags) 48 | foreach (item ${GAZEBO_LDFLAGS}) 49 | set(ld_flags "${ld_flags} ${item}") 50 | endforeach () 51 | 52 | include_directories(${GAZEBO_INCLUDE_DIRS}) 53 | link_directories(${GAZEBO_LIBRARY_DIRS}) 54 | 55 | # build charging station plugin 56 | add_library(ChargingStationPlugin SHARED src/ChargingStationPlugin.cpp) 57 | target_link_libraries(ChargingStationPlugin 58 | ${GAZEBO_LIBRARIES} 59 | ${catkin_LIBRARIES}) 60 | 61 | # Install Gazebo launch files 62 | install(DIRECTORY launch/ 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 64 | ) 65 | -------------------------------------------------------------------------------- /autodock_sim/launch/dock_sim.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 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 48 | 49 | 50 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /autodock_sim/launch/tb3_dock_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 50 | 51 | 52 | 53 | 57 | 58 | 59 | 63 | 64 | -------------------------------------------------------------------------------- /autodock_sim/launch/tb3_nav_dock_sim.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 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 64 | 65 | 66 | 68 | 69 | 70 | 71 | 75 | 76 | 77 | 81 | 82 | -------------------------------------------------------------------------------- /autodock_sim/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/maps/map.pgm -------------------------------------------------------------------------------- /autodock_sim/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial10/materials/scripts/Fiducial10.material: -------------------------------------------------------------------------------- 1 | material Fiducial10/Marker 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture 6x6_1000-10.png 10 | } 11 | } 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial10/materials/textures/6x6_1000-10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/Fiducial10/materials/textures/6x6_1000-10.png -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial10/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Fiducial10 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial10/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0 0 0 3.14 7 | 8 | 9 | 10 | 0.15 0.15 1e-5 11 | 12 | 13 | 14 | 19 | 1 1 1 1 20 | 1 1 1 1 21 | 0 0 0 1 22 | 1 1 1 0 23 | 24 | __default__ 25 | 26 | 27 | 0 0 0 0 -0 0 28 | 1 29 | 0 30 | 31 | 32 | 33 | 1 34 | 1 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial11/materials/scripts/Fiducial11.material: -------------------------------------------------------------------------------- 1 | material Fiducial11/Marker 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture 6x6_1000-11.png 10 | } 11 | } 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial11/materials/textures/6x6_1000-11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/Fiducial11/materials/textures/6x6_1000-11.png -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial11/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Fiducial11 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial11/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0 0 0 3.14 7 | 8 | 9 | 10 | 0.15 0.15 1e-5 11 | 12 | 13 | 14 | 19 | 1 1 1 1 20 | 1 1 1 1 21 | 0 0 0 1 22 | 1 1 1 0 23 | 24 | __default__ 25 | 26 | 27 | 0 0 0 0 -0 0 28 | 1 29 | 0 30 | 31 | 32 | 33 | 1 34 | 1 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial20/materials/scripts/Fiducial20.material: -------------------------------------------------------------------------------- 1 | material Fiducial20/Marker 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture 6x6_1000-20.png 10 | } 11 | } 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial20/materials/textures/6x6_1000-20.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/Fiducial20/materials/textures/6x6_1000-20.png -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial20/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Fiducial20 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /autodock_sim/models/Fiducial20/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0 0 0 3.14 7 | 8 | 9 | 10 | 0.05 0.05 1e-5 11 | 12 | 13 | 14 | 19 | 1 1 1 1 20 | 1 1 1 1 21 | 0 0 0 1 22 | 1 1 1 0 23 | 24 | __default__ 25 | 26 | 27 | 0 0 0 0 -0 0 28 | 1 29 | 0 30 | 31 | 32 | 33 | 1 34 | 1 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /autodock_sim/models/MiniMockCharger/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | MiniMockCharger 5 | 1.0 6 | model.sdf 7 | 8 | 9 | YouLiang 10 | 11 | 12 | 13 | 14 | Mini Mock Charging Station 15 | 16 | 17 | -------------------------------------------------------------------------------- /autodock_sim/models/MiniMockCharger/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 5 | 0 6 | 7 | 8 | 9 | -0.051 0 0.2 0 0 0 10 | 11 | 12 | 13 | 14 | .1 .5 0.4 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | .1 .5 0.4 23 | 24 | 25 | 26 | .8 1 1 1 27 | 1 1 1 1 28 | 0.1 0.1 0.1 1 29 | 0.3 0.3 0.3 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 0 -0.1 0.13 -0 -1.57 3.14 37 | 38 | 39 | 40 | 0.1 0.1 1e-5 41 | 42 | 43 | 44 | 49 | 0 0 0 1 50 | 1 1 1 0 51 | 52 | 53 | 54 | 55 | 56 | 0 0 0.13 -0 -1.57 3.14 57 | 58 | 59 | 60 | 0.04 0.04 1e-5 61 | 62 | 63 | 64 | 69 | 0 0 0 1 70 | 1 1 1 0 71 | 72 | 73 | 74 | 75 | 76 | 77 | 0 0.1 0.13 -0 -1.57 3.14 78 | 79 | 80 | 81 | 0.1 0.1 1e-5 82 | 83 | 84 | 85 | 90 | 0 0 0 1 91 | 1 1 1 0 92 | 93 | 94 | 95 | 96 | 97 | charger_base 98 | left_marker 99 | 100 | 101 | 102 | charger_base 103 | centre_marker 104 | 105 | 106 | 107 | charger_base 108 | right_marker 109 | 110 | 111 | 112 | charger_base 113 | charger_contact 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /autodock_sim/models/MockCharger/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | MockCharger 5 | 1.0 6 | model.sdf 7 | 8 | 9 | YouLiang 10 | 11 | 12 | 13 | 14 | Mock AutoDock Charging Station 15 | 16 | 17 | -------------------------------------------------------------------------------- /autodock_sim/models/MockCharger/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 5 | 0 6 | 7 | 8 | 9 | -0.201 0 0.6 0 0 0 10 | 11 | 12 | 13 | 14 | .4 .8 1.2 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | .4 .8 1.2 23 | 24 | 25 | 26 | .7 .7 .5 1 27 | 1 1 1 1 28 | 0.1 0.1 0.1 1 29 | 0 0 0 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 0 0 0.8 0 0 0 37 | 38 | 39 | 0.1 0 0.7 0 0 0 40 | 0 41 | 42 | 0.6 43 | 1 44 | 1 45 | 46 | 47 | 48 | 49 | 2 50 | 51 | 52 | .06 .2 .2 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | .06 .2 .2 61 | 62 | 63 | 64 | .7 0 0 1 65 | 1 1 1 1 66 | 0.1 0.1 0.1 1 67 | 0 0 0 0 68 | 69 | 70 | 71 | 72 | 73 | true 74 | 0.5 75 | 76 | charger_collision 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | true 86 | 87 | charger_contact/charger_led 88 | 1 89 | 1 90 | 1 0.1 0 91 | 92 | 0 1 0 93 | 15 94 | mock_docking_robot::body::collision 95 | 96 | /gazebo/default/charger_base1/charger_contact/charger_contactsensor/contacts 97 | 98 | 99 | 100 | 101 | 102 | left_aruco_tag 103 | model://Fiducial10 104 | 0 -0.2 0.2 0 -1.57 0.0 105 | true 106 | 107 | 108 | 109 | 110 | centre_aruco_tag 111 | model://Fiducial20 112 | 0 0 0.2 -0 -1.57 0 113 | true 114 | 115 | 116 | 117 | 118 | right_aruco_tag 119 | model://Fiducial11 120 | 0 0.2 0.2 -0 -1.57 0 121 | true 122 | 123 | 124 | 125 | charger_base 126 | left_aruco_tag::marker 127 | 128 | 129 | 130 | charger_base 131 | centre_aruco_tag::marker 132 | 133 | 134 | 135 | charger_base 136 | right_aruco_tag::marker 137 | 138 | 139 | 140 | charger_base 141 | charger_contact 142 | 143 | 144 | 145 | -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/meshes/FatWheel.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'FatWheel.blend' 2 | # Material Count: 1 3 | 4 | newmtl MockRobot 5 | Ns 96.078431 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.197778 0.197778 0.197778 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd MockRobot_Diffuse.png 14 | -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/meshes/FatWheel.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.79 (sub 0) OBJ File: 'FatWheel.blend' 2 | # www.blender.org 3 | mtllib FatWheel.mtl 4 | o FatWheelR_Cube.008 5 | v 0.000000 0.016688 0.043389 6 | v 0.000000 -0.016700 0.043389 7 | v 0.018048 0.016688 0.038553 8 | v 0.018048 -0.016700 0.038553 9 | v 0.033654 0.016688 0.022946 10 | v 0.033655 -0.016700 0.022946 11 | v 0.039803 0.016688 0.000000 12 | v 0.039803 -0.016700 0.000000 13 | v 0.033654 0.016688 -0.022946 14 | v 0.033655 -0.016700 -0.022946 15 | v 0.018048 0.016688 -0.038553 16 | v 0.018048 -0.016700 -0.038553 17 | v 0.000000 0.016688 -0.043389 18 | v 0.000000 -0.016700 -0.043389 19 | v -0.018048 0.016688 -0.038553 20 | v -0.018048 -0.016700 -0.038553 21 | v -0.033655 0.016688 -0.022946 22 | v -0.033655 -0.016700 -0.022946 23 | v -0.039803 0.016688 0.000000 24 | v -0.039803 -0.016700 0.000000 25 | v -0.033655 0.016688 0.022946 26 | v -0.033655 -0.016700 0.022946 27 | v -0.018048 0.016688 0.038553 28 | v -0.018048 -0.016700 0.038553 29 | v 0.000000 0.014746 0.056004 30 | v 0.000000 -0.014758 0.056004 31 | v 0.028002 -0.014758 0.048501 32 | v 0.028002 0.014746 0.048501 33 | v 0.048501 -0.014758 0.028002 34 | v 0.048501 0.014746 0.028002 35 | v 0.056004 -0.014758 0.000000 36 | v 0.056004 0.014746 0.000000 37 | v 0.048501 -0.014758 -0.028002 38 | v 0.048501 0.014746 -0.028002 39 | v 0.028002 -0.014758 -0.048501 40 | v 0.028002 0.014746 -0.048501 41 | v 0.000000 -0.014758 -0.056004 42 | v 0.000000 0.014746 -0.056004 43 | v -0.028002 -0.014758 -0.048501 44 | v -0.028002 0.014746 -0.048501 45 | v -0.048501 -0.014758 -0.028002 46 | v -0.048501 0.014746 -0.028002 47 | v -0.056004 -0.014758 0.000000 48 | v -0.056004 0.014746 0.000000 49 | v -0.048501 -0.014758 0.028002 50 | v -0.048501 0.014746 0.028002 51 | v -0.028002 -0.014758 0.048501 52 | v -0.028002 0.014746 0.048501 53 | v 0.000000 0.018155 0.056004 54 | v 0.028002 0.018155 0.048501 55 | v 0.028002 -0.018166 0.048501 56 | v 0.000000 -0.018166 0.056004 57 | v 0.048501 0.018155 0.028002 58 | v 0.048501 -0.018166 0.028002 59 | v 0.056004 0.018155 0.000000 60 | v 0.056004 -0.018166 0.000000 61 | v 0.048501 0.018155 -0.028002 62 | v 0.048501 -0.018166 -0.028002 63 | v 0.028002 0.018155 -0.048501 64 | v 0.028002 -0.018166 -0.048501 65 | v 0.000000 0.018155 -0.056004 66 | v 0.000000 -0.018166 -0.056004 67 | v -0.028002 0.018155 -0.048501 68 | v -0.028002 -0.018166 -0.048501 69 | v -0.048501 0.018155 -0.028002 70 | v -0.048501 -0.018166 -0.028002 71 | v -0.056004 0.018155 0.000000 72 | v -0.056004 -0.018166 0.000000 73 | v -0.048501 0.018155 0.028002 74 | v -0.048501 -0.018166 0.028002 75 | v -0.028002 0.018155 0.048501 76 | v -0.028002 -0.018166 0.048501 77 | v 0.000000 0.018155 0.054362 78 | v 0.026442 0.018155 0.047277 79 | v 0.026442 -0.018166 0.047277 80 | v 0.000000 -0.018166 0.054362 81 | v 0.046284 0.018155 0.027435 82 | v 0.046284 -0.018166 0.027435 83 | v 0.053635 0.018155 0.000000 84 | v 0.053635 -0.018166 0.000000 85 | v 0.046284 0.018155 -0.027435 86 | v 0.046284 -0.018166 -0.027435 87 | v 0.026442 0.018155 -0.047277 88 | v 0.026442 -0.018166 -0.047277 89 | v 0.000000 0.018155 -0.054362 90 | v 0.000000 -0.018166 -0.054362 91 | v -0.026442 0.018155 -0.047277 92 | v -0.026442 -0.018166 -0.047277 93 | v -0.046284 0.018155 -0.027435 94 | v -0.046284 -0.018166 -0.027435 95 | v -0.053635 0.018155 0.000000 96 | v -0.053635 -0.018166 0.000000 97 | v -0.046284 0.018155 0.027435 98 | v -0.046284 -0.018166 0.027435 99 | v -0.026442 0.018155 0.047277 100 | v -0.026442 -0.018166 0.047277 101 | v 0.015133 -0.011577 0.034264 102 | v 0.000000 -0.011577 0.038319 103 | v -0.015133 -0.011577 0.034264 104 | v -0.028856 -0.011577 0.020541 105 | v -0.034360 -0.011577 0.000000 106 | v -0.028856 -0.011577 -0.020541 107 | v -0.015133 -0.011577 -0.034264 108 | v 0.000000 -0.011577 -0.038319 109 | v 0.015133 -0.011577 -0.034264 110 | v 0.028856 -0.011577 -0.020541 111 | v 0.034360 -0.011577 0.000000 112 | v 0.028856 -0.011577 0.020541 113 | v 0.000000 0.011565 0.038319 114 | v 0.015133 0.011565 0.034264 115 | v 0.028856 0.011565 0.020541 116 | v 0.034360 0.011565 0.000000 117 | v 0.028856 0.011565 -0.020541 118 | v 0.015133 0.011565 -0.034264 119 | v 0.000000 0.011565 -0.038319 120 | v -0.015133 0.011565 -0.034264 121 | v -0.028856 0.011565 -0.020541 122 | v -0.034360 0.011565 0.000000 123 | v -0.028856 0.011565 0.020541 124 | v -0.015133 0.011565 0.034264 125 | v 0.000000 -0.011577 0.039825 126 | v 0.016565 -0.011577 0.035386 127 | v -0.016566 -0.011577 0.035386 128 | v -0.030890 -0.011577 0.021062 129 | v -0.036534 -0.011577 0.000000 130 | v -0.030890 -0.011577 -0.021062 131 | v -0.016565 -0.011577 -0.035387 132 | v 0.000000 -0.011577 -0.039825 133 | v 0.016565 -0.011577 -0.035386 134 | v 0.030890 -0.011577 -0.021062 135 | v 0.036534 -0.011577 0.000000 136 | v 0.030890 -0.011577 0.021062 137 | v 0.016565 0.011565 0.035386 138 | v 0.000000 0.011565 0.039825 139 | v 0.030890 0.011565 0.021062 140 | v 0.036534 0.011565 0.000000 141 | v 0.030890 0.011565 -0.021062 142 | v 0.016565 0.011565 -0.035386 143 | v 0.000000 0.011565 -0.039825 144 | v -0.016566 0.011565 -0.035387 145 | v -0.030890 0.011565 -0.021062 146 | v -0.036534 0.011565 0.000000 147 | v -0.030890 0.011565 0.021062 148 | v -0.016566 0.011565 0.035386 149 | v 0.000000 -0.016911 0.044971 150 | v 0.019258 -0.016911 0.039811 151 | v 0.035476 -0.016911 0.023593 152 | v 0.041797 -0.016911 0.000000 153 | v 0.035476 -0.016911 -0.023593 154 | v 0.019258 -0.016911 -0.039811 155 | v 0.000000 -0.016911 -0.044971 156 | v -0.019258 -0.016911 -0.039811 157 | v -0.035476 -0.016911 -0.023593 158 | v -0.041797 -0.016911 0.000000 159 | v -0.035476 -0.016911 0.023593 160 | v -0.019258 -0.016911 0.039811 161 | v 0.019434 0.016982 0.039698 162 | v 0.000000 0.016982 0.044905 163 | v 0.035644 0.016982 0.023489 164 | v 0.041937 0.016982 0.000000 165 | v 0.035644 0.016982 -0.023489 166 | v 0.019434 0.016982 -0.039698 167 | v 0.000000 0.016982 -0.044905 168 | v -0.019435 0.016982 -0.039698 169 | v -0.035644 0.016982 -0.023489 170 | v -0.041937 0.016982 0.000000 171 | v -0.035644 0.016982 0.023489 172 | v -0.019435 0.016982 0.039698 173 | vt 0.225693 0.889772 174 | vt 0.212431 0.882111 175 | vt 0.255080 0.860371 176 | vt 0.247423 0.751230 177 | vt 0.234161 0.758891 178 | vt 0.212431 0.716221 179 | vt 0.204774 0.868842 180 | vt 0.204774 0.854861 181 | vt 0.220186 0.839441 182 | vt 0.210415 0.845085 183 | vt 0.234161 0.839441 184 | vt 0.247423 0.847102 185 | vt 0.255080 0.874352 186 | vt 0.249439 0.884128 187 | vt 0.239668 0.889772 188 | vt 0.220186 0.758891 189 | vt 0.210415 0.753247 190 | vt 0.204774 0.743472 191 | vt 0.204774 0.729490 192 | vt 0.225693 0.708561 193 | vt 0.255080 0.737961 194 | vt 0.239668 0.708561 195 | vt 0.255080 0.723980 196 | vt 0.249439 0.714204 197 | vt 0.282537 0.854192 198 | vt 0.280082 0.874775 199 | vt 0.280082 0.854192 200 | vt 0.282537 0.874775 201 | vt 0.280082 0.894373 202 | vt 0.282537 0.707259 203 | vt 0.280082 0.723497 204 | vt 0.280082 0.707259 205 | vt 0.282537 0.744128 206 | vt 0.280082 0.744128 207 | vt 0.282537 0.763623 208 | vt 0.280082 0.763623 209 | vt 0.280082 0.894373 210 | vt 0.282537 0.910425 211 | vt 0.280082 0.910425 212 | vt 0.282537 0.931008 213 | vt 0.280082 0.931008 214 | vt 0.282537 0.950606 215 | vt 0.280082 0.950606 216 | vt 0.151717 0.863154 217 | vt 0.149263 0.879392 218 | vt 0.149263 0.863154 219 | vt 0.151717 0.879392 220 | vt 0.149263 0.900023 221 | vt 0.151717 0.900023 222 | vt 0.149263 0.919518 223 | vt 0.282537 0.838140 224 | vt 0.280082 0.838140 225 | vt 0.258836 0.838140 226 | vt 0.256381 0.854192 227 | vt 0.256381 0.838140 228 | vt 0.258836 0.854192 229 | vt 0.128016 0.900023 230 | vt 0.125562 0.919518 231 | vt 0.125562 0.900023 232 | vt 0.128016 0.919518 233 | vt 0.128016 0.879392 234 | vt 0.125562 0.879392 235 | vt 0.128016 0.863154 236 | vt 0.125562 0.863154 237 | vt 0.258836 0.931008 238 | vt 0.256381 0.950606 239 | vt 0.256381 0.931008 240 | vt 0.258836 0.950606 241 | vt 0.258836 0.910425 242 | vt 0.256381 0.910425 243 | vt 0.258836 0.894373 244 | vt 0.256381 0.894373 245 | vt 0.256381 0.744128 246 | vt 0.258836 0.763623 247 | vt 0.256381 0.763623 248 | vt 0.258836 0.744128 249 | vt 0.258836 0.723497 250 | vt 0.256381 0.723497 251 | vt 0.258836 0.707259 252 | vt 0.256381 0.707259 253 | vt 0.258836 0.874775 254 | vt 0.256381 0.894373 255 | vt 0.256381 0.874775 256 | vt 0.258836 0.894373 257 | vt 0.059402 0.774203 258 | vt 0.076168 0.785206 259 | vt 0.058089 0.774764 260 | vt 0.075996 0.861736 261 | vt 0.058089 0.852711 262 | vt 0.058924 0.851875 263 | vt 0.049298 0.756695 264 | vt 0.047650 0.756676 265 | vt 0.096204 0.861736 266 | vt 0.076168 0.863154 267 | vt 0.049298 0.736231 268 | vt 0.047650 0.735790 269 | vt 0.113917 0.851504 270 | vt 0.097044 0.863154 271 | vt 0.058088 0.717702 272 | vt 0.059525 0.718510 273 | vt 0.125562 0.834623 274 | vt 0.115123 0.852711 275 | vt 0.076168 0.707259 276 | vt 0.077025 0.708401 277 | vt 0.124144 0.833783 278 | vt 0.125562 0.813737 279 | vt 0.097044 0.707259 280 | vt 0.096738 0.708401 281 | vt 0.124144 0.813566 282 | vt 0.115123 0.795650 283 | vt 0.113810 0.718262 284 | vt 0.115123 0.717702 285 | vt 0.097216 0.786624 286 | vt 0.114288 0.796486 287 | vt 0.123914 0.735771 288 | vt 0.125562 0.735790 289 | vt 0.077008 0.786624 290 | vt 0.097044 0.785206 291 | vt 0.123914 0.756234 292 | vt 0.125562 0.756676 293 | vt 0.059295 0.796856 294 | vt 0.076168 0.785206 295 | vt 0.115123 0.774763 296 | vt 0.113687 0.773956 297 | vt 0.047650 0.813737 298 | vt 0.058089 0.795649 299 | vt 0.097044 0.785206 300 | vt 0.096187 0.784064 301 | vt 0.049068 0.814578 302 | vt 0.047650 0.834623 303 | vt 0.076474 0.784064 304 | vt 0.049068 0.834795 305 | vt 0.065689 0.770235 306 | vt 0.076140 0.854273 307 | vt 0.063706 0.847091 308 | vt 0.057435 0.755932 309 | vt 0.092656 0.854273 310 | vt 0.057435 0.738413 311 | vt 0.107889 0.845474 312 | vt 0.066190 0.723240 313 | vt 0.080486 0.714983 314 | vt 0.116684 0.830234 315 | vt 0.094975 0.714983 316 | vt 0.116684 0.813710 317 | vt 0.107523 0.722230 318 | vt 0.097072 0.794088 319 | vt 0.109506 0.801270 320 | vt 0.115777 0.736533 321 | vt 0.080556 0.794088 322 | vt 0.115777 0.754053 323 | vt 0.065322 0.802887 324 | vt 0.107021 0.769225 325 | vt 0.092725 0.777483 326 | vt 0.056528 0.818127 327 | vt 0.078236 0.777483 328 | vt 0.056528 0.834651 329 | vt 0.326423 0.779425 330 | vt 0.330112 0.767234 331 | vt 0.330112 0.780500 332 | vt 0.330112 0.790847 333 | vt 0.320005 0.959773 334 | vt 0.323694 0.972965 335 | vt 0.320005 0.974616 336 | vt 0.320005 0.942868 337 | vt 0.323694 0.959342 338 | vt 0.323694 0.931611 339 | vt 0.320005 0.929561 340 | vt 0.322734 0.834921 341 | vt 0.326423 0.819986 342 | vt 0.326423 0.834907 343 | vt 0.322734 0.847097 344 | vt 0.326423 0.848173 345 | vt 0.326423 0.858519 346 | vt 0.128720 0.934360 347 | vt 0.125031 0.921168 348 | vt 0.128720 0.919518 349 | vt 0.128720 0.951266 350 | vt 0.125031 0.934792 351 | vt 0.125031 0.962522 352 | vt 0.128720 0.964572 353 | vt 0.326423 0.767248 354 | vt 0.330112 0.752313 355 | vt 0.330112 0.908119 356 | vt 0.326423 0.895929 357 | vt 0.330112 0.894853 358 | vt 0.330112 0.923040 359 | vt 0.326423 0.908105 360 | vt 0.319045 0.916255 361 | vt 0.322734 0.927512 362 | vt 0.319045 0.929561 363 | vt 0.322734 0.899781 364 | vt 0.319045 0.899349 365 | vt 0.322734 0.886157 366 | vt 0.319045 0.884507 367 | vt 0.327383 0.956672 368 | vt 0.323694 0.968094 369 | vt 0.323694 0.957748 370 | vt 0.323694 0.944482 371 | vt 0.323694 0.929561 372 | vt 0.327383 0.944496 373 | vt 0.322734 0.765620 374 | vt 0.319045 0.754363 375 | vt 0.322734 0.752313 376 | vt 0.319045 0.782094 377 | vt 0.322734 0.782525 378 | vt 0.319045 0.795717 379 | vt 0.322734 0.797368 380 | vt 0.330112 0.884507 381 | vt 0.209648 0.844318 382 | vt 0.220343 0.838140 383 | vt 0.234932 0.838140 384 | vt 0.248530 0.845995 385 | vt 0.256381 0.859599 386 | vt 0.256381 0.874195 387 | vt 0.250206 0.884895 388 | vt 0.239511 0.891073 389 | vt 0.224922 0.891073 390 | vt 0.211324 0.883218 391 | vt 0.203473 0.869614 392 | vt 0.203473 0.855018 393 | vt 0.209648 0.754014 394 | vt 0.203473 0.743314 395 | vt 0.203473 0.728718 396 | vt 0.211324 0.715114 397 | vt 0.224922 0.707259 398 | vt 0.239511 0.707259 399 | vt 0.250206 0.713437 400 | vt 0.256381 0.724137 401 | vt 0.256381 0.738733 402 | vt 0.248530 0.752338 403 | vt 0.234932 0.760192 404 | vt 0.220343 0.760192 405 | vt 0.057785 0.834626 406 | vt 0.057785 0.818725 407 | vt 0.066338 0.803903 408 | vt 0.081153 0.795345 409 | vt 0.097047 0.795345 410 | vt 0.108700 0.802076 411 | vt 0.115427 0.813734 412 | vt 0.115427 0.829636 413 | vt 0.106874 0.844458 414 | vt 0.092059 0.853015 415 | vt 0.076165 0.853015 416 | vt 0.064512 0.846284 417 | vt 0.091974 0.776428 418 | vt 0.078519 0.776428 419 | vt 0.105739 0.768477 420 | vt 0.114292 0.753655 421 | vt 0.114292 0.736540 422 | vt 0.106345 0.722769 423 | vt 0.081238 0.716038 424 | vt 0.094693 0.716038 425 | vt 0.067473 0.723989 426 | vt 0.058919 0.738811 427 | vt 0.058919 0.755926 428 | vt 0.066867 0.769697 429 | vt 0.282537 0.894373 430 | vt 0.282537 0.723497 431 | vt 0.282537 0.894373 432 | vt 0.151717 0.919518 433 | vt 0.326423 0.788921 434 | vt 0.323694 0.943824 435 | vt 0.322734 0.821225 436 | vt 0.322734 0.856593 437 | vt 0.125031 0.950309 438 | vt 0.326423 0.753553 439 | vt 0.326423 0.921801 440 | vt 0.322734 0.915298 441 | vt 0.327383 0.966169 442 | vt 0.327383 0.930800 443 | vt 0.319045 0.766577 444 | vt 0.326423 0.886432 445 | vn 0.0000 -1.0000 -0.0000 446 | vn -0.0000 1.0000 0.0000 447 | vn 0.0000 -0.6532 0.7571 448 | vn 0.5000 0.0000 0.8660 449 | vn 0.0000 0.0000 1.0000 450 | vn 0.3786 -0.6532 0.6557 451 | vn 0.8660 0.0000 0.5000 452 | vn 0.6557 -0.6532 0.3786 453 | vn 1.0000 0.0000 0.0000 454 | vn 0.6557 -0.6532 -0.3786 455 | vn 0.8660 0.0000 -0.5000 456 | vn 0.3786 -0.6532 -0.6557 457 | vn 0.5000 0.0000 -0.8660 458 | vn 0.0000 -0.6532 -0.7571 459 | vn 0.0000 0.0000 -1.0000 460 | vn -0.3786 -0.6532 -0.6557 461 | vn -0.5000 0.0000 -0.8660 462 | vn -0.6557 -0.6532 -0.3786 463 | vn -0.8660 0.0000 -0.5000 464 | vn -1.0000 0.0000 0.0000 465 | vn -0.7571 -0.6532 0.0000 466 | vn -0.8660 0.0000 0.5000 467 | vn -0.6557 -0.6532 0.3786 468 | vn -0.5000 0.0000 0.8660 469 | vn -0.3786 -0.6532 0.6557 470 | vn 0.0000 0.6532 0.7571 471 | vn -0.3786 0.6532 0.6557 472 | vn -0.6557 0.6532 0.3786 473 | vn -0.7571 0.6532 0.0000 474 | vn -0.6557 0.6532 -0.3786 475 | vn -0.3786 0.6532 -0.6557 476 | vn 0.0000 0.6532 -0.7571 477 | vn 0.3786 0.6532 -0.6557 478 | vn 0.6557 0.6532 -0.3786 479 | vn 0.7571 0.6532 0.0000 480 | vn 0.6557 0.6532 0.3786 481 | vn 0.3786 0.6532 0.6557 482 | vn -0.0252 0.9988 -0.0408 483 | vn -0.0271 -0.9987 -0.0436 484 | vn 0.0000 -0.9984 -0.0553 485 | vn -0.0380 0.9991 -0.0208 486 | vn -0.0403 -0.9989 -0.0220 487 | vn -0.0416 0.9991 0.0000 488 | vn -0.0440 -0.9990 0.0000 489 | vn -0.0380 0.9991 0.0208 490 | vn 0.7571 -0.6532 0.0000 491 | vn -0.0252 0.9988 0.0408 492 | vn -0.0403 -0.9989 0.0220 493 | vn 0.0000 0.9987 0.0514 494 | vn -0.0271 -0.9987 0.0436 495 | vn 0.0252 0.9988 0.0408 496 | vn 0.0271 -0.9987 0.0436 497 | vn 0.0000 -0.9984 0.0553 498 | vn 0.0380 0.9991 0.0208 499 | vn 0.0403 -0.9989 0.0220 500 | vn 0.0416 0.9991 0.0000 501 | vn 0.0440 -0.9990 0.0000 502 | vn 0.0380 0.9991 -0.0208 503 | vn 0.0252 0.9988 -0.0408 504 | vn 0.0403 -0.9989 -0.0220 505 | vn 0.0000 0.9987 -0.0514 506 | vn 0.0271 -0.9987 -0.0436 507 | vn -0.0694 0.9903 -0.1201 508 | vn -0.0603 -0.9922 -0.1088 509 | vn 0.0000 -0.9912 -0.1325 510 | vn -0.1051 0.9926 -0.0609 511 | vn -0.0953 -0.9938 -0.0567 512 | vn -0.1151 0.9933 0.0000 513 | vn -0.1054 -0.9944 0.0000 514 | vn -0.1051 0.9926 0.0609 515 | vn -0.0694 0.9903 0.1201 516 | vn -0.0952 -0.9938 0.0567 517 | vn 0.0000 0.9885 0.1512 518 | vn -0.0603 -0.9922 0.1088 519 | vn 0.0694 0.9903 0.1201 520 | vn 0.0603 -0.9922 0.1088 521 | vn 0.0000 -0.9912 0.1325 522 | vn 0.1051 0.9926 0.0609 523 | vn 0.0953 -0.9938 0.0567 524 | vn 0.1151 0.9933 0.0000 525 | vn 0.1054 -0.9944 0.0000 526 | vn 0.1051 0.9926 -0.0609 527 | vn 0.0694 0.9903 -0.1201 528 | vn 0.0952 -0.9938 -0.0567 529 | vn 0.0000 0.9885 -0.1512 530 | vn 0.0603 -0.9922 -0.1088 531 | vn 0.0000 -0.8545 -0.5194 532 | vn -0.2286 -0.8831 -0.4096 533 | vn 0.0000 -0.8834 -0.4686 534 | vn 0.2286 -0.8831 -0.4096 535 | vn 0.4041 -0.8829 -0.2392 536 | vn 0.2664 -0.8514 -0.4517 537 | vn 0.4702 -0.8825 0.0000 538 | vn 0.4640 -0.8458 -0.2631 539 | vn 0.4640 -0.8458 0.2631 540 | vn 0.4041 -0.8829 0.2392 541 | vn 0.2664 -0.8514 0.4517 542 | vn 0.2286 -0.8831 0.4096 543 | vn 0.0000 -0.8545 0.5194 544 | vn 0.0000 -0.8834 0.4686 545 | vn -0.2286 -0.8831 0.4096 546 | vn -0.4041 -0.8829 0.2392 547 | vn -0.2664 -0.8514 0.4517 548 | vn -0.4702 -0.8826 0.0000 549 | vn -0.4640 -0.8458 0.2631 550 | vn -0.4640 -0.8458 -0.2631 551 | vn -0.4041 -0.8829 -0.2392 552 | vn -0.2664 -0.8514 -0.4517 553 | vn -0.2383 0.8684 -0.4348 554 | vn 0.0000 0.8545 -0.5194 555 | vn 0.0000 0.8667 -0.4988 556 | vn -0.4194 0.8725 -0.2507 557 | vn -0.2664 0.8514 -0.4517 558 | vn -0.4863 0.8738 0.0000 559 | vn -0.4640 0.8458 -0.2631 560 | vn -0.4640 0.8458 0.2631 561 | vn -0.4194 0.8725 0.2507 562 | vn -0.2664 0.8514 0.4517 563 | vn -0.2383 0.8684 0.4348 564 | vn 0.0000 0.8545 0.5194 565 | vn 0.0000 0.8667 0.4988 566 | vn 0.2383 0.8684 0.4348 567 | vn 0.4194 0.8725 0.2507 568 | vn 0.2664 0.8514 0.4517 569 | vn 0.4863 0.8738 0.0000 570 | vn 0.4640 0.8458 0.2631 571 | vn 0.4640 0.8458 -0.2631 572 | vn 0.4194 0.8725 -0.2507 573 | vn 0.2664 0.8514 -0.4517 574 | vn 0.2383 0.8684 -0.4348 575 | vn 0.5375 -0.8433 0.0000 576 | vn -0.5375 -0.8433 0.0000 577 | vn -0.5375 0.8433 0.0000 578 | vn 0.5375 0.8433 0.0000 579 | usemtl MockRobot 580 | s off 581 | f 106/1/1 107/2/1 102/3/1 582 | f 118/4/2 119/5/2 112/6/2 583 | f 108/7/1 97/8/1 99/9/1 584 | f 97/8/1 98/10/1 99/9/1 585 | f 99/9/1 100/11/1 108/7/1 586 | f 100/11/1 101/12/1 108/7/1 587 | f 101/12/1 102/3/1 107/2/1 588 | f 108/7/1 101/12/1 107/2/1 589 | f 102/3/1 103/13/1 106/1/1 590 | f 103/13/1 104/14/1 105/15/1 591 | f 106/1/1 103/13/1 105/15/1 592 | f 120/16/2 109/17/2 110/18/2 593 | f 110/18/2 111/19/2 120/16/2 594 | f 111/19/2 112/6/2 119/5/2 595 | f 120/16/2 111/19/2 119/5/2 596 | f 112/6/2 113/20/2 117/21/2 597 | f 113/20/2 114/22/2 116/23/2 598 | f 114/22/2 115/24/2 116/23/2 599 | f 116/23/2 117/21/2 113/20/2 600 | f 117/21/2 118/4/2 112/6/2 601 | s 1 602 | f 52/25/3 27/26/4 26/27/5 603 | f 51/28/6 29/29/7 27/26/4 604 | f 54/30/8 31/31/9 29/32/7 605 | f 31/31/9 58/33/10 33/34/11 606 | f 33/34/11 60/35/12 35/36/13 607 | f 35/37/13 62/38/14 37/39/15 608 | f 37/39/15 64/40/16 39/41/17 609 | f 39/41/17 66/42/18 41/43/19 610 | f 66/44/18 43/45/20 41/46/19 611 | f 68/47/21 45/48/22 43/45/20 612 | f 70/49/23 47/50/24 45/48/22 613 | f 72/51/25 26/27/5 47/52/24 614 | f 48/53/24 49/54/26 71/55/27 615 | f 48/53/24 26/27/5 25/56/5 616 | f 46/57/22 71/58/27 69/59/28 617 | f 45/48/22 48/60/24 46/57/22 618 | f 44/61/20 69/59/28 67/62/29 619 | f 43/45/20 46/57/22 44/61/20 620 | f 42/63/19 67/62/29 65/64/30 621 | f 41/46/19 44/61/20 42/63/19 622 | f 40/65/17 65/66/30 63/67/31 623 | f 40/65/17 41/43/19 42/68/19 624 | f 38/69/15 63/67/31 61/70/32 625 | f 37/39/15 40/65/17 38/69/15 626 | f 36/71/13 61/70/32 59/72/33 627 | f 35/37/13 38/69/15 36/71/13 628 | f 57/73/34 36/74/13 59/75/33 629 | f 34/76/11 35/36/13 36/74/13 630 | f 32/77/9 57/73/34 55/78/35 631 | f 32/77/9 33/34/11 34/76/11 632 | f 30/79/7 55/78/35 53/80/36 633 | f 29/32/7 32/77/9 30/79/7 634 | f 28/81/4 53/82/36 50/83/37 635 | f 27/26/4 30/84/7 28/81/4 636 | f 25/56/5 50/83/37 49/54/26 637 | f 25/56/5 27/26/4 28/81/4 638 | f 74/85/38 49/86/26 50/87/37 639 | f 75/88/39 52/89/3 76/90/40 640 | f 77/91/41 50/87/37 53/92/36 641 | f 78/93/42 51/94/6 75/88/39 642 | f 79/95/43 53/92/36 55/96/35 643 | f 80/97/44 54/98/8 78/93/42 644 | f 79/95/43 57/99/34 81/100/45 645 | f 80/97/44 58/101/10 56/102/46 646 | f 81/100/45 59/103/33 83/104/47 647 | f 82/105/48 60/106/12 58/101/10 648 | f 83/104/47 61/107/32 85/108/49 649 | f 84/109/50 62/110/14 60/106/12 650 | f 87/111/51 61/107/32 63/112/31 651 | f 88/113/52 62/110/14 86/114/53 652 | f 89/115/54 63/112/31 65/116/30 653 | f 90/117/55 64/118/16 88/113/52 654 | f 91/119/56 65/116/30 67/120/29 655 | f 92/121/57 66/122/18 90/117/55 656 | f 91/119/56 69/123/28 93/124/58 657 | f 92/121/57 70/125/23 68/126/21 658 | f 93/124/58 71/127/27 95/128/59 659 | f 94/129/60 72/130/25 70/125/23 660 | f 95/128/59 49/86/26 73/131/61 661 | f 96/132/62 52/89/3 72/130/25 662 | f 157/133/63 73/131/61 74/85/38 663 | f 146/134/64 76/90/40 145/135/65 664 | f 159/136/66 74/85/38 77/91/41 665 | f 147/137/67 75/88/39 146/134/64 666 | f 160/138/68 77/91/41 79/95/43 667 | f 148/139/69 78/93/42 147/137/67 668 | f 160/138/68 81/100/45 161/140/70 669 | f 148/139/69 82/105/48 80/97/44 670 | f 161/140/70 83/104/47 162/141/71 671 | f 149/142/72 84/109/50 82/105/48 672 | f 162/141/71 85/108/49 163/143/73 673 | f 150/144/74 86/114/53 84/109/50 674 | f 164/145/75 85/108/49 87/111/51 675 | f 152/146/76 86/114/53 151/147/77 676 | f 165/148/78 87/111/51 89/115/54 677 | f 153/149/79 88/113/52 152/146/76 678 | f 166/150/80 89/115/54 91/119/56 679 | f 154/151/81 90/117/55 153/149/79 680 | f 166/150/80 93/124/58 167/152/82 681 | f 154/151/81 94/129/60 92/121/57 682 | f 167/152/82 95/128/59 168/153/83 683 | f 155/154/84 96/132/62 94/129/60 684 | f 168/153/83 73/131/61 158/155/85 685 | f 156/156/86 76/90/40 96/132/62 686 | f 121/157/87 4/158/88 2/159/89 687 | f 24/160/90 121/157/87 2/159/89 688 | f 22/161/91 123/162/92 24/163/90 689 | f 20/164/93 124/165/94 22/161/91 690 | f 126/166/95 20/164/93 18/167/96 691 | f 127/168/97 18/169/96 16/170/98 692 | f 128/171/99 16/170/98 14/172/100 693 | f 12/173/101 128/171/99 14/172/100 694 | f 10/174/102 129/175/103 12/176/101 695 | f 8/177/104 130/178/105 10/174/102 696 | f 132/179/106 8/177/104 6/180/107 697 | f 122/181/108 6/182/107 4/158/88 698 | f 3/183/109 134/184/110 1/185/111 699 | f 5/186/112 133/187/113 3/183/109 700 | f 7/188/114 135/189/115 5/190/112 701 | f 137/191/116 7/188/114 9/192/117 702 | f 138/193/118 9/192/117 11/194/119 703 | f 139/195/120 11/196/119 13/197/121 704 | f 15/198/122 139/195/120 13/197/121 705 | f 17/199/123 140/200/124 15/198/122 706 | f 19/201/125 141/202/126 17/203/123 707 | f 143/204/127 19/201/125 21/205/128 708 | f 144/206/129 21/205/128 23/207/130 709 | f 134/184/110 23/208/130 1/185/111 710 | f 97/8/1 121/209/87 98/10/1 711 | f 99/9/1 121/209/87 123/210/92 712 | f 100/11/1 123/210/92 124/211/94 713 | f 101/12/1 124/211/94 125/212/131 714 | f 101/12/1 126/213/95 102/3/1 715 | f 102/3/1 127/214/97 103/13/1 716 | f 103/13/1 128/215/99 104/14/1 717 | f 105/15/1 128/215/99 129/216/103 718 | f 106/1/1 129/216/103 130/217/105 719 | f 107/2/1 130/217/105 131/218/132 720 | f 107/2/1 132/219/106 108/7/1 721 | f 108/7/1 122/220/108 97/8/1 722 | f 110/18/2 134/221/110 133/222/113 723 | f 111/19/2 133/222/113 135/223/115 724 | f 112/6/2 135/223/115 136/224/133 725 | f 112/6/2 137/225/116 113/20/2 726 | f 113/20/2 138/226/118 114/22/2 727 | f 114/22/2 139/227/120 115/24/2 728 | f 116/23/2 139/227/120 140/228/124 729 | f 117/21/2 140/228/124 141/229/126 730 | f 118/4/2 141/229/126 142/230/134 731 | f 118/4/2 143/231/127 119/5/2 732 | f 119/5/2 144/232/129 120/16/2 733 | f 120/16/2 134/221/110 109/17/2 734 | f 24/233/90 145/135/65 156/156/86 735 | f 22/234/91 156/156/86 155/154/84 736 | f 20/235/93 155/154/84 154/151/81 737 | f 20/235/93 153/149/79 18/236/96 738 | f 18/236/96 152/146/76 16/237/98 739 | f 16/237/98 151/147/77 14/238/100 740 | f 12/239/101 151/147/77 150/144/74 741 | f 10/240/102 150/144/74 149/142/72 742 | f 8/241/104 149/142/72 148/139/69 743 | f 8/241/104 147/137/67 6/242/107 744 | f 6/242/107 146/134/64 4/243/88 745 | f 4/243/88 145/135/65 2/244/89 746 | f 23/245/130 158/155/85 1/246/111 747 | f 21/247/128 168/153/83 23/245/130 748 | f 19/248/125 167/152/82 21/247/128 749 | f 19/248/125 165/148/78 166/150/80 750 | f 17/249/123 164/145/75 165/148/78 751 | f 15/250/122 163/143/73 164/145/75 752 | f 11/251/119 163/143/73 13/252/121 753 | f 9/253/117 162/141/71 11/251/119 754 | f 7/254/114 161/140/70 9/253/117 755 | f 7/254/114 159/136/66 160/138/68 756 | f 5/255/112 157/133/63 159/136/66 757 | f 3/256/109 158/155/85 157/133/63 758 | f 52/25/3 51/28/6 27/26/4 759 | f 51/28/6 54/257/8 29/29/7 760 | f 54/30/8 56/258/46 31/31/9 761 | f 31/31/9 56/258/46 58/33/10 762 | f 33/34/11 58/33/10 60/35/12 763 | f 35/37/13 60/259/12 62/38/14 764 | f 37/39/15 62/38/14 64/40/16 765 | f 39/41/17 64/40/16 66/42/18 766 | f 66/44/18 68/47/21 43/45/20 767 | f 68/47/21 70/49/23 45/48/22 768 | f 70/49/23 72/260/25 47/50/24 769 | f 72/51/25 52/25/3 26/27/5 770 | f 48/53/24 25/56/5 49/54/26 771 | f 48/53/24 47/52/24 26/27/5 772 | f 46/57/22 48/60/24 71/58/27 773 | f 45/48/22 47/50/24 48/60/24 774 | f 44/61/20 46/57/22 69/59/28 775 | f 43/45/20 45/48/22 46/57/22 776 | f 42/63/19 44/61/20 67/62/29 777 | f 41/46/19 43/45/20 44/61/20 778 | f 40/65/17 42/68/19 65/66/30 779 | f 40/65/17 39/41/17 41/43/19 780 | f 38/69/15 40/65/17 63/67/31 781 | f 37/39/15 39/41/17 40/65/17 782 | f 36/71/13 38/69/15 61/70/32 783 | f 35/37/13 37/39/15 38/69/15 784 | f 57/73/34 34/76/11 36/74/13 785 | f 34/76/11 33/34/11 35/36/13 786 | f 32/77/9 34/76/11 57/73/34 787 | f 32/77/9 31/31/9 33/34/11 788 | f 30/79/7 32/77/9 55/78/35 789 | f 29/32/7 31/31/9 32/77/9 790 | f 28/81/4 30/84/7 53/82/36 791 | f 27/26/4 29/29/7 30/84/7 792 | f 25/56/5 28/81/4 50/83/37 793 | f 25/56/5 26/27/5 27/26/4 794 | f 74/85/38 73/131/61 49/86/26 795 | f 75/88/39 51/94/6 52/89/3 796 | f 77/91/41 74/85/38 50/87/37 797 | f 78/93/42 54/98/8 51/94/6 798 | f 79/95/43 77/91/41 53/92/36 799 | f 80/97/44 56/102/46 54/98/8 800 | f 79/95/43 55/96/35 57/99/34 801 | f 80/97/44 82/105/48 58/101/10 802 | f 81/100/45 57/99/34 59/103/33 803 | f 82/105/48 84/109/50 60/106/12 804 | f 83/104/47 59/103/33 61/107/32 805 | f 84/109/50 86/114/53 62/110/14 806 | f 87/111/51 85/108/49 61/107/32 807 | f 88/113/52 64/118/16 62/110/14 808 | f 89/115/54 87/111/51 63/112/31 809 | f 90/117/55 66/122/18 64/118/16 810 | f 91/119/56 89/115/54 65/116/30 811 | f 92/121/57 68/126/21 66/122/18 812 | f 91/119/56 67/120/29 69/123/28 813 | f 92/121/57 94/129/60 70/125/23 814 | f 93/124/58 69/123/28 71/127/27 815 | f 94/129/60 96/132/62 72/130/25 816 | f 95/128/59 71/127/27 49/86/26 817 | f 96/132/62 76/90/40 52/89/3 818 | f 157/133/63 158/155/85 73/131/61 819 | f 146/134/64 75/88/39 76/90/40 820 | f 159/136/66 157/133/63 74/85/38 821 | f 147/137/67 78/93/42 75/88/39 822 | f 160/138/68 159/136/66 77/91/41 823 | f 148/139/69 80/97/44 78/93/42 824 | f 160/138/68 79/95/43 81/100/45 825 | f 148/139/69 149/142/72 82/105/48 826 | f 161/140/70 81/100/45 83/104/47 827 | f 149/142/72 150/144/74 84/109/50 828 | f 162/141/71 83/104/47 85/108/49 829 | f 150/144/74 151/147/77 86/114/53 830 | f 164/145/75 163/143/73 85/108/49 831 | f 152/146/76 88/113/52 86/114/53 832 | f 165/148/78 164/145/75 87/111/51 833 | f 153/149/79 90/117/55 88/113/52 834 | f 166/150/80 165/148/78 89/115/54 835 | f 154/151/81 92/121/57 90/117/55 836 | f 166/150/80 91/119/56 93/124/58 837 | f 154/151/81 155/154/84 94/129/60 838 | f 167/152/82 93/124/58 95/128/59 839 | f 155/154/84 156/156/86 96/132/62 840 | f 168/153/83 95/128/59 73/131/61 841 | f 156/156/86 145/135/65 76/90/40 842 | f 121/157/87 122/181/108 4/158/88 843 | f 24/160/90 123/261/92 121/157/87 844 | f 22/161/91 124/165/94 123/162/92 845 | f 20/164/93 125/262/131 124/165/94 846 | f 126/166/95 125/262/131 20/164/93 847 | f 127/168/97 126/263/95 18/169/96 848 | f 128/171/99 127/168/97 16/170/98 849 | f 12/173/101 129/264/103 128/171/99 850 | f 10/174/102 130/178/105 129/175/103 851 | f 8/177/104 131/265/132 130/178/105 852 | f 132/179/106 131/265/132 8/177/104 853 | f 122/181/108 132/266/106 6/182/107 854 | f 3/183/109 133/187/113 134/184/110 855 | f 5/186/112 135/267/115 133/187/113 856 | f 7/188/114 136/268/133 135/189/115 857 | f 137/191/116 136/268/133 7/188/114 858 | f 138/193/118 137/191/116 9/192/117 859 | f 139/195/120 138/269/118 11/196/119 860 | f 15/198/122 140/200/124 139/195/120 861 | f 17/199/123 141/270/126 140/200/124 862 | f 19/201/125 142/271/134 141/202/126 863 | f 143/204/127 142/271/134 19/201/125 864 | f 144/206/129 143/204/127 21/205/128 865 | f 134/184/110 144/272/129 23/208/130 866 | f 97/8/1 122/220/108 121/209/87 867 | f 99/9/1 98/10/1 121/209/87 868 | f 100/11/1 99/9/1 123/210/92 869 | f 101/12/1 100/11/1 124/211/94 870 | f 101/12/1 125/212/131 126/213/95 871 | f 102/3/1 126/213/95 127/214/97 872 | f 103/13/1 127/214/97 128/215/99 873 | f 105/15/1 104/14/1 128/215/99 874 | f 106/1/1 105/15/1 129/216/103 875 | f 107/2/1 106/1/1 130/217/105 876 | f 107/2/1 131/218/132 132/219/106 877 | f 108/7/1 132/219/106 122/220/108 878 | f 110/18/2 109/17/2 134/221/110 879 | f 111/19/2 110/18/2 133/222/113 880 | f 112/6/2 111/19/2 135/223/115 881 | f 112/6/2 136/224/133 137/225/116 882 | f 113/20/2 137/225/116 138/226/118 883 | f 114/22/2 138/226/118 139/227/120 884 | f 116/23/2 115/24/2 139/227/120 885 | f 117/21/2 116/23/2 140/228/124 886 | f 118/4/2 117/21/2 141/229/126 887 | f 118/4/2 142/230/134 143/231/127 888 | f 119/5/2 143/231/127 144/232/129 889 | f 120/16/2 144/232/129 134/221/110 890 | f 24/233/90 2/244/89 145/135/65 891 | f 22/234/91 24/233/90 156/156/86 892 | f 20/235/93 22/234/91 155/154/84 893 | f 20/235/93 154/151/81 153/149/79 894 | f 18/236/96 153/149/79 152/146/76 895 | f 16/237/98 152/146/76 151/147/77 896 | f 12/239/101 14/238/100 151/147/77 897 | f 10/240/102 12/239/101 150/144/74 898 | f 8/241/104 10/240/102 149/142/72 899 | f 8/241/104 148/139/69 147/137/67 900 | f 6/242/107 147/137/67 146/134/64 901 | f 4/243/88 146/134/64 145/135/65 902 | f 23/245/130 168/153/83 158/155/85 903 | f 21/247/128 167/152/82 168/153/83 904 | f 19/248/125 166/150/80 167/152/82 905 | f 19/248/125 17/249/123 165/148/78 906 | f 17/249/123 15/250/122 164/145/75 907 | f 15/250/122 13/252/121 163/143/73 908 | f 11/251/119 162/141/71 163/143/73 909 | f 9/253/117 161/140/70 162/141/71 910 | f 7/254/114 160/138/68 161/140/70 911 | f 7/254/114 5/255/112 159/136/66 912 | f 5/255/112 3/256/109 157/133/63 913 | f 3/256/109 1/246/111 158/155/85 914 | -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/meshes/MockRobot.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'MockRobotFinal.blend' 2 | # Material Count: 1 3 | 4 | newmtl MockRobot 5 | Ns 96.078431 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.197778 0.197778 0.197778 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd MockRobot_Diffuse.png 14 | -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/meshes/MockRobot_Diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/MockRobot/meshes/MockRobot_Diffuse.png -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/meshes/SmallWheel.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'SmallWheel.blend' 2 | # Material Count: 1 3 | 4 | newmtl MockRobot 5 | Ns 96.078431 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.197778 0.197778 0.197778 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd MockRobot_Diffuse.png 14 | -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/meshes/SmallWheel.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.79 (sub 0) OBJ File: 'SmallWheel.blend' 2 | # www.blender.org 3 | mtllib SmallWheel.mtl 4 | o SmallWhRFront_Cube.000 5 | v -0.000000 -0.008780 0.023380 6 | v -0.000000 0.008780 0.023380 7 | v 0.014852 -0.008780 0.017228 8 | v 0.014852 0.008780 0.017228 9 | v 0.021988 -0.008780 0.000000 10 | v 0.021988 0.008780 0.000000 11 | v 0.014852 -0.008780 -0.017228 12 | v 0.014852 0.008780 -0.017228 13 | v -0.000000 -0.008780 -0.023380 14 | v -0.000000 0.008780 -0.023380 15 | v -0.014852 -0.008780 -0.017228 16 | v -0.014852 0.008780 -0.017228 17 | v -0.021988 -0.008780 0.000000 18 | v -0.021988 0.008780 0.000000 19 | v -0.014852 -0.008780 0.017228 20 | v -0.014852 0.008780 0.017228 21 | v -0.000000 -0.005853 0.027437 22 | v -0.000000 0.005853 0.027437 23 | v 0.019401 0.005853 0.019401 24 | v 0.019401 -0.005853 0.019401 25 | v 0.027437 0.005853 0.000000 26 | v 0.027437 -0.005853 0.000000 27 | v 0.019401 0.005853 -0.019401 28 | v 0.019401 -0.005853 -0.019401 29 | v -0.000000 0.005853 -0.027437 30 | v -0.000000 -0.005853 -0.027437 31 | v -0.019401 0.005853 -0.019401 32 | v -0.019401 -0.005853 -0.019401 33 | v -0.027437 0.005853 0.000000 34 | v -0.027437 -0.005853 0.000000 35 | v -0.019401 0.005853 0.019401 36 | v -0.019401 -0.005853 0.019401 37 | v -0.000000 -0.008780 0.027437 38 | v 0.019401 -0.008780 0.019401 39 | v 0.019401 0.008780 0.019401 40 | v -0.000000 0.008780 0.027437 41 | v 0.027437 -0.008780 0.000000 42 | v 0.027437 0.008780 0.000000 43 | v 0.019401 -0.008780 -0.019401 44 | v 0.019401 0.008780 -0.019401 45 | v -0.000000 -0.008780 -0.027437 46 | v -0.000000 0.008780 -0.027437 47 | v -0.019401 -0.008780 -0.019401 48 | v -0.019401 0.008780 -0.019401 49 | v -0.027437 -0.008780 0.000000 50 | v -0.027437 0.008780 0.000000 51 | v -0.019401 -0.008780 0.019401 52 | v -0.019401 0.008780 0.019401 53 | v 0.010466 0.001476 0.013597 54 | v 0.016098 0.001476 0.000000 55 | v 0.010466 0.001476 -0.013597 56 | v -0.000000 0.001476 -0.017932 57 | v -0.010466 0.001476 -0.013597 58 | v -0.016098 0.001476 0.000000 59 | v -0.010466 0.001476 0.013597 60 | v -0.000000 0.001476 0.017932 61 | v -0.000000 -0.001476 0.017932 62 | v -0.010466 -0.001476 0.013597 63 | v -0.016098 -0.001476 0.000000 64 | v -0.010466 -0.001476 -0.013597 65 | v -0.000000 -0.001476 -0.017932 66 | v 0.010466 -0.001476 -0.013597 67 | v 0.016098 -0.001476 0.000000 68 | v 0.010466 -0.001476 0.013597 69 | v 0.018655 0.001476 0.000000 70 | v 0.012601 0.001476 0.014617 71 | v 0.012601 0.001476 -0.014617 72 | v -0.000000 0.001476 -0.019836 73 | v -0.012601 0.001476 -0.014617 74 | v -0.018655 0.001476 0.000000 75 | v -0.012601 0.001476 0.014617 76 | v -0.000000 0.001476 0.019836 77 | v -0.012601 -0.001476 0.014617 78 | v -0.000000 -0.001476 0.019836 79 | v -0.018655 -0.001476 0.000000 80 | v -0.012601 -0.001476 -0.014617 81 | v -0.000000 -0.001476 -0.019836 82 | v 0.012601 -0.001476 -0.014617 83 | v 0.018655 -0.001476 0.000000 84 | v 0.012601 -0.001476 0.014617 85 | v -0.013726 -0.005128 0.015923 86 | v -0.000000 -0.005128 0.021608 87 | v -0.020322 -0.005128 0.000000 88 | v -0.013726 -0.005128 -0.015923 89 | v -0.000000 -0.005128 -0.021608 90 | v 0.013726 -0.005128 -0.015923 91 | v 0.020322 -0.005128 0.000000 92 | v 0.013726 -0.005128 0.015923 93 | v -0.000000 -0.008780 0.025409 94 | v 0.017126 -0.008780 0.018315 95 | v 0.024712 -0.008780 0.000000 96 | v 0.017126 -0.008780 -0.018315 97 | v -0.000000 -0.008780 -0.025409 98 | v -0.017126 -0.008780 -0.018315 99 | v -0.024712 -0.008780 0.000000 100 | v -0.017126 -0.008780 0.018315 101 | v 0.017126 0.008780 0.018315 102 | v -0.000000 0.008780 0.025409 103 | v 0.024712 0.008780 0.000000 104 | v 0.017126 0.008780 -0.018315 105 | v -0.000000 0.008780 -0.025409 106 | v -0.017126 0.008780 -0.018315 107 | v -0.024713 0.008780 0.000000 108 | v -0.017126 0.008780 0.018315 109 | v 0.020322 0.005128 0.000000 110 | v 0.013726 0.005128 0.015923 111 | v 0.013726 0.005128 -0.015923 112 | v -0.000000 0.005128 -0.021608 113 | v -0.013726 0.005128 -0.015923 114 | v -0.020322 0.005128 0.000000 115 | v -0.013726 0.005128 0.015923 116 | v -0.000000 0.005128 0.021608 117 | vt 0.123681 0.871919 118 | vt 0.123681 0.882523 119 | vt 0.102260 0.880796 120 | vt 0.193500 0.890828 121 | vt 0.200995 0.898326 122 | vt 0.179574 0.896599 123 | vt 0.117912 0.888294 124 | vt 0.109755 0.888294 125 | vt 0.102260 0.870192 126 | vt 0.116186 0.864421 127 | vt 0.108029 0.864421 128 | vt 0.200995 0.908930 129 | vt 0.195226 0.914701 130 | vt 0.187068 0.914701 131 | vt 0.179574 0.907203 132 | vt 0.185342 0.890828 133 | vt 0.130827 0.934635 134 | vt 0.128720 0.919518 135 | vt 0.130827 0.919518 136 | vt 0.130827 0.945749 137 | vt 0.128720 0.934635 138 | vt 0.094696 0.978706 139 | vt 0.096804 0.963582 140 | vt 0.096804 0.978706 141 | vt 0.094696 0.952612 142 | vt 0.094696 0.963582 143 | vt 0.215473 0.958420 144 | vt 0.217580 0.973538 145 | vt 0.215473 0.973538 146 | vt 0.217580 0.947306 147 | vt 0.217580 0.958420 148 | vt 0.107341 0.963581 149 | vt 0.109449 0.978706 150 | vt 0.107341 0.978706 151 | vt 0.107341 0.952612 152 | vt 0.109449 0.963581 153 | vt 0.096804 0.952612 154 | vt 0.098911 0.963581 155 | vt 0.096804 0.963581 156 | vt 0.096804 0.978706 157 | vt 0.098911 0.978706 158 | vt 0.204935 0.947306 159 | vt 0.207043 0.958420 160 | vt 0.204935 0.958420 161 | vt 0.215473 0.947306 162 | vt 0.204935 0.973538 163 | vt 0.207043 0.973538 164 | vt 0.084158 0.963582 165 | vt 0.086266 0.952612 166 | vt 0.086266 0.963582 167 | vt 0.086266 0.978706 168 | vt 0.084158 0.978706 169 | vt 0.141365 0.945749 170 | vt 0.139257 0.934635 171 | vt 0.141365 0.934635 172 | vt 0.139257 0.945749 173 | vt 0.139257 0.919518 174 | vt 0.141365 0.919518 175 | vt 0.307138 0.742434 176 | vt 0.293230 0.743784 177 | vt 0.293789 0.742434 178 | vt 0.049000 0.928000 179 | vt 0.047650 0.941914 180 | vt 0.047650 0.926785 181 | vt 0.317232 0.732335 182 | vt 0.308352 0.743784 183 | vt 0.059094 0.917901 184 | vt 0.058343 0.916087 185 | vt 0.319045 0.717957 186 | vt 0.319045 0.733086 187 | vt 0.073466 0.916087 188 | vt 0.073370 0.917901 189 | vt 0.317232 0.718053 190 | vt 0.308352 0.707259 191 | vt 0.084158 0.926785 192 | vt 0.082809 0.927345 193 | vt 0.294444 0.708609 194 | vt 0.307793 0.708609 195 | vt 0.082809 0.940700 196 | vt 0.084158 0.941914 197 | vt 0.284349 0.718708 198 | vt 0.293230 0.707259 199 | vt 0.072715 0.950799 200 | vt 0.073465 0.952612 201 | vt 0.282537 0.733086 202 | vt 0.282537 0.717957 203 | vt 0.058343 0.952612 204 | vt 0.058439 0.950799 205 | vt 0.284349 0.732990 206 | vt 0.049000 0.941355 207 | vt 0.214951 0.993890 208 | vt 0.212321 0.984426 209 | vt 0.214951 0.984768 210 | vt 0.213992 0.769650 211 | vt 0.211362 0.781532 212 | vt 0.211362 0.769120 213 | vt 0.213992 0.762525 214 | vt 0.211362 0.761359 215 | vt 0.321675 0.809774 216 | vt 0.319045 0.799614 217 | vt 0.321675 0.799079 218 | vt 0.321675 0.818896 219 | vt 0.319045 0.809432 220 | vt 0.154010 0.968413 221 | vt 0.156640 0.956530 222 | vt 0.156640 0.968943 223 | vt 0.154010 0.975537 224 | vt 0.156640 0.976704 225 | vt 0.212321 0.974608 226 | vt 0.214951 0.974073 227 | vt 0.216622 0.773093 228 | vt 0.219252 0.779687 229 | vt 0.216622 0.780854 230 | vt 0.216622 0.760680 231 | vt 0.219252 0.772563 232 | vt 0.123460 0.975126 233 | vt 0.126090 0.965662 234 | vt 0.126090 0.974784 235 | vt 0.123460 0.984944 236 | vt 0.126090 0.985479 237 | vt 0.206103 0.769120 238 | vt 0.203473 0.762525 239 | vt 0.206103 0.761359 240 | vt 0.206103 0.781532 241 | vt 0.203473 0.769650 242 | vt 0.192176 0.979336 243 | vt 0.194806 0.969872 244 | vt 0.194806 0.978993 245 | vt 0.192176 0.989154 246 | vt 0.194806 0.989689 247 | vt 0.108615 0.889561 248 | vt 0.100559 0.881501 249 | vt 0.100559 0.870102 250 | vt 0.107504 0.863154 251 | vt 0.117326 0.863154 252 | vt 0.125382 0.871214 253 | vt 0.125382 0.882613 254 | vt 0.118437 0.889561 255 | vt 0.195751 0.915968 256 | vt 0.185929 0.915968 257 | vt 0.177873 0.907908 258 | vt 0.177873 0.896509 259 | vt 0.184818 0.889561 260 | vt 0.194640 0.889561 261 | vt 0.202696 0.897621 262 | vt 0.202696 0.909020 263 | vt 0.197436 0.978651 264 | vt 0.197436 0.990224 265 | vt 0.197436 0.968782 266 | vt 0.208732 0.782020 267 | vt 0.208732 0.768589 268 | vt 0.208732 0.760192 269 | vt 0.128720 0.974442 270 | vt 0.128720 0.986014 271 | vt 0.128720 0.964572 272 | vt 0.213992 0.760192 273 | vt 0.213992 0.773623 274 | vt 0.213992 0.782020 275 | vt 0.286162 0.732894 276 | vt 0.286162 0.719459 277 | vt 0.295657 0.709959 278 | vt 0.307234 0.709959 279 | vt 0.315420 0.718149 280 | vt 0.315420 0.731584 281 | vt 0.305924 0.741084 282 | vt 0.294348 0.741084 283 | vt 0.058535 0.948986 284 | vt 0.050349 0.940796 285 | vt 0.071964 0.948986 286 | vt 0.081460 0.939486 287 | vt 0.073274 0.919714 288 | vt 0.081460 0.927904 289 | vt 0.059845 0.919714 290 | vt 0.050349 0.929214 291 | vt 0.217580 0.985110 292 | vt 0.217580 0.973538 293 | vt 0.159270 0.969473 294 | vt 0.159270 0.977870 295 | vt 0.159270 0.956043 296 | vt 0.324305 0.819986 297 | vt 0.324305 0.810116 298 | vt 0.324305 0.798544 299 | vt 0.208732 0.768589 300 | vt 0.208732 0.760192 301 | vt 0.208732 0.782020 302 | vt 0.217580 0.994980 303 | vt 0.128720 0.945749 304 | vt 0.096804 0.952612 305 | vt 0.109449 0.952612 306 | vt 0.098911 0.952612 307 | vt 0.207043 0.947306 308 | vt 0.084158 0.952612 309 | vt 0.212321 0.992800 310 | vt 0.213992 0.781045 311 | vt 0.319045 0.817806 312 | vt 0.154010 0.957018 313 | vt 0.219252 0.761168 314 | vt 0.123460 0.966752 315 | vt 0.203473 0.781045 316 | vt 0.192176 0.970962 317 | vn -0.0000 1.0000 0.0000 318 | vn 0.0000 -1.0000 -0.0000 319 | vn 0.7071 0.0000 0.7071 320 | vn 0.0000 0.6302 0.7764 321 | vn 0.0000 0.0000 1.0000 322 | vn 1.0000 0.0000 0.0000 323 | vn 0.5490 0.6302 0.5490 324 | vn 0.5490 0.6302 -0.5490 325 | vn 0.7764 0.6302 0.0000 326 | vn 0.0000 0.0000 -1.0000 327 | vn 0.7071 0.0000 -0.7071 328 | vn -0.7071 0.0000 -0.7071 329 | vn 0.0000 0.6302 -0.7764 330 | vn -0.7764 0.6302 0.0000 331 | vn -0.5490 0.6302 -0.5490 332 | vn -0.7071 0.0000 0.7071 333 | vn -1.0000 0.0000 0.0000 334 | vn -0.5490 0.6302 0.5490 335 | vn 0.0000 -0.6302 0.7764 336 | vn -0.5490 -0.6302 0.5490 337 | vn -0.7764 -0.6302 0.0000 338 | vn -0.5490 -0.6302 -0.5490 339 | vn 0.0000 -0.6302 -0.7764 340 | vn 0.5490 -0.6302 -0.5490 341 | vn 0.7764 -0.6302 0.0000 342 | vn 0.5490 -0.6302 0.5490 343 | vn -0.9098 0.4151 0.0000 344 | vn -0.4409 0.7862 -0.4330 345 | vn -0.6411 0.4259 -0.6385 346 | vn -0.4409 0.7862 0.4330 347 | vn -0.6411 0.4259 0.6385 348 | vn 0.0000 0.7901 0.6130 349 | vn 0.0000 0.4365 0.8997 350 | vn 0.6410 0.4259 0.6385 351 | vn 0.9098 0.4151 0.0000 352 | vn 0.4409 0.7862 0.4330 353 | vn 0.4409 0.7862 -0.4330 354 | vn 0.6410 0.4259 -0.6385 355 | vn 0.0000 0.7901 -0.6130 356 | vn 0.0000 0.4365 -0.8997 357 | vn 0.6410 -0.4259 -0.6385 358 | vn 0.0000 -0.7901 -0.6130 359 | vn 0.0000 -0.4365 -0.8997 360 | vn 0.9098 -0.4151 0.0000 361 | vn 0.4409 -0.7862 -0.4330 362 | vn 0.4409 -0.7862 0.4330 363 | vn 0.6410 -0.4259 0.6385 364 | vn 0.0000 -0.7901 0.6130 365 | vn 0.0000 -0.4365 0.8997 366 | vn -0.6411 -0.4259 0.6385 367 | vn -0.9098 -0.4151 0.0000 368 | vn -0.4409 -0.7862 0.4330 369 | vn -0.4409 -0.7862 -0.4330 370 | vn -0.6411 -0.4259 -0.6385 371 | vn -0.6228 0.7823 0.0000 372 | vn 0.6229 0.7823 0.0000 373 | vn 0.6229 -0.7823 0.0000 374 | vn -0.6228 -0.7823 0.0000 375 | vn -0.2998 -0.9043 -0.3039 376 | vn 0.0000 -0.9063 -0.4225 377 | vn -0.4315 -0.9021 0.0000 378 | vn -0.2998 -0.9043 0.3039 379 | vn 0.0000 -0.9063 0.4225 380 | vn 0.2998 -0.9043 0.3039 381 | vn 0.4315 -0.9021 0.0000 382 | vn 0.2998 -0.9043 -0.3039 383 | vn 0.2998 0.9043 -0.3039 384 | vn 0.0000 0.9063 -0.4225 385 | vn 0.4315 0.9021 0.0000 386 | vn 0.2998 0.9043 0.3039 387 | vn -0.2998 0.9043 0.3039 388 | vn 0.0000 0.9063 0.4225 389 | vn -0.4315 0.9021 0.0000 390 | vn -0.2998 0.9043 -0.3039 391 | usemtl MockRobot 392 | s off 393 | f 54/1/1 55/2/1 50/3/1 394 | f 62/4/2 63/5/2 60/6/2 395 | f 56/7/1 49/8/1 55/2/1 396 | f 49/8/1 50/3/1 55/2/1 397 | f 50/3/1 51/9/1 53/10/1 398 | f 51/9/1 52/11/1 53/10/1 399 | f 53/10/1 54/1/1 50/3/1 400 | f 64/12/2 57/13/2 58/14/2 401 | f 58/14/2 59/15/2 64/12/2 402 | f 59/15/2 60/6/2 63/5/2 403 | f 64/12/2 59/15/2 63/5/2 404 | f 60/6/2 61/16/2 62/4/2 405 | s 1 406 | f 19/17/3 36/18/4 18/19/5 407 | f 21/20/6 35/21/7 19/17/3 408 | f 21/22/6 40/23/8 38/24/9 409 | f 25/25/10 40/23/8 23/26/11 410 | f 27/27/12 42/28/13 25/29/10 411 | f 27/27/12 46/30/14 44/31/15 412 | f 31/32/16 46/33/14 29/34/17 413 | f 18/35/5 48/36/18 31/32/16 414 | f 33/37/19 32/38/16 47/39/20 415 | f 32/38/16 18/35/5 31/32/16 416 | f 45/40/21 32/38/16 30/41/17 417 | f 32/38/16 29/34/17 30/41/17 418 | f 45/42/21 28/43/12 43/44/22 419 | f 28/43/12 29/45/17 27/27/12 420 | f 41/46/23 28/43/12 26/47/10 421 | f 28/43/12 25/29/10 26/47/10 422 | f 39/48/24 26/49/10 24/50/11 423 | f 24/50/11 25/25/10 23/26/11 424 | f 39/48/24 22/51/6 37/52/25 425 | f 22/51/6 23/26/11 21/22/6 426 | f 37/53/25 20/54/3 34/55/26 427 | f 22/56/6 19/17/3 20/54/3 428 | f 34/55/26 17/57/5 33/58/19 429 | f 20/54/3 18/19/5 17/57/5 430 | f 90/59/2 33/60/19 89/61/2 431 | f 97/62/1 36/63/4 35/64/7 432 | f 91/65/2 34/66/26 90/59/2 433 | f 99/67/1 35/64/7 38/68/9 434 | f 91/65/2 39/69/24 37/70/25 435 | f 99/67/1 40/71/8 100/72/1 436 | f 92/73/2 41/74/23 39/69/24 437 | f 100/72/1 42/75/13 101/76/1 438 | f 94/77/2 41/74/23 93/78/2 439 | f 102/79/1 42/75/13 44/80/15 440 | f 95/81/2 43/82/22 94/77/2 441 | f 103/83/1 44/80/15 46/84/14 442 | f 95/81/2 47/85/20 45/86/21 443 | f 103/83/1 48/87/18 104/88/1 444 | f 96/89/2 33/60/19 47/85/20 445 | f 104/88/1 36/63/4 98/90/1 446 | f 105/91/27 66/92/28 106/93/29 447 | f 67/94/30 105/95/27 107/96/31 448 | f 68/97/32 107/96/31 108/98/33 449 | f 109/99/34 68/100/32 108/101/33 450 | f 110/102/35 69/103/36 109/99/34 451 | f 71/104/37 110/105/35 111/106/38 452 | f 72/107/39 111/106/38 112/108/40 453 | f 106/93/29 72/109/39 112/110/40 454 | f 81/111/41 74/112/42 82/113/43 455 | f 83/114/44 73/115/45 81/111/41 456 | f 76/116/46 83/117/44 84/118/47 457 | f 77/119/48 84/118/47 85/120/49 458 | f 86/121/50 77/122/48 85/123/49 459 | f 87/124/51 78/125/52 86/121/50 460 | f 80/126/53 87/127/51 88/128/54 461 | f 74/129/42 88/128/54 82/130/43 462 | f 50/3/1 66/131/28 65/132/55 463 | f 50/3/1 67/133/30 51/9/1 464 | f 51/9/1 68/134/32 52/11/1 465 | f 53/10/1 68/134/32 69/135/36 466 | f 54/1/1 69/135/36 70/136/56 467 | f 54/1/1 71/137/37 55/2/1 468 | f 55/2/1 72/138/39 56/7/1 469 | f 49/8/1 72/138/39 66/131/28 470 | f 58/14/2 74/139/42 73/140/45 471 | f 59/15/2 73/140/45 75/141/57 472 | f 59/15/2 76/142/46 60/6/2 473 | f 60/6/2 77/143/48 61/16/2 474 | f 62/4/2 77/143/48 78/144/52 475 | f 63/5/2 78/144/52 79/145/58 476 | f 63/5/2 80/146/53 64/12/2 477 | f 64/12/2 74/139/42 57/13/2 478 | f 82/130/43 3/147/59 1/148/60 479 | f 88/128/54 5/149/61 3/147/59 480 | f 5/150/61 86/121/50 7/151/62 481 | f 7/151/62 85/123/49 9/152/63 482 | f 85/120/49 11/153/64 9/154/63 483 | f 84/118/47 13/155/65 11/153/64 484 | f 13/156/65 81/111/41 15/157/66 485 | f 15/157/66 82/113/43 1/158/60 486 | f 15/159/66 89/61/2 96/89/2 487 | f 13/160/65 96/89/2 95/81/2 488 | f 13/160/65 94/77/2 11/161/64 489 | f 11/161/64 93/78/2 9/162/63 490 | f 7/163/62 93/78/2 92/73/2 491 | f 5/164/61 92/73/2 91/65/2 492 | f 5/164/61 90/59/2 3/165/59 493 | f 3/165/59 89/61/2 1/166/60 494 | f 16/167/67 98/90/1 2/168/68 495 | f 14/169/69 104/88/1 16/167/67 496 | f 14/169/69 102/79/1 103/83/1 497 | f 12/170/70 101/76/1 102/79/1 498 | f 8/171/71 101/76/1 10/172/72 499 | f 6/173/73 100/72/1 8/171/71 500 | f 6/173/73 97/62/1 99/67/1 501 | f 4/174/74 98/90/1 97/62/1 502 | f 4/175/74 112/110/40 2/176/68 503 | f 112/108/40 16/177/67 2/178/68 504 | f 111/106/38 14/179/69 16/177/67 505 | f 14/180/69 109/99/34 12/181/70 506 | f 12/181/70 108/101/33 10/182/72 507 | f 108/98/33 8/183/71 10/184/72 508 | f 107/96/31 6/185/73 8/183/71 509 | f 6/186/73 106/93/29 4/175/74 510 | f 19/17/3 35/21/7 36/18/4 511 | f 21/20/6 38/187/9 35/21/7 512 | f 21/22/6 23/26/11 40/23/8 513 | f 25/25/10 42/188/13 40/23/8 514 | f 27/27/12 44/31/15 42/28/13 515 | f 27/27/12 29/45/17 46/30/14 516 | f 31/32/16 48/36/18 46/33/14 517 | f 18/35/5 36/189/4 48/36/18 518 | f 33/37/19 17/190/5 32/38/16 519 | f 32/38/16 17/190/5 18/35/5 520 | f 45/40/21 47/39/20 32/38/16 521 | f 32/38/16 31/32/16 29/34/17 522 | f 45/42/21 30/191/17 28/43/12 523 | f 28/43/12 30/191/17 29/45/17 524 | f 41/46/23 43/44/22 28/43/12 525 | f 28/43/12 27/27/12 25/29/10 526 | f 39/48/24 41/192/23 26/49/10 527 | f 24/50/11 26/49/10 25/25/10 528 | f 39/48/24 24/50/11 22/51/6 529 | f 22/51/6 24/50/11 23/26/11 530 | f 37/53/25 22/56/6 20/54/3 531 | f 22/56/6 21/20/6 19/17/3 532 | f 34/55/26 20/54/3 17/57/5 533 | f 20/54/3 19/17/3 18/19/5 534 | f 90/59/2 34/66/26 33/60/19 535 | f 97/62/1 98/90/1 36/63/4 536 | f 91/65/2 37/70/25 34/66/26 537 | f 99/67/1 97/62/1 35/64/7 538 | f 91/65/2 92/73/2 39/69/24 539 | f 99/67/1 38/68/9 40/71/8 540 | f 92/73/2 93/78/2 41/74/23 541 | f 100/72/1 40/71/8 42/75/13 542 | f 94/77/2 43/82/22 41/74/23 543 | f 102/79/1 101/76/1 42/75/13 544 | f 95/81/2 45/86/21 43/82/22 545 | f 103/83/1 102/79/1 44/80/15 546 | f 95/81/2 96/89/2 47/85/20 547 | f 103/83/1 46/84/14 48/87/18 548 | f 96/89/2 89/61/2 33/60/19 549 | f 104/88/1 48/87/18 36/63/4 550 | f 105/91/27 65/193/55 66/92/28 551 | f 67/94/30 65/194/55 105/95/27 552 | f 68/97/32 67/94/30 107/96/31 553 | f 109/99/34 69/103/36 68/100/32 554 | f 110/102/35 70/195/56 69/103/36 555 | f 71/104/37 70/196/56 110/105/35 556 | f 72/107/39 71/104/37 111/106/38 557 | f 106/93/29 66/92/28 72/109/39 558 | f 81/111/41 73/115/45 74/112/42 559 | f 83/114/44 75/197/57 73/115/45 560 | f 76/116/46 75/198/57 83/117/44 561 | f 77/119/48 76/116/46 84/118/47 562 | f 86/121/50 78/125/52 77/122/48 563 | f 87/124/51 79/199/58 78/125/52 564 | f 80/126/53 79/200/58 87/127/51 565 | f 74/129/42 80/126/53 88/128/54 566 | f 50/3/1 49/8/1 66/131/28 567 | f 50/3/1 65/132/55 67/133/30 568 | f 51/9/1 67/133/30 68/134/32 569 | f 53/10/1 52/11/1 68/134/32 570 | f 54/1/1 53/10/1 69/135/36 571 | f 54/1/1 70/136/56 71/137/37 572 | f 55/2/1 71/137/37 72/138/39 573 | f 49/8/1 56/7/1 72/138/39 574 | f 58/14/2 57/13/2 74/139/42 575 | f 59/15/2 58/14/2 73/140/45 576 | f 59/15/2 75/141/57 76/142/46 577 | f 60/6/2 76/142/46 77/143/48 578 | f 62/4/2 61/16/2 77/143/48 579 | f 63/5/2 62/4/2 78/144/52 580 | f 63/5/2 79/145/58 80/146/53 581 | f 64/12/2 80/146/53 74/139/42 582 | f 82/130/43 88/128/54 3/147/59 583 | f 88/128/54 87/127/51 5/149/61 584 | f 5/150/61 87/124/51 86/121/50 585 | f 7/151/62 86/121/50 85/123/49 586 | f 85/120/49 84/118/47 11/153/64 587 | f 84/118/47 83/117/44 13/155/65 588 | f 13/156/65 83/114/44 81/111/41 589 | f 15/157/66 81/111/41 82/113/43 590 | f 15/159/66 1/166/60 89/61/2 591 | f 13/160/65 15/159/66 96/89/2 592 | f 13/160/65 95/81/2 94/77/2 593 | f 11/161/64 94/77/2 93/78/2 594 | f 7/163/62 9/162/63 93/78/2 595 | f 5/164/61 7/163/62 92/73/2 596 | f 5/164/61 91/65/2 90/59/2 597 | f 3/165/59 90/59/2 89/61/2 598 | f 16/167/67 104/88/1 98/90/1 599 | f 14/169/69 103/83/1 104/88/1 600 | f 14/169/69 12/170/70 102/79/1 601 | f 12/170/70 10/172/72 101/76/1 602 | f 8/171/71 100/72/1 101/76/1 603 | f 6/173/73 99/67/1 100/72/1 604 | f 6/173/73 4/174/74 97/62/1 605 | f 4/174/74 2/168/68 98/90/1 606 | f 4/175/74 106/93/29 112/110/40 607 | f 112/108/40 111/106/38 16/177/67 608 | f 111/106/38 110/105/35 14/179/69 609 | f 14/180/69 110/102/35 109/99/34 610 | f 12/181/70 109/99/34 108/101/33 611 | f 108/98/33 107/96/31 8/183/71 612 | f 107/96/31 105/95/27 6/185/73 613 | f 6/186/73 105/91/27 106/93/29 614 | -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | MockRobot 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Roselle Carmen 10 | rosellec@openrobotics.org 11 | 12 | 13 | 14 | A "kawaii" robot in CGH 15 | 16 | 17 | -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | / 8 | true 9 | 10 | 10.0 11 | 12 | joint_tire_left 13 | joint_tire_right 14 | 15 | 0.42 16 | 1.22 17 | 18 | 150 19 | 0.5 20 | 21 | true 22 | false 23 | odom 24 | chassis 25 | 26 | 27 | 28 | 29 | -0.35 0 .2 0 0 3.14 30 | 31 | 0.1 32 | 33 | .10 34 | .10 35 | .30 36 | 37 | 38 | 39 | 40 | 41 | 0.03 0.2 0.03 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 0.03 0.2 0.03 50 | 51 | 52 | 53 | 54 | 1 0 0 1 55 | 1 1 1 1 56 | 0.1 0.1 0.1 1 57 | 0 0 0 0 58 | 59 | 60 | 61 | 62 | 63 | 64 | 10.0 65 | 66 | 1.204 67 | 68 | 70 | 74 | 711 75 | 400 76 | R8G8B8 77 | 78 | 79 | 0.02 80 | 300 81 | 82 | 83 | gaussian 84 | 87 | 0.0 88 | 0.02 89 | 90 | 91 | 92 | true 93 | 0.0 94 | /camera/color 95 | image_raw 96 | camera_info 97 | camera_link 98 | 0.07 99 | 0.0 100 | 0.0 101 | 0.0 102 | 0.0 103 | 0.0 104 | 105 | 106 | 107 | 108 | 109 | 0 0 0 0 0 0 110 | 111 | 112 | 0 0 0.693 0 0 0 113 | 170 114 | 115 | 34.6605 116 | 32.8369 117 | 13.0691 118 | 119 | 120 | 121 | 122 | 0 0 0.7 0 0 0 123 | 124 | 125 | 0.7 0.7 1.4 126 | 127 | 128 | 129 | 130 | 131 | 0.0 132 | 133 | 134 | 0.0 135 | 0.0 136 | 137 | 138 | 0.0 139 | 0.0 140 | 0.0 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | model://MockRobot/meshes/MockRobot.obj 149 | 150 | 151 | 152 | .17 -.21 .03 0 0 0 153 | 154 | model://MockRobot/meshes/SmallWheel.obj 155 | 156 | 157 | 158 | -.24 -.21 .03 0 0 0 159 | 160 | model://MockRobot/meshes/SmallWheel.obj 161 | 162 | 163 | 164 | .17 .21 .03 0 0 0 165 | 166 | model://MockRobot/meshes/SmallWheel.obj 167 | 168 | 169 | 170 | -.24 .21 .03 0 0 0 171 | 172 | model://MockRobot/meshes/SmallWheel.obj 173 | 174 | 175 | 176 | 177 | 178 | 180 | 0 .21 .06 0 0 0 181 | 182 | 1.00 183 | 184 | 1.00 185 | 1.00 186 | 1.00 187 | 188 | 189 | 190 | 191 | 192 | body 193 | suspension_tire_left 194 | 195 | 0 0 1 196 | 197 | 500 198 | -0.05 199 | 2000 200 | 201 | 202 | 203 | 204 | true 205 | 206 | 207 | 208 | 209 | 210 | 0 .21 .06 0 0 0 211 | 212 | 0 0 0 0 0 0 213 | 5.0 214 | 215 | 0.757897446874 216 | 1.302975553365 217 | 0.757897446874 218 | 219 | 220 | 221 | 222 | model://MockRobot/meshes/FatWheel.obj 223 | 224 | 225 | 226 | 227 | 0.06137 228 | 229 | 230 | 231 | 232 | 10000000.0 233 | 1.0 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | suspension_tire_left 242 | tire_left 243 | 244 | 0 1 0 245 | 246 | 247 | 248 | 249 | 251 | 0 -.21 .06 0 0 0 252 | 253 | 1.00 254 | 255 | 1.00 256 | 1.00 257 | 1.00 258 | 259 | 260 | 261 | 262 | 263 | body 264 | suspension_tire_right 265 | 266 | 0 0 1 267 | 268 | 500 269 | -0.05 270 | 2000 271 | 272 | 273 | 274 | 275 | true 276 | 277 | 278 | 279 | 280 | 281 | 0 -.21 .06 0 0 0 282 | 283 | 0 0 0 0 0 0 284 | 5.0 285 | 286 | 0.757897446874 287 | 1.302975553365 288 | 0.757897446874 289 | 290 | 291 | 292 | 293 | model://MockRobot/meshes/FatWheel.obj 294 | 295 | 296 | 297 | 298 | 0.06137 299 | 300 | 301 | 302 | 303 | 10000000.0 304 | 1.0 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | suspension_tire_right 313 | tire_right 314 | 315 | 0 1 0 316 | 317 | 318 | 319 | 320 | body 321 | camera_link 322 | 323 | 0 0 2 324 | 325 | 326 | 327 | 328 | 329 | -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/thumbnails/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/MockRobot/thumbnails/1.png -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/thumbnails/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/MockRobot/thumbnails/2.png -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/thumbnails/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/MockRobot/thumbnails/3.png -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/thumbnails/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/MockRobot/thumbnails/4.png -------------------------------------------------------------------------------- /autodock_sim/models/MockRobot/thumbnails/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/autodock_sim/models/MockRobot/thumbnails/5.png -------------------------------------------------------------------------------- /autodock_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autodock_sim 4 | 2.5.21 5 | 6 | Simulation and launch files for autodock 7 | 8 | 9 | You Liang 10 | 11 | Apache 2.0 12 | catkin 13 | cmake_modules 14 | gazebo_dev 15 | 16 | aruco_detect 17 | autodock_core 18 | gazebo_dev 19 | 20 | gazebo_msgs 21 | roslib 22 | roscpp 23 | tf 24 | std_srvs 25 | rosgraph_msgs 26 | dynamic_reconfigure 27 | std_msgs 28 | geometry_msgs 29 | 30 | 31 | -------------------------------------------------------------------------------- /autodock_sim/src/ChargingStationPlugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021 Open Source Robotics Foundation 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 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace gazebo 31 | { 32 | class ChargingStationPlugin : public FlashLightPlugin 33 | { 34 | public: 35 | using FlashLightPlugin::Load; 36 | using FlashLightPlugin::OnUpdate; 37 | 38 | using TriggerReq = std_srvs::Trigger::Request; 39 | using TriggerRes = std_srvs::Trigger::Response; 40 | 41 | ChargingStationPlugin() : 42 | charging_led_on_(false) 43 | {} 44 | 45 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 46 | { 47 | // Store the pointer to the model 48 | ROS_WARN("\n\t\t START ChargingStationPlugin!"); 49 | FlashLightPlugin::Load(_parent, _sdf); 50 | load_param(_sdf); 51 | 52 | this->rosnode_ = new ros::NodeHandle(); 53 | this->charging_srv_ = this->rosnode_->advertiseService( 54 | "trigger_charger_srv", 55 | &ChargingStationPlugin::TriggerChargerCallback, 56 | this); 57 | 58 | // TODO sdf reader topic 59 | _gazebo_node = gazebo::transport::NodePtr(new gazebo::transport::Node()); 60 | _gazebo_node->Init(); 61 | _charge_state_sub = _gazebo_node->Subscribe( 62 | param_.contact_topic, 63 | &ChargingStationPlugin::contact_state_cb, this); 64 | } 65 | 66 | // Called by the world update start event 67 | void OnUpdate() 68 | { 69 | FlashLightPlugin::OnUpdate(); 70 | if (charging_led_on_ && 71 | ((ros::Time::now() - trigger_charger_time_).sec > param_.reset_timeout)) 72 | { 73 | ROS_INFO("Switch back to original charging led color"); 74 | this->ChangeColor( 75 | param_.light_name, param_.light_link_name, param_.original_color); 76 | charging_led_on_ = false; 77 | } 78 | } 79 | 80 | private: 81 | /// sdf param loader 82 | void load_param(sdf::ElementPtr _sdf) 83 | { 84 | if (_sdf->HasElement("target_color")) 85 | param_.target_color = _sdf->Get("target_color"); 86 | else 87 | ROS_ERROR(" is not defined"); 88 | 89 | if (_sdf->HasElement("contact_topic")) 90 | param_.contact_topic = _sdf->Get("contact_topic"); 91 | else 92 | ROS_WARN(" is not defined, use default %s", 93 | param_.contact_topic.c_str()); 94 | 95 | if (_sdf->HasElement("target_collision")) 96 | param_.target_collision = _sdf->Get("target_collision"); 97 | else 98 | ROS_WARN(" is not defined, use default %s", 99 | param_.target_collision.c_str()); 100 | 101 | if (_sdf->HasElement("reset_timeout")) 102 | param_.reset_timeout = _sdf->Get("reset_timeout"); 103 | else 104 | ROS_WARN(" is not defined, use default %f", 105 | param_.reset_timeout); 106 | 107 | // Light Block 108 | if (_sdf->HasElement("light")) 109 | { 110 | sdf::ElementPtr sdfLight = _sdf->GetElement("light"); 111 | if (sdfLight->HasElement("color")) 112 | param_.original_color = sdfLight->Get("color"); 113 | else 114 | ROS_ERROR("original_color: `` is not defined"); 115 | 116 | if (sdfLight->HasElement("id")) 117 | { 118 | std::string lightId = sdfLight->Get("id"); 119 | int posDelim = lightId.rfind("/"); 120 | param_.light_name = lightId.substr(posDelim + 1, lightId.length()); 121 | param_.light_link_name = lightId.substr(0, posDelim); 122 | ROS_WARN("Light name: %s, LinkName %s", 123 | param_.light_name.c_str(), param_.light_link_name.c_str()); 124 | } 125 | else 126 | gzerr << "Parameter is missing." << std::endl; 127 | } 128 | else 129 | ROS_ERROR("Parameter `` block is not defined"); 130 | } 131 | 132 | void contact_state_cb(ConstContactsPtr &_msg) 133 | { 134 | // check if there's any contact, 135 | // and check if collision is as defined target 136 | if (_msg->contact().size() != 0 && 137 | (param_.target_collision == _msg->contact().Get(0).collision2())) 138 | { 139 | // update collision time as this is a valid one 140 | last_collision_time_ = ros::Time::now(); 141 | // TODO check if body_1_wrench force (x) exceeds a threshhold 142 | last_collision_verbose_msg_ = _msg->contact().Get(0).DebugString(); 143 | 144 | last_collision_x_wrench = 0.0; 145 | for (const auto wrench : _msg->contact().Get(0).wrench()) 146 | { 147 | last_collision_x_wrench += wrench.body_1_wrench().force().x(); 148 | } 149 | } 150 | } 151 | 152 | // gz transport 153 | gazebo::transport::NodePtr _gazebo_node; 154 | gazebo::transport::SubscriberPtr _charge_state_sub; 155 | 156 | // ros node 157 | ros::NodeHandle *rosnode_; 158 | ros::ServiceServer charging_srv_; 159 | 160 | // runtime dynamic variables 161 | ros::Time last_collision_time_; 162 | std::string last_collision_verbose_msg_; 163 | double last_collision_x_wrench; 164 | ros::Time trigger_charger_time_; 165 | std::atomic charging_led_on_; 166 | 167 | struct PluginParam 168 | { 169 | std::string contact_topic = "gazebo/default/physics/contacts"; 170 | std::string target_collision = "mock_docking_robot::body::collision"; 171 | double reset_timeout = 8.0; 172 | double max_allowable_force; /// TODO 173 | ignition::math::Color target_color; 174 | ignition::math::Color original_color; 175 | std::string light_name; 176 | std::string light_link_name; 177 | }; 178 | 179 | PluginParam param_; 180 | 181 | bool TriggerChargerCallback( 182 | TriggerReq &req, TriggerRes &res) 183 | { 184 | ROS_INFO("Received Charging Trigger Request"); 185 | 186 | trigger_charger_time_ = ros::Time::now(); 187 | ROS_INFO("Time Debug: last_collision_time: %d.%d", 188 | last_collision_time_.sec, last_collision_time_.nsec); 189 | 190 | // Check if there's any collision within time duration 191 | const auto time_diff = trigger_charger_time_ - last_collision_time_; 192 | if (time_diff.sec > param_.reset_timeout) 193 | { 194 | res.message = "No contact betwwen the robot and the charger"; 195 | res.success = false; 196 | } 197 | else 198 | { 199 | // ROS_INFO("Contact Point: %s", last_collision_verbose_msg_.c_str()); 200 | this->ChangeColor( 201 | param_.light_name, param_.light_link_name, param_.target_color); 202 | res.message = "valid contact with robot, total x wrench force: " + 203 | std::to_string(last_collision_x_wrench) + " N"; 204 | charging_led_on_ = true; 205 | res.success = true; 206 | } 207 | return true; 208 | } 209 | }; 210 | 211 | // Register this plugin with the simulator 212 | GZ_REGISTER_MODEL_PLUGIN(ChargingStationPlugin) 213 | } 214 | -------------------------------------------------------------------------------- /autodock_sim/worlds/charging.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0.002 5 | 4000 6 | 7 | 8 | 9 | model://sun 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | 0 0 2 0 -0 0 17 | 0 18 | 19 | 0.6 20 | 1 21 | 1 22 | 23 | 24 | 25 | 26 | model://MockCharger 27 | charger_base1 28 | 0 0 0 0 0 0 29 | True 30 | 31 | 32 | 33 | model://MockRobot 34 | mock_docking_robot 35 | 36 | 37 | 2.0 0.35 0 0 0 0.3 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /autodock_sim/worlds/tb3_charging.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0.002 5 | 2000 6 | 7 | 8 | 9 | model://sun 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | 0 0 2 0 -0 0 17 | 0 18 | 19 | 0.6 20 | 1 21 | 1 22 | 23 | 24 | 25 | 26 | model://MiniMockCharger 27 | charger_base1 28 | -0.5 0 0 0 0 0 29 | True 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /docs/architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/docs/architecture.png -------------------------------------------------------------------------------- /docs/dock_gz_sim.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/docs/dock_gz_sim.gif -------------------------------------------------------------------------------- /docs/state_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/docs/state_diagram.png -------------------------------------------------------------------------------- /docs/tb3_dock_sim.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/autodock/fd9c2a9aa6d05e4df84883008562e49506f690c0/docs/tb3_dock_sim.gif --------------------------------------------------------------------------------