├── .gitignore ├── LICENSE ├── README.md ├── agvs_control ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ └── agvs_control.yaml ├── launch │ └── agvs_control.launch └── package.xml ├── agvs_gazebo ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── agvs.launch │ ├── agvs_floor_1.launch │ ├── agvs_office.launch │ └── agvs_sim_complete.launch ├── models │ └── agvs_floor_1 │ │ ├── meshes │ │ ├── floor_1.dae │ │ └── floor_magnets_1.dae │ │ ├── model-1_2.sdf │ │ ├── model-1_3.sdf │ │ ├── model-1_4.sdf │ │ ├── model.config │ │ └── model.sdf ├── package.xml └── worlds │ ├── agvs.world │ ├── agvs_floor_1.world │ └── agvs_office.world ├── agvs_robot_control ├── .rosinstall ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── agvs_robot_control.launch │ └── agvs_robot_control.yaml ├── package.xml └── src │ └── agvs_robot_control.cpp ├── agvs_sim ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml └── agvs_sim_bringup ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch ├── agvs_gmapping.launch ├── agvs_hector_mapping.launch ├── amcl_diff.launch ├── amcl_diff_2.launch ├── map_server.launch ├── mapping.rviz ├── purepursuit_nav.rviz └── robot_pose_ekf.launch ├── maps ├── agvs_floor_1.pgm ├── agvs_floor_1.yaml ├── willow.pgm └── willow.yaml ├── package.xml └── scripts └── save_map.sh /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Robotnik Automation 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 18 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 20 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 21 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 22 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 23 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # agvs_sim 2 | Agvs robot sim packages 3 | 4 | ## agvs_control 5 | This package contains the configuration files for the Gazebo controllers used by the model. 6 | 7 | ## agvs_gazebo 8 | This package contains the configuration files to launch the Gazebo environment along with the simulated robot. 9 | 10 | ## agvs_robot_control 11 | This package contains the simulated controlled that interfaces with Gazebo controllers. 12 | 13 | ## agvs_sim_bringup 14 | This package contains all the launch and config files to launch all the needed packages to load the simulated environment. 15 | -------------------------------------------------------------------------------- /agvs_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package agvs_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * indigo-0.1.1 8 | * Redoing CHANGELOGS 9 | 10 | 0.1.1 (2015-07-29) 11 | ------------------ 12 | * Adding changelogs for the release 13 | * Adding the install macro to the CMakelists 14 | * Wrong catkin package macro invocation 15 | * Cleaning CMakelists and more 16 | * First indigo version commit 17 | * Contributors: Elena Gambaro, Isaac IY Saito, RomanRobotnik 18 | -------------------------------------------------------------------------------- /agvs_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(agvs_control) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | 7 | install(DIRECTORY launch 8 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | 10 | install(DIRECTORY config 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 12 | 13 | 14 | -------------------------------------------------------------------------------- /agvs_control/config/agvs_control.yaml: -------------------------------------------------------------------------------- 1 | agvs: 2 | # Velocity controllers 3 | # joint_back_wheel_effort_controller: 4 | # type: effort_controllers/JointEffortController 5 | # joint: joint_back_wheel 6 | # pid: {p: 1000.0, i: 500, d: 500.0} 7 | 8 | joint_back_wheel_controller: 9 | type: velocity_controllers/JointVelocityController 10 | joint: joint_back_wheel 11 | pid: {p: 1000.0, i: 0.01, d: 100.0} 12 | 13 | # joint_front_wheel_effort_controller: 14 | # type: effort_controllers/JointEffortController 15 | # joint: joint_front_wheel 16 | # pid: {p: 1000.0, i: 500, d: 500.0} 17 | 18 | joint_front_wheel_controller: 19 | type: velocity_controllers/JointVelocityController 20 | joint: joint_front_wheel 21 | pid: {p: 1000.0, i: 0.01, d: 100.0} 22 | 23 | # Position controllers 24 | joint_back_motor_wheel_controller: 25 | type: effort_controllers/JointPositionController 26 | joint: joint_back_motor_wheel 27 | pid: {p: 100.0, i: 0.01, d: 10.0} 28 | # Be careful with the selection of the pid values, e.g. d: 50.0 produces a nan in the velocity ??? 29 | 30 | joint_front_motor_wheel_controller: 31 | type: effort_controllers/JointPositionController 32 | joint: joint_front_motor_wheel 33 | pid: {p: 100.0, i: 0.01, d: 10.0} 34 | 35 | joint_elevator_controller: 36 | type: effort_controllers/JointPositionController 37 | joint: elevator_joint 38 | pid: {p: 10000.0, i: 10000.0, d: 500.0} 39 | 40 | joint_read_state_controller: 41 | type: joint_state_controller/JointStateController 42 | publish_rate: 100.0 43 | -------------------------------------------------------------------------------- /agvs_control/launch/agvs_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /agvs_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | agvs_control 4 | 0.1.1 5 | The agvs_control package. Config files used for Gazebo motor controllers. 6 | 7 | Román Navarro 8 | Roberto Guzmán 9 | 10 | 11 | BSD 12 | 13 | http://wiki.ros.org/agvs_control 14 | https://github.com/RobotnikAutomation/agvs_sim 15 | https://github.com/RobotnikAutomation/agvs_sim/issues 16 | 17 | Roberto Guzmán 18 | 19 | 20 | catkin 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /agvs_gazebo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package agvs_gazebo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * indigo-0.1.1 8 | * Redoing CHANGELOGS 9 | 10 | 0.1.1 (2015-07-29) 11 | ------------------ 12 | * agvs_gazebo: adding run dependency on agvs_pad and agvs_robot_control 13 | * Adding changelogs for the release 14 | * Adding the install macro to the CMakelists 15 | * Cleaning CMakelists and more 16 | * First indigo version commit 17 | * Contributors: Elena Gambaro, RomanRobotnik 18 | -------------------------------------------------------------------------------- /agvs_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(agvs_gazebo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | agvs_description 9 | gazebo_ros 10 | roscpp 11 | std_msgs 12 | std_srvs 13 | tf 14 | joint_state_controller 15 | velocity_controllers 16 | effort_controllers 17 | ) 18 | 19 | catkin_package() 20 | 21 | install(DIRECTORY launch 22 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 23 | 24 | install(DIRECTORY worlds 25 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 26 | -------------------------------------------------------------------------------- /agvs_gazebo/launch/agvs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /agvs_gazebo/launch/agvs_floor_1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /agvs_gazebo/launch/agvs_office.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /agvs_gazebo/launch/agvs_sim_complete.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /agvs_gazebo/models/agvs_floor_1/meshes/floor_1.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | VCGLab 6 | VCGLib | MeshLab 7 | 8 | Y_UP 9 | vie jun 10 12:32:39 2016 10 | vie jun 10 12:32:39 2016 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 42.6667 35.7121 0 42.6667 35.7121 2.3876 42.6665 33.1217 0 42.6665 35.7121 2.3876 44.4953 33.1217 2.3876 44.4953 29.1629 2.3876 44.88 28.7629 0 44.88 27.1629 0 44.88 27.1629 2.3876 45.28 27.1629 0 45.28 26.7629 2.3876 44.4611 22.6629 2.3876 45.28 22.6629 2.3876 45.28 22.2629 0 45.28 22.2629 2.3876 44.4709 22.2629 0 44.4709 22.2629 2.3876 44.4709 19.645 0 44.5832 19.645 0 44.5832 19.645 2.3876 45.289 19.469 0 44.5832 19.469 2.3876 45.289 19.469 2.3876 45.289 14.1658 2.3876 40.6759 13.8934 0 44.7858 13.8934 2.3876 40.6759 11.684 2.3876 40.6759 11.684 0 30.2281 13.3529 0 11.2333 14.2249 0 11.2333 14.9653 0 11.2333 14.9653 2.3876 9.83974 14.9653 2.3876 9.83974 14.1873 2.3876 9.43974 14.1873 0 9.43974 14.5873 0 9.43974 14.1873 2.3876 8.18974 14.5873 0 9.43974 14.5873 2.3876 7.78975 14.1873 2.3876 7.78975 14.5873 2.3876 5.28975 14.5873 0 5.68975 14.2379 0 5.68975 14.2379 2.3876 1.48414 8.91515 0 4.52095 8.91515 2.3876 4.52095 8.35635 0 -0.000254112 8.35635 2.3876 -0.000254112 -0.000253991 0 7.72135 -0.000253991 0 7.72135 8.35635 0 7.72135 -0.000253991 2.3876 7.72135 8.35635 2.3876 7.46735 8.35635 0 7.78975 8.91515 0 7.46735 8.91515 2.3876 7.78975 8.91515 2.3876 8.18974 7.35854 0 10.8709 8.94055 0 10.8709 7.35854 2.3876 11.0741 8.94055 0 11.0741 8.94055 2.3876 11.0741 11.9885 2.3876 26.7205 6.92976 0 29.3621 6.92976 0 29.3621 8.75856 2.3876 47.3707 8.75856 0 47.3707 8.75856 2.3876 47.3707 8.94055 0 47.6501 8.94055 0 47.3707 8.94055 2.3876 47.6501 8.94055 2.3876 47.6501 11.9885 0 49.4789 11.9885 0 47.6501 11.9885 2.3876 49.4789 13.8178 0 49.4789 13.8178 2.3876 47.6501 13.8178 2.3876 47.6501 35.7121 0 46.9389 35.7121 0 47.6501 35.7121 2.3876 90.5399 64.1348 2.3876 92.5099 64.1348 0 92.5099 66.5366 0 92.5099 64.1348 2.3876 92.5099 86.9566 2.3876 92.5099 89.4347 0 90.6811 89.4347 0 90.6811 109.601 0 92.5099 109.601 0 92.5099 109.601 2.3876 90.6811 112.077 2.3876 90.6811 132.245 2.3876 92.5099 132.245 0 92.5099 134.721 0 92.5099 134.721 2.3876 90.6811 134.721 2.3876 89.735 159.258 0 90.6811 159.258 2.3876 97.6545 159.613 0 97.6545 175.514 0 88.5493 175.514 0 97.6545 175.514 2.3876 88.5493 169.378 0 88.5493 169.378 2.3876 87.7356 166.866 0 87.2855 166.866 0 87.7356 166.866 2.3876 87.2855 166.866 2.3876 87.2855 159.613 2.3876 87.9062 159.613 0 87.9062 159.613 2.3876 87.0079 159.258 2.3876 85.1786 134.721 2.3876 85.1786 132.321 2.3876 87.0074 132.321 0 87.0074 132.321 2.3876 85.1786 112.077 0 85.1786 109.677 0 85.1786 109.677 2.3876 87.0074 109.677 0 87.0074 109.677 2.3876 85.1786 89.4332 0 85.1786 89.4332 2.3876 85.1786 87.0329 0 87.0074 87.0329 0 85.1786 87.0329 2.3876 87.0074 66.5478 0 87.0074 66.5478 2.3876 47.7962 66.5478 2.3876 47.7962 66.262 2.3876 47.4596 68.0908 2.3876 42.677 68.0908 2.3876 42.677 36.1693 2.3876 44.5005 35.7121 0 44.5005 35.7121 2.3876 42.6717 35.7121 0 44.5008 35.7119 2.3876 44.5008 35.7119 0 42.6772 36.1696 2.3876 47.4594 68.0905 2.3876 47.4594 68.0905 0 47.4594 66.2617 0 47.7965 66.2617 0 47.7965 66.5475 0 87.0077 87.0331 2.3876 87.0077 87.0331 0 85.1789 87.0331 0 87.0077 109.677 2.3876 85.1789 109.677 2.3876 87.0077 109.677 0 85.1789 109.677 0 85.1789 112.077 2.3876 87.0077 112.077 2.3876 87.0077 112.077 0 87.0077 132.321 0 85.1789 134.721 2.3876 87.0077 134.721 0 87.0081 159.258 2.3876 87.0081 159.258 0 87.9064 159.614 0 87.2857 159.614 2.3876 87.2857 159.614 0 87.7358 169.378 2.3876 87.7358 166.866 0 87.7358 169.378 0 88.5495 169.378 2.3876 88.5495 169.378 0 88.5495 175.513 2.3876 97.6543 159.614 2.3876 97.6543 175.513 0 97.6543 159.614 0 89.7347 159.614 2.3876 89.7347 159.257 2.3876 92.5097 134.721 0 92.5097 109.601 2.3876 90.6809 109.601 2.3876 92.5097 89.4345 0 90.6809 66.5364 2.3876 92.5097 64.135 0 90.5396 63.3349 0 87.439 63.3349 0 87.439 63.0301 2.3876 47.4594 36.1696 0 46.9387 36.1696 2.3876 46.9387 35.7119 2.3876 46.9387 35.7119 0 47.6499 35.7119 2.3876 49.4787 13.8176 2.3876 49.4787 13.8176 0 49.4787 11.9888 2.3876 49.4787 11.9888 0 47.6499 8.9408 2.3876 47.6499 8.9408 0 47.3705 8.9408 2.3876 47.3705 8.75882 0 29.3619 8.75882 2.3876 26.7208 6.93002 2.3876 26.7208 6.93002 0 11.0739 8.9408 2.3876 10.8707 8.9408 0 8.19 9.1876 0 7.78949 8.9154 2.3876 7.78949 9.1876 0 7.46709 8.9154 2.3876 7.46709 8.9154 0 7.46709 8.35609 2.3876 7.72109 8.35609 2.3876 7.46709 8.35609 0 7.72109 1.43178e-08 0 4.24237e-08 1.43178e-08 2.3876 4.5212 8.35609 0 4.5212 8.9154 2.3876 4.5212 8.9154 0 1.48439 13.8377 0 5.69001 14.2382 0 5.29001 14.2382 0 7.78949 14.5871 2.3876 5.29001 14.5871 0 7.78949 14.1871 0 8.19 14.1871 2.3876 9.43949 14.5871 2.3876 9.43949 14.1871 2.3876 9.83999 14.965 2.3876 11.233 14.965 2.3876 11.233 14.2247 0 30.2278 14.2247 0 30.2278 14.2247 2.3876 31.0654 13.3527 0 31.0654 11.6837 0 40.6762 11.6837 2.3876 40.6762 13.8931 2.3876 44.7861 13.8931 0 45.2893 14.1655 2.3876 45.2893 19.4692 0 44.5835 19.4692 0 44.5835 19.6453 2.3876 45.2803 22.2627 0 44.4614 26.7626 2.3876 44.4614 26.7626 0 45.2803 27.1631 0 45.2803 27.1631 2.3876 44.8803 28.7626 0 45.2803 28.7626 0 44.4955 29.1631 2.3876 44.4955 29.1631 0 46.9389 36.1693 0 42.6717 35.7119 0 44.5008 36.1696 0 42.6665 35.7121 0 42.6667 33.1219 0 44.4953 33.1217 0 44.4955 33.1219 0 44.4953 29.1629 0 45.28 29.1629 0 45.28 28.7629 0 45.2803 29.1631 0 44.8803 27.1631 0 45.28 26.7629 0 45.2803 26.7626 0 44.4611 26.7629 0 44.4611 22.6629 0 44.4614 22.6632 0 45.28 22.6629 0 45.2803 22.6632 0 44.4711 22.2627 0 44.4711 19.6453 0 44.5832 19.469 0 44.5835 19.6453 0 45.2893 14.1655 0 45.289 14.1658 0 44.7858 14.1658 0 44.7861 14.1655 0 44.7858 13.8934 0 40.6762 13.8931 0 40.6762 11.6837 0 31.0656 11.684 0 31.0656 13.3529 0 30.2278 13.3527 0 30.2281 14.2249 0 9.83974 14.9653 0 11.233 14.965 0 9.83999 14.965 0 9.83974 14.1873 0 9.83999 14.1871 0 9.43949 14.1871 0 9.43949 14.5871 0 8.18974 14.1873 0 8.19 14.5871 0 7.78975 14.1873 0 8.19 14.1871 0 7.78975 14.5873 0 7.78949 14.5871 0 5.28975 14.2379 0 5.68975 13.8379 0 1.48414 13.8379 0 5.69001 13.8377 0 1.48439 8.9154 0 4.52095 8.91515 0 -0.000254112 8.35635 0 4.24237e-08 8.35609 0 4.24237e-08 1.43178e-08 0 7.72109 8.35609 0 7.46735 8.91515 0 7.78949 8.9154 0 7.78975 9.18734 0 8.18974 9.18734 0 8.19 7.3588 0 10.8709 7.35854 0 10.8707 7.3588 0 11.0739 8.9408 0 11.0739 11.9888 0 11.0741 11.9885 0 26.7205 11.9885 0 26.7208 11.9888 0 29.3621 8.75856 0 29.3619 6.93002 0 29.3619 8.75882 0 47.3705 8.9408 0 47.6499 11.9888 0 47.6501 13.8178 0 47.6499 13.8176 0 47.6499 35.7119 0 46.9387 36.1696 0 47.4596 36.1693 0 47.4594 63.0301 0 87.4392 63.0298 0 87.439 63.0301 0 90.5399 63.3346 0 90.5399 64.1348 0 90.5396 64.135 0 92.5097 66.5364 0 90.6811 66.5366 0 90.6811 86.9566 0 90.6809 66.5364 0 90.6809 86.9569 0 92.5097 86.9569 0 92.5099 86.9566 0 90.6809 89.4345 0 90.6809 109.601 0 92.5097 109.601 0 92.5099 112.077 0 90.6811 112.077 0 92.5097 112.077 0 90.6809 112.077 0 90.6811 132.245 0 90.6809 132.245 0 92.5097 132.245 0 90.6811 134.721 0 90.6811 159.258 0 90.6809 134.721 0 90.6809 159.257 0 89.7347 159.257 0 89.735 159.613 0 89.7347 159.614 0 88.5495 175.513 0 87.7356 169.378 0 87.2857 166.866 0 87.2855 159.613 0 87.9062 159.258 0 87.9064 159.257 0 87.0079 159.258 0 87.0074 134.721 0 85.1786 134.721 0 85.1786 132.321 0 85.1789 134.721 0 85.1789 132.321 0 87.0074 112.077 0 85.1789 112.077 0 87.0074 89.4332 0 87.0077 89.4329 0 85.1789 89.4329 0 87.0077 66.5475 0 47.7962 66.5478 0 47.7962 66.262 0 47.4596 66.262 0 47.4596 68.0908 0 42.6772 68.0905 0 42.677 68.0908 0 42.677 36.1693 0 42.6772 36.1696 0 44.5005 36.1693 0 87.4392 63.3346 0 42.6667 33.1219 2.3876 42.6665 33.1217 2.3876 42.6717 35.7121 2.3876 42.6717 35.7119 2.3876 44.5008 36.1696 2.3876 44.5005 36.1693 2.3876 42.6772 68.0905 2.3876 47.4596 66.262 2.3876 47.4594 66.2617 2.3876 47.7965 66.2617 2.3876 47.7965 66.5475 2.3876 87.0077 66.5475 2.3876 87.0074 87.0329 2.3876 85.1789 87.0331 2.3876 87.0074 89.4332 2.3876 85.1789 89.4329 2.3876 87.0077 89.4329 2.3876 85.1786 112.077 2.3876 87.0074 112.077 2.3876 87.0077 132.321 2.3876 85.1789 132.321 2.3876 87.0074 134.721 2.3876 87.0077 134.721 2.3876 87.9062 159.258 2.3876 87.9064 159.257 2.3876 87.9064 159.614 2.3876 87.2857 166.866 2.3876 87.7358 166.866 2.3876 87.7356 169.378 2.3876 88.5493 175.514 2.3876 97.6545 159.613 2.3876 97.6543 175.513 2.3876 89.735 159.613 2.3876 89.735 159.258 2.3876 90.6809 159.257 2.3876 90.6809 134.721 2.3876 92.5097 134.721 2.3876 92.5099 132.245 2.3876 92.5097 132.245 2.3876 90.6809 132.245 2.3876 90.6809 112.077 2.3876 92.5099 112.077 2.3876 92.5097 112.077 2.3876 90.6811 109.601 2.3876 90.6809 89.4345 2.3876 90.6811 89.4347 2.3876 92.5099 89.4347 2.3876 92.5097 89.4345 2.3876 92.5097 86.9569 2.3876 90.6811 86.9566 2.3876 90.6809 86.9569 2.3876 90.6811 66.5366 2.3876 92.5097 66.5364 2.3876 92.5099 66.5366 2.3876 92.5097 64.135 2.3876 90.5399 63.3346 2.3876 90.5396 64.135 2.3876 90.5396 63.3349 2.3876 87.4392 63.3346 2.3876 87.439 63.3349 2.3876 47.4596 36.1693 2.3876 47.4594 63.0301 2.3876 47.4594 36.1696 2.3876 46.9389 36.1693 2.3876 46.9389 35.7121 2.3876 47.6499 13.8176 2.3876 49.4789 11.9885 2.3876 47.6499 11.9888 2.3876 47.3705 8.75882 2.3876 29.3621 6.92976 2.3876 29.3619 6.93002 2.3876 26.7205 6.92976 2.3876 26.7205 11.9885 2.3876 26.7208 11.9888 2.3876 11.0739 11.9888 2.3876 10.8709 8.94055 2.3876 10.8707 8.9408 2.3876 8.18974 7.35854 2.3876 10.8707 7.3588 2.3876 8.19 7.3588 2.3876 8.18974 9.18734 2.3876 8.19 9.1876 2.3876 7.78949 9.1876 2.3876 7.78975 9.18734 2.3876 7.46735 8.35635 2.3876 -0.000254112 -0.000253991 2.3876 7.72109 1.43178e-08 2.3876 4.24237e-08 8.35609 2.3876 4.52095 8.35635 2.3876 4.5212 8.35609 2.3876 1.48439 8.9154 2.3876 1.48414 8.91515 2.3876 1.48414 13.8379 2.3876 5.68975 13.8379 2.3876 1.48439 13.8377 2.3876 5.69001 13.8377 2.3876 5.28975 14.2379 2.3876 5.69001 14.2382 2.3876 5.29001 14.2382 2.3876 5.29001 14.5871 2.3876 5.28975 14.5873 2.3876 7.78949 14.1871 2.3876 8.18974 14.1873 2.3876 8.18974 14.5873 2.3876 8.19 14.5871 2.3876 9.83999 14.1871 2.3876 11.2333 14.2249 2.3876 11.233 14.2247 2.3876 30.2281 14.2249 2.3876 30.2278 13.3527 2.3876 30.2281 13.3529 2.3876 31.0656 13.3529 2.3876 31.0654 13.3527 2.3876 31.0656 11.684 2.3876 31.0654 11.6837 2.3876 40.6759 13.8934 2.3876 44.7861 13.8931 2.3876 44.7858 14.1658 2.3876 44.7861 14.1655 2.3876 45.2893 19.4692 2.3876 44.5835 19.4692 2.3876 44.4709 19.645 2.3876 44.4711 19.6453 2.3876 44.4711 22.2627 2.3876 45.2803 22.2627 2.3876 45.2803 22.6632 2.3876 44.4614 22.6632 2.3876 44.4611 26.7629 2.3876 45.28 27.1629 2.3876 45.2803 26.7626 2.3876 44.8803 27.1631 2.3876 44.88 28.7629 2.3876 44.8803 28.7626 2.3876 45.28 28.7629 2.3876 45.2803 28.7626 2.3876 45.28 29.1629 2.3876 45.2803 29.1631 2.3876 44.4955 33.1219 2.3876 47.4596 63.0298 0 47.4596 63.0298 2.3876 87.4392 63.0298 2.3876 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 0 1 0 0 1 0 -1 0 0 -1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 -3.12885e-06 0 1 -3.12885e-06 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 1.69857e-05 1 0 1.69857e-05 1 0 -1 1.71018e-05 0 -1 1.71018e-05 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 -1.74127e-05 0 1 -1.74127e-05 0 -1.69859e-05 -1 0 -1.69859e-05 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 3.12885e-06 0 -1 3.12885e-06 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0 1 4.25164e-16 0 1 4.25164e-16 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 1 0 0 1 0 0 0 -1 0 0 -1 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 |

0 0 249 0 1 0 1 1 249 1 3 1 136 2 247 2 385 2 385 3 247 3 386 3 383 4 250 4 1 4 1 5 250 5 0 5 249 6 2 6 3 6 3 7 2 7 384 7 2 8 251 8 384 8 384 9 251 9 4 9 251 10 253 10 4 10 4 11 253 11 5 11 253 12 254 12 5 12 5 13 254 13 518 13 254 14 255 14 518 14 518 15 255 15 516 15 255 16 6 16 516 16 516 17 6 17 514 17 6 18 7 18 514 18 514 19 7 19 8 19 7 20 9 20 8 20 8 21 9 21 511 21 9 22 258 22 511 22 511 23 258 23 10 23 258 24 260 24 10 24 10 25 260 25 510 25 260 26 261 26 510 26 510 27 261 27 11 27 261 28 263 28 11 28 11 29 263 29 12 29 263 30 13 30 12 30 12 31 13 31 14 31 13 32 15 32 14 32 14 33 15 33 16 33 15 34 17 34 16 34 16 35 17 35 504 35 17 36 18 36 504 36 504 37 18 37 19 37 18 38 267 38 19 38 19 39 267 39 21 39 267 40 20 40 21 40 21 41 20 41 22 41 20 42 270 42 22 42 22 43 270 43 23 43 270 44 271 44 23 44 23 45 271 45 500 45 271 46 273 46 500 46 500 47 273 47 25 47 273 48 24 48 25 48 25 49 24 49 498 49 24 50 27 50 498 50 498 51 27 51 26 51 27 52 276 52 26 52 26 53 276 53 496 53 276 54 277 54 496 54 496 55 277 55 494 55 277 56 28 56 494 56 494 57 28 57 493 57 28 58 279 58 493 58 493 59 279 59 491 59 279 60 29 60 491 60 491 61 29 61 489 61 29 62 30 62 489 62 489 63 30 63 31 63 30 64 280 64 31 64 31 65 280 65 32 65 280 66 283 66 32 66 32 67 283 67 33 67 283 68 34 68 33 68 33 69 34 69 36 69 34 70 35 70 36 70 36 71 35 71 38 71 35 72 37 72 38 72 38 73 37 73 486 73 37 74 287 74 486 74 486 75 287 75 485 75 287 76 289 76 485 76 485 77 289 77 39 77 289 78 291 78 39 78 39 79 291 79 40 79 291 80 41 80 40 80 40 81 41 81 483 81 41 82 293 82 483 82 483 83 293 83 479 83 293 84 42 84 479 84 479 85 42 85 43 85 42 86 294 86 43 86 43 87 294 87 476 87 294 88 295 88 476 88 476 89 295 89 475 89 295 90 44 90 475 90 475 91 44 91 474 91 44 92 298 92 474 92 474 93 298 93 45 93 298 94 46 94 45 94 45 95 46 95 471 95 46 96 299 96 471 96 471 97 299 97 47 97 299 98 48 98 47 98 47 99 48 99 468 99 48 100 49 100 468 100 468 101 49 101 51 101 49 102 50 102 51 102 51 103 50 103 52 103 50 104 53 104 52 104 52 105 53 105 467 105 53 106 303 106 467 106 467 107 303 107 55 107 303 108 54 108 55 108 55 109 54 109 56 109 54 110 305 110 56 110 56 111 305 111 466 111 305 112 306 112 466 112 466 113 306 113 463 113 306 114 57 114 463 114 463 115 57 115 460 115 57 116 308 116 460 116 460 117 308 117 59 117 308 118 58 118 59 118 59 119 58 119 458 119 58 120 60 120 458 120 458 121 60 121 61 121 60 122 312 122 61 122 61 123 312 123 62 123 312 124 313 124 62 124 62 125 313 125 455 125 313 126 63 126 455 126 455 127 63 127 454 127 63 128 64 128 454 128 454 129 64 129 452 129 64 130 315 130 452 130 452 131 315 131 65 131 315 132 66 132 65 132 65 133 66 133 67 133 66 134 68 134 67 134 67 135 68 135 70 135 68 136 69 136 70 136 70 137 69 137 71 137 69 138 72 138 71 138 71 139 72 139 74 139 72 140 73 140 74 140 74 141 73 141 449 141 73 142 75 142 449 142 449 143 75 143 76 143 75 144 320 144 76 144 76 145 320 145 77 145 320 146 78 146 77 146 77 147 78 147 80 147 78 148 79 148 80 148 80 149 79 149 447 149 79 150 246 150 447 150 447 151 246 151 446 151 382 152 328 152 441 152 441 153 328 153 438 153 328 154 329 154 438 154 438 155 329 155 81 155 329 156 82 156 81 156 81 157 82 157 84 157 82 158 83 158 84 158 84 159 83 159 436 159 83 160 332 160 436 160 436 161 332 161 434 161 332 162 333 162 434 162 434 163 333 163 432 163 333 164 337 164 432 164 432 165 337 165 85 165 337 166 86 166 85 166 85 167 86 167 429 167 86 168 87 168 429 168 429 169 87 169 428 169 87 170 88 170 428 170 428 171 88 171 426 171 88 172 89 172 426 172 426 173 89 173 90 173 89 174 341 174 90 174 90 175 341 175 424 175 341 176 342 176 424 176 424 177 342 177 91 177 342 178 345 178 91 178 91 179 345 179 92 179 345 180 93 180 92 180 92 181 93 181 420 181 93 182 94 182 420 182 420 183 94 183 95 183 94 184 348 184 95 184 95 185 348 185 96 185 348 186 349 186 96 186 96 187 349 187 98 187 349 188 97 188 98 188 98 189 97 189 416 189 97 190 353 190 416 190 416 191 353 191 415 191 353 192 99 192 415 192 415 193 99 193 413 193 99 194 100 194 413 194 413 195 100 195 102 195 100 196 101 196 102 196 102 197 101 197 412 197 101 198 103 198 412 198 412 199 103 199 104 199 103 200 356 200 104 200 104 201 356 201 411 201 356 202 105 202 411 202 411 203 105 203 107 203 105 204 106 204 107 204 107 205 106 205 108 205 106 206 358 206 108 206 108 207 358 207 109 207 358 208 110 208 109 208 109 209 110 209 111 209 110 210 359 210 111 210 111 211 359 211 406 211 359 212 361 212 406 212 406 213 361 213 112 213 361 214 362 214 112 214 112 215 362 215 404 215 362 216 363 216 404 216 404 217 363 217 113 217 363 218 364 218 113 218 113 219 364 219 114 219 364 220 115 220 114 220 114 221 115 221 116 221 115 222 367 222 116 222 116 223 367 223 401 223 367 224 117 224 401 224 401 225 117 225 400 225 117 226 118 226 400 226 400 227 118 227 119 227 118 228 120 228 119 228 119 229 120 229 121 229 120 230 369 230 121 230 121 231 369 231 397 231 369 232 122 232 397 232 397 233 122 233 123 233 122 234 124 234 123 234 123 235 124 235 126 235 124 236 125 236 126 236 126 237 125 237 395 237 125 238 127 238 395 238 395 239 127 239 128 239 127 240 373 240 128 240 128 241 373 241 129 241 373 242 374 242 129 242 129 243 374 243 130 243 374 244 375 244 130 244 130 245 375 245 390 245 375 246 376 246 390 246 390 247 376 247 131 247 376 248 378 248 131 248 131 249 378 249 132 249 378 250 379 250 132 250 132 251 379 251 133 251 379 252 381 252 133 252 133 253 381 253 388 253 381 254 134 254 388 254 388 255 134 255 135 255 134 256 136 256 135 256 135 257 136 257 385 257 386 258 247 258 137 258 137 259 247 259 138 259 137 260 138 260 387 260 387 261 138 261 248 261 387 262 248 262 139 262 139 263 248 263 380 263 139 264 380 264 389 264 389 265 380 265 377 265 389 266 377 266 140 266 140 267 377 267 141 267 140 268 141 268 391 268 391 269 141 269 142 269 391 270 142 270 392 270 392 271 142 271 143 271 392 272 143 272 393 272 393 273 143 273 144 273 393 274 144 274 394 274 394 275 144 275 372 275 394 276 372 276 145 276 145 277 372 277 146 277 145 278 146 278 396 278 396 279 146 279 147 279 396 280 147 280 398 280 398 281 147 281 371 281 398 282 371 282 399 282 399 283 371 283 370 283 399 284 370 284 148 284 148 285 370 285 150 285 148 286 150 286 149 286 149 287 150 287 151 287 149 288 151 288 152 288 152 289 151 289 368 289 152 290 368 290 153 290 153 291 368 291 154 291 153 292 154 292 402 292 402 293 154 293 155 293 402 294 155 294 403 294 403 295 155 295 366 295 403 296 366 296 156 296 156 297 366 297 365 297 156 298 365 298 405 298 405 299 365 299 157 299 405 300 157 300 158 300 158 301 157 301 159 301 158 302 159 302 407 302 407 303 159 303 360 303 407 304 360 304 408 304 408 305 360 305 160 305 408 306 160 306 161 306 161 307 160 307 162 307 161 308 162 308 409 308 409 309 162 309 357 309 409 310 357 310 410 310 410 311 357 311 164 311 410 312 164 312 163 312 163 313 164 313 165 313 163 314 165 314 166 314 166 315 165 315 167 315 166 316 167 316 168 316 168 317 167 317 355 317 168 318 355 318 414 318 414 319 355 319 170 319 414 320 170 320 169 320 169 321 170 321 171 321 169 322 171 322 172 322 172 323 171 323 354 323 172 324 354 324 173 324 173 325 354 325 352 325 173 326 352 326 417 326 417 327 352 327 351 327 417 328 351 328 418 328 418 329 351 329 350 329 418 330 350 330 419 330 419 331 350 331 174 331 419 332 174 332 421 332 421 333 174 333 347 333 421 334 347 334 422 334 422 335 347 335 346 335 422 336 346 336 423 336 423 337 346 337 344 337 423 338 344 338 425 338 425 339 344 339 343 339 425 340 343 340 175 340 175 341 343 341 340 341 175 342 340 342 176 342 176 343 340 343 339 343 176 344 339 344 427 344 427 345 339 345 338 345 427 346 338 346 430 346 430 347 338 347 177 347 430 348 177 348 431 348 431 349 177 349 336 349 431 350 336 350 433 350 433 351 336 351 335 351 433 352 335 352 178 352 178 353 335 353 334 353 178 354 334 354 435 354 435 355 334 355 331 355 435 356 331 356 437 356 437 357 331 357 179 357 437 358 179 358 439 358 439 359 179 359 330 359 439 360 330 360 440 360 440 361 330 361 180 361 440 362 180 362 442 362 442 363 180 363 181 363 442 364 181 364 182 364 182 365 181 365 327 365 182 366 327 366 444 366 444 367 327 367 325 367 444 368 325 368 445 368 445 369 325 369 183 369 445 370 183 370 184 370 184 371 183 371 323 371 184 372 323 372 185 372 185 373 323 373 186 373 185 374 186 374 187 374 187 375 186 375 322 375 187 376 322 376 448 376 448 377 322 377 321 377 448 378 321 378 188 378 188 379 321 379 189 379 188 380 189 380 190 380 190 381 189 381 191 381 190 382 191 382 450 382 450 383 191 383 319 383 450 384 319 384 192 384 192 385 319 385 193 385 192 386 193 386 194 386 194 387 193 387 318 387 194 388 318 388 451 388 451 389 318 389 195 389 451 390 195 390 196 390 196 391 195 391 317 391 196 392 317 392 453 392 453 393 317 393 316 393 453 394 316 394 197 394 197 395 316 395 198 395 197 396 198 396 456 396 456 397 198 397 314 397 456 398 314 398 457 398 457 399 314 399 311 399 457 400 311 400 199 400 199 401 311 401 310 401 199 402 310 402 459 402 459 403 310 403 200 403 459 404 200 404 461 404 461 405 200 405 309 405 461 406 309 406 462 406 462 407 309 407 307 407 462 408 307 408 464 408 464 409 307 409 201 409 464 410 201 410 465 410 465 411 201 411 203 411 465 412 203 412 202 412 202 413 203 413 304 413 202 414 304 414 204 414 204 415 304 415 205 415 204 416 205 416 206 416 206 417 205 417 208 417 206 418 208 418 207 418 207 419 208 419 302 419 207 420 302 420 469 420 469 421 302 421 209 421 469 422 209 422 210 422 210 423 209 423 301 423 210 424 301 424 470 424 470 425 301 425 300 425 470 426 300 426 472 426 472 427 300 427 211 427 472 428 211 428 212 428 212 429 211 429 213 429 212 430 213 430 473 430 473 431 213 431 297 431 473 432 297 432 477 432 477 433 297 433 214 433 477 434 214 434 478 434 478 435 214 435 296 435 478 436 296 436 480 436 480 437 296 437 215 437 480 438 215 438 481 438 481 439 215 439 216 439 481 440 216 440 482 440 482 441 216 441 218 441 482 442 218 442 217 442 217 443 218 443 292 443 217 444 292 444 484 444 484 445 292 445 219 445 484 446 219 446 220 446 220 447 219 447 290 447 220 448 290 448 487 448 487 449 290 449 288 449 487 450 288 450 221 450 221 451 288 451 286 451 221 452 286 452 222 452 222 453 286 453 285 453 222 454 285 454 488 454 488 455 285 455 284 455 488 456 284 456 223 456 223 457 284 457 282 457 223 458 282 458 224 458 224 459 282 459 281 459 224 460 281 460 490 460 490 461 281 461 225 461 490 462 225 462 227 462 227 463 225 463 226 463 227 464 226 464 492 464 492 465 226 465 278 465 492 466 278 466 495 466 495 467 278 467 228 467 495 468 228 468 497 468 497 469 228 469 229 469 497 470 229 470 230 470 230 471 229 471 275 471 230 472 275 472 231 472 231 473 275 473 274 473 231 474 274 474 499 474 499 475 274 475 232 475 499 476 232 476 501 476 501 477 232 477 272 477 501 478 272 478 233 478 233 479 272 479 269 479 233 480 269 480 502 480 502 481 269 481 234 481 502 482 234 482 503 482 503 483 234 483 235 483 503 484 235 484 236 484 236 485 235 485 268 485 236 486 268 486 505 486 505 487 268 487 266 487 505 488 266 488 506 488 506 489 266 489 265 489 506 490 265 490 507 490 507 491 265 491 237 491 507 492 237 492 508 492 508 493 237 493 264 493 508 494 264 494 509 494 509 495 264 495 262 495 509 496 262 496 238 496 238 497 262 497 239 497 238 498 239 498 512 498 512 499 239 499 259 499 512 500 259 500 241 500 241 501 259 501 240 501 241 502 240 502 513 502 513 503 240 503 257 503 513 504 257 504 515 504 515 505 257 505 242 505 515 506 242 506 517 506 517 507 242 507 243 507 517 508 243 508 519 508 519 509 243 509 256 509 519 510 256 510 244 510 244 511 256 511 245 511 244 512 245 512 520 512 520 513 245 513 252 513 520 514 252 514 383 514 383 515 252 515 250 515 246 516 324 516 446 516 446 517 324 517 443 517 247 518 136 518 138 518 138 519 136 519 134 519 138 520 134 520 248 520 248 521 134 521 381 521 249 522 0 522 2 522 2 523 0 523 250 523 2 524 250 524 251 524 251 525 250 525 252 525 251 526 252 526 253 526 253 527 252 527 245 527 253 528 245 528 254 528 254 529 245 529 256 529 254 530 256 530 255 530 255 531 256 531 243 531 255 532 243 532 6 532 6 533 243 533 242 533 6 534 242 534 7 534 7 535 242 535 257 535 7 536 257 536 9 536 9 537 257 537 240 537 9 538 240 538 258 538 258 539 240 539 259 539 258 540 259 540 260 540 260 541 259 541 239 541 260 542 239 542 261 542 261 543 239 543 262 543 261 544 262 544 263 544 263 545 262 545 264 545 263 546 264 546 13 546 13 547 264 547 237 547 13 548 237 548 15 548 15 549 237 549 265 549 15 550 265 550 17 550 17 551 265 551 266 551 17 552 266 552 18 552 18 553 266 553 268 553 18 554 268 554 267 554 267 555 268 555 235 555 267 556 235 556 20 556 20 557 235 557 234 557 20 558 234 558 270 558 270 559 234 559 269 559 270 560 269 560 271 560 271 561 269 561 272 561 271 562 272 562 273 562 273 563 272 563 232 563 273 564 232 564 24 564 24 565 232 565 274 565 24 566 274 566 27 566 27 567 274 567 275 567 27 568 275 568 276 568 276 569 275 569 229 569 276 570 229 570 277 570 277 571 229 571 228 571 277 572 228 572 28 572 28 573 228 573 278 573 28 574 278 574 279 574 279 575 278 575 226 575 279 576 226 576 29 576 29 577 226 577 225 577 29 578 225 578 30 578 30 579 225 579 281 579 30 580 281 580 280 580 280 581 281 581 282 581 280 582 282 582 283 582 283 583 282 583 284 583 283 584 284 584 34 584 34 585 284 585 285 585 34 586 285 586 35 586 35 587 285 587 286 587 35 588 286 588 37 588 37 589 286 589 288 589 37 590 288 590 287 590 287 591 288 591 290 591 287 592 290 592 289 592 289 593 290 593 219 593 289 594 219 594 291 594 291 595 219 595 292 595 291 596 292 596 41 596 41 597 292 597 218 597 41 598 218 598 293 598 293 599 218 599 216 599 293 600 216 600 42 600 42 601 216 601 215 601 42 602 215 602 294 602 294 603 215 603 296 603 294 604 296 604 295 604 295 605 296 605 214 605 295 606 214 606 44 606 44 607 214 607 297 607 44 608 297 608 298 608 298 609 297 609 213 609 298 610 213 610 46 610 46 611 213 611 211 611 46 612 211 612 299 612 299 613 211 613 300 613 299 614 300 614 48 614 48 615 300 615 301 615 48 616 301 616 49 616 49 617 301 617 209 617 49 618 209 618 50 618 50 619 209 619 302 619 50 620 302 620 53 620 53 621 302 621 208 621 53 622 208 622 303 622 303 623 208 623 205 623 303 624 205 624 54 624 54 625 205 625 304 625 54 626 304 626 305 626 305 627 304 627 203 627 305 628 203 628 306 628 306 629 203 629 201 629 306 630 201 630 57 630 57 631 201 631 307 631 57 632 307 632 308 632 308 633 307 633 309 633 308 634 309 634 58 634 58 635 309 635 200 635 58 636 200 636 60 636 60 637 200 637 310 637 60 638 310 638 312 638 312 639 310 639 311 639 312 640 311 640 313 640 313 641 311 641 314 641 313 642 314 642 63 642 63 643 314 643 198 643 63 644 198 644 64 644 64 645 198 645 316 645 64 646 316 646 315 646 315 647 316 647 317 647 315 648 317 648 66 648 66 649 317 649 195 649 66 650 195 650 68 650 68 651 195 651 318 651 68 652 318 652 69 652 69 653 318 653 193 653 69 654 193 654 72 654 72 655 193 655 319 655 72 656 319 656 73 656 73 657 319 657 191 657 73 658 191 658 75 658 75 659 191 659 189 659 75 660 189 660 320 660 320 661 189 661 321 661 320 662 321 662 78 662 78 663 321 663 322 663 78 664 322 664 79 664 79 665 322 665 186 665 79 666 186 666 246 666 246 667 186 667 323 667 246 668 323 668 324 668 324 669 323 669 183 669 324 670 183 670 521 670 521 671 183 671 325 671 521 672 325 672 326 672 326 673 325 673 327 673 326 674 327 674 382 674 382 675 327 675 181 675 382 676 181 676 328 676 328 677 181 677 180 677 328 678 180 678 329 678 329 679 180 679 330 679 329 680 330 680 82 680 82 681 330 681 179 681 82 682 179 682 83 682 83 683 179 683 331 683 83 684 331 684 332 684 332 685 331 685 334 685 332 686 334 686 333 686 333 687 334 687 335 687 333 688 335 688 337 688 337 689 335 689 336 689 337 690 336 690 86 690 86 691 336 691 177 691 86 692 177 692 87 692 87 693 177 693 338 693 87 694 338 694 88 694 88 695 338 695 339 695 88 696 339 696 89 696 89 697 339 697 340 697 89 698 340 698 341 698 341 699 340 699 343 699 341 700 343 700 342 700 342 701 343 701 344 701 342 702 344 702 345 702 345 703 344 703 346 703 345 704 346 704 93 704 93 705 346 705 347 705 93 706 347 706 94 706 94 707 347 707 174 707 94 708 174 708 348 708 348 709 174 709 350 709 348 710 350 710 349 710 349 711 350 711 351 711 349 712 351 712 97 712 97 713 351 713 352 713 97 714 352 714 353 714 353 715 352 715 354 715 353 716 354 716 99 716 99 717 354 717 171 717 99 718 171 718 100 718 100 719 171 719 170 719 100 720 170 720 101 720 101 721 170 721 355 721 101 722 355 722 103 722 103 723 355 723 167 723 103 724 167 724 356 724 356 725 167 725 165 725 356 726 165 726 105 726 105 727 165 727 164 727 105 728 164 728 106 728 106 729 164 729 357 729 106 730 357 730 358 730 358 731 357 731 162 731 358 732 162 732 110 732 110 733 162 733 160 733 110 734 160 734 359 734 359 735 160 735 360 735 359 736 360 736 361 736 361 737 360 737 159 737 361 738 159 738 362 738 362 739 159 739 157 739 362 740 157 740 363 740 363 741 157 741 365 741 363 742 365 742 364 742 364 743 365 743 366 743 364 744 366 744 115 744 115 745 366 745 155 745 115 746 155 746 367 746 367 747 155 747 154 747 367 748 154 748 117 748 117 749 154 749 368 749 117 750 368 750 118 750 118 751 368 751 151 751 118 752 151 752 120 752 120 753 151 753 150 753 120 754 150 754 369 754 369 755 150 755 370 755 369 756 370 756 122 756 122 757 370 757 371 757 122 758 371 758 124 758 124 759 371 759 147 759 124 760 147 760 125 760 125 761 147 761 146 761 125 762 146 762 127 762 127 763 146 763 372 763 127 764 372 764 373 764 373 765 372 765 144 765 373 766 144 766 374 766 374 767 144 767 143 767 374 768 143 768 375 768 375 769 143 769 142 769 375 770 142 770 376 770 376 771 142 771 141 771 376 772 141 772 378 772 378 773 141 773 377 773 378 774 377 774 379 774 379 775 377 775 380 775 379 776 380 776 381 776 381 777 380 777 248 777 326 778 382 778 523 778 523 779 382 779 441 779 1 780 3 780 383 780 383 781 3 781 384 781 383 782 384 782 520 782 520 783 384 783 4 783 385 784 386 784 135 784 135 785 386 785 137 785 135 786 137 786 388 786 388 787 137 787 387 787 388 788 387 788 133 788 133 789 387 789 139 789 133 790 139 790 132 790 132 791 139 791 389 791 132 792 389 792 131 792 131 793 389 793 140 793 131 794 140 794 390 794 390 795 140 795 391 795 390 796 391 796 130 796 130 797 391 797 392 797 130 798 392 798 129 798 129 799 392 799 393 799 129 800 393 800 128 800 128 801 393 801 394 801 128 802 394 802 395 802 395 803 394 803 145 803 395 804 145 804 126 804 126 805 145 805 396 805 126 806 396 806 123 806 123 807 396 807 398 807 123 808 398 808 397 808 397 809 398 809 399 809 397 810 399 810 121 810 121 811 399 811 148 811 121 812 148 812 119 812 119 813 148 813 149 813 119 814 149 814 400 814 400 815 149 815 152 815 400 816 152 816 401 816 401 817 152 817 153 817 401 818 153 818 116 818 116 819 153 819 402 819 116 820 402 820 114 820 114 821 402 821 403 821 114 822 403 822 113 822 113 823 403 823 156 823 113 824 156 824 404 824 404 825 156 825 405 825 404 826 405 826 112 826 112 827 405 827 158 827 112 828 158 828 406 828 406 829 158 829 407 829 406 830 407 830 111 830 111 831 407 831 408 831 111 832 408 832 109 832 109 833 408 833 161 833 109 834 161 834 108 834 108 835 161 835 409 835 108 836 409 836 107 836 107 837 409 837 410 837 107 838 410 838 411 838 411 839 410 839 163 839 411 840 163 840 104 840 104 841 163 841 166 841 104 842 166 842 412 842 412 843 166 843 168 843 412 844 168 844 102 844 102 845 168 845 414 845 102 846 414 846 413 846 413 847 414 847 169 847 413 848 169 848 415 848 415 849 169 849 172 849 415 850 172 850 416 850 416 851 172 851 173 851 416 852 173 852 98 852 98 853 173 853 417 853 98 854 417 854 96 854 96 855 417 855 418 855 96 856 418 856 95 856 95 857 418 857 419 857 95 858 419 858 420 858 420 859 419 859 421 859 420 860 421 860 92 860 92 861 421 861 422 861 92 862 422 862 91 862 91 863 422 863 423 863 91 864 423 864 424 864 424 865 423 865 425 865 424 866 425 866 90 866 90 867 425 867 175 867 90 868 175 868 426 868 426 869 175 869 176 869 426 870 176 870 428 870 428 871 176 871 427 871 428 872 427 872 429 872 429 873 427 873 430 873 429 874 430 874 85 874 85 875 430 875 431 875 85 876 431 876 432 876 432 877 431 877 433 877 432 878 433 878 434 878 434 879 433 879 178 879 434 880 178 880 436 880 436 881 178 881 435 881 436 882 435 882 84 882 84 883 435 883 437 883 84 884 437 884 81 884 81 885 437 885 439 885 81 886 439 886 438 886 438 887 439 887 440 887 438 888 440 888 441 888 441 889 440 889 442 889 441 890 442 890 523 890 523 891 442 891 182 891 523 892 182 892 522 892 522 893 182 893 444 893 522 894 444 894 443 894 443 895 444 895 445 895 443 896 445 896 446 896 446 897 445 897 184 897 446 898 184 898 447 898 447 899 184 899 185 899 447 900 185 900 80 900 80 901 185 901 187 901 80 902 187 902 77 902 77 903 187 903 448 903 77 904 448 904 76 904 76 905 448 905 188 905 76 906 188 906 449 906 449 907 188 907 190 907 449 908 190 908 74 908 74 909 190 909 450 909 74 910 450 910 71 910 71 911 450 911 192 911 71 912 192 912 70 912 70 913 192 913 194 913 70 914 194 914 67 914 67 915 194 915 451 915 67 916 451 916 65 916 65 917 451 917 196 917 65 918 196 918 452 918 452 919 196 919 453 919 452 920 453 920 454 920 454 921 453 921 197 921 454 922 197 922 455 922 455 923 197 923 456 923 455 924 456 924 62 924 62 925 456 925 457 925 62 926 457 926 61 926 61 927 457 927 199 927 61 928 199 928 458 928 458 929 199 929 459 929 458 930 459 930 59 930 59 931 459 931 461 931 59 932 461 932 460 932 460 933 461 933 462 933 460 934 462 934 463 934 463 935 462 935 464 935 463 936 464 936 466 936 466 937 464 937 465 937 466 938 465 938 56 938 56 939 465 939 202 939 56 940 202 940 55 940 55 941 202 941 204 941 55 942 204 942 467 942 467 943 204 943 206 943 467 944 206 944 52 944 52 945 206 945 207 945 52 946 207 946 51 946 51 947 207 947 469 947 51 948 469 948 468 948 468 949 469 949 210 949 468 950 210 950 47 950 47 951 210 951 470 951 47 952 470 952 471 952 471 953 470 953 472 953 471 954 472 954 45 954 45 955 472 955 212 955 45 956 212 956 474 956 474 957 212 957 473 957 474 958 473 958 475 958 475 959 473 959 477 959 475 960 477 960 476 960 476 961 477 961 478 961 476 962 478 962 43 962 43 963 478 963 480 963 43 964 480 964 479 964 479 965 480 965 481 965 479 966 481 966 483 966 483 967 481 967 482 967 483 968 482 968 40 968 40 969 482 969 217 969 40 970 217 970 39 970 39 971 217 971 484 971 39 972 484 972 485 972 485 973 484 973 220 973 485 974 220 974 486 974 486 975 220 975 487 975 486 976 487 976 38 976 38 977 487 977 221 977 38 978 221 978 36 978 36 979 221 979 222 979 36 980 222 980 33 980 33 981 222 981 488 981 33 982 488 982 32 982 32 983 488 983 223 983 32 984 223 984 31 984 31 985 223 985 224 985 31 986 224 986 489 986 489 987 224 987 490 987 489 988 490 988 491 988 491 989 490 989 227 989 491 990 227 990 493 990 493 991 227 991 492 991 493 992 492 992 494 992 494 993 492 993 495 993 494 994 495 994 496 994 496 995 495 995 497 995 496 996 497 996 26 996 26 997 497 997 230 997 26 998 230 998 498 998 498 999 230 999 231 999 498 1000 231 1000 25 1000 25 1001 231 1001 499 1001 25 1002 499 1002 500 1002 500 1003 499 1003 501 1003 500 1004 501 1004 23 1004 23 1005 501 1005 233 1005 23 1006 233 1006 22 1006 22 1007 233 1007 502 1007 22 1008 502 1008 21 1008 21 1009 502 1009 503 1009 21 1010 503 1010 19 1010 19 1011 503 1011 236 1011 19 1012 236 1012 504 1012 504 1013 236 1013 505 1013 504 1014 505 1014 16 1014 16 1015 505 1015 506 1015 16 1016 506 1016 14 1016 14 1017 506 1017 507 1017 14 1018 507 1018 12 1018 12 1019 507 1019 508 1019 12 1020 508 1020 11 1020 11 1021 508 1021 509 1021 11 1022 509 1022 510 1022 510 1023 509 1023 238 1023 510 1024 238 1024 10 1024 10 1025 238 1025 512 1025 10 1026 512 1026 511 1026 511 1027 512 1027 241 1027 511 1028 241 1028 8 1028 8 1029 241 1029 513 1029 8 1030 513 1030 514 1030 514 1031 513 1031 515 1031 514 1032 515 1032 516 1032 516 1033 515 1033 517 1033 516 1034 517 1034 518 1034 518 1035 517 1035 519 1035 518 1036 519 1036 5 1036 5 1037 519 1037 244 1037 5 1038 244 1038 4 1038 4 1039 244 1039 520 1039 324 1040 521 1040 443 1040 443 1041 521 1041 522 1041 521 1042 326 1042 522 1042 522 1043 326 1043 523 1043

45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /agvs_gazebo/models/agvs_floor_1/model-1_2.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 0 0 0 0 0 0 6 | 7 | 8 | 9 | 10 | model://agvs_floor_1/meshes/floor_1.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://agvs_floor_1/meshes/floor_1.dae 18 | 19 | 20 | false 21 | 22 | 23 | 24 | 25 | 26 | 27 | model://agvs_floor_1/meshes/floor_magnets_1.dae 28 | 29 | 30 | false 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /agvs_gazebo/models/agvs_floor_1/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 0 0 0 0 0 0 6 | 7 | 8 | 9 | 10 | model://agvs_floor_1/meshes/floor_1.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://agvs_floor_1/meshes/floor_1.dae 18 | 19 | 20 | false 21 | 22 | 23 | 24 | 25 | 26 | 27 | model://agvs_floor_1/meshes/floor_magnets_1.dae 28 | 29 | 30 | false 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /agvs_gazebo/models/agvs_floor_1/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 0 0 0 0 0 0 6 | 7 | 8 | 9 | 10 | model://agvs_floor_1/meshes/floor_1.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://agvs_floor_1/meshes/floor_1.dae 18 | 19 | 20 | false 21 | 22 | 23 | 24 | 25 | 26 | 27 | model://agvs_floor_1/meshes/floor_magnets_1.dae 28 | 29 | 30 | false 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /agvs_gazebo/models/agvs_floor_1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | AGVS Floor 1 5 | 1.0 6 | model-1_2.sdf 7 | model-1_3.sdf 8 | model-1_4.sdf 9 | model.sdf 10 | 11 | 12 | Robotnik 13 | rnavarro@robotnik.es 14 | 15 | 16 | 17 | Work area example of the AGVS robot 18 | 19 | 20 | -------------------------------------------------------------------------------- /agvs_gazebo/models/agvs_floor_1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 0 0 0 0 0 0 6 | 7 | 8 | 9 | 10 | model://agvs_floor_1/meshes/floor_1.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://agvs_floor_1/meshes/floor_1.dae 18 | 19 | 20 | false 21 | 22 | 23 | 24 | 25 | 26 | 27 | model://agvs_floor_1/meshes/floor_magnets_1.dae 28 | 29 | 30 | false 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /agvs_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | agvs_gazebo 4 | 0.1.1 5 | The agvs_gazebo package. Launch files and worlds to run Gazebo. 6 | 7 | 8 | Roberto Guzmán 9 | Román Navarro 10 | 11 | BSD 12 | 13 | http://wiki.ros.org/agvs_gazebo 14 | https://github.com/RobotnikAutomation/agvs_sim 15 | https://github.com/RobotnikAutomation/agvs_sim/issues 16 | 17 | Roberto Guzmán 18 | 19 | 20 | 21 | catkin 22 | agvs_description 23 | agvs_pad 24 | agvs_robot_control 25 | gazebo_ros 26 | roscpp 27 | std_msgs 28 | std_srvs 29 | tf 30 | joint_state_controller 31 | velocity_controllers 32 | effort_controllers 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /agvs_gazebo/worlds/agvs.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 -9.8 6 | 7 | 8 | quick 9 | 20 10 | 1.0 11 | 12 | 13 | 0.0 14 | 0.2 15 | 100.0 16 | 0.001 17 | 18 | 19 | 0.001 20 | 21 | 22 | 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 23 | 24 | 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 25 | 26 | 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 27 | 28 | 29 | 0.000000 0.000000 1.000000 30 | 200 200 31 | 32 | 33 | 34 | 35 | 36 | 50.000000 37 | 50.000000 38 | 0.000000 0.000000 0.000000 39 | 0.000000 40 | 0.000000 41 | 42 | 43 | 44 | 0.000000 45 | 100000.000000 46 | 47 | 48 | 49 | 0.000000 50 | 0.200000 51 | 1000000000.000000 52 | 1.000000 53 | 100.000000 54 | 0.001000 55 | 56 | 57 | 58 | 0.000000 59 | 60 | 1 61 | 0 62 | 0 63 | 64 | 1 65 | 66 | 67 | 68 | model://sun 69 | 70 | 71 | 74 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /agvs_gazebo/worlds/agvs_floor_1.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 10 39 | 40 | 41 | 0 42 | 43 | 44 | 0 0 1 45 | 100 100 46 | 47 | 48 | 49 | 53 | 54 | 55 | 56 | 0 57 | 0 58 | 59 | 0 60 | 0 61 | 1 62 | 63 | 64 | 65 | 0.001 66 | 1 67 | 1000 68 | 0 0 -9.8 69 | 70 | 71 | 0.4 0.4 0.4 1 72 | 0.7 0.7 0.7 1 73 | 1 74 | 75 | 76 | EARTH_WGS84 77 | 0 78 | 0 79 | 0 80 | 0 81 | 82 | 83 | 1 84 | -1 -3.55049 0 0 -0 0 85 | 86 | 87 | 88 | 89 | model://agvs_floor_1/meshes/floor_1.dae 90 | 91 | 92 | 10 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | model://agvs_floor_1/meshes/floor_1.dae 107 | 108 | 109 | 0 110 | 111 | 112 | 0 113 | 0 114 | 115 | 0 116 | 0 117 | 1 118 | 119 | 120 | 121 | 122 | 123 | model://agvs_floor_1/meshes/floor_magnets_1.dae 124 | 125 | 126 | 0 127 | 128 | 129 | 0 130 | 0 131 | 132 | 0 133 | 0 134 | 1 135 | 136 | 137 | 138 | 17 365000000 139 | 17 525304364 140 | 1467628628 172137528 141 | 142 | -1 -3.55049 0 0 -0 0 143 | 144 | -1 -3.55049 0 0 -0 0 145 | 0 0 0 0 -0 0 146 | 0 0 0 0 -0 0 147 | 0 0 0 0 -0 0 148 | 149 | 150 | -1 -3.55049 0 0 -0 0 151 | 0 0 0 0 -0 0 152 | 0 0 0 0 -0 0 153 | 0 0 0 0 -0 0 154 | 155 | 156 | 157 | 0 0 0 0 -0 0 158 | 159 | 0 0 0 0 -0 0 160 | 0 0 0 0 -0 0 161 | 0 0 0 0 -0 0 162 | 0 0 0 0 -0 0 163 | 164 | 165 | 166 | 167 | 168 | 12.8392 -7.43319 4.89587 0 0.275643 2.35619 169 | orbit 170 | 171 | 172 | 173 | 174 | -------------------------------------------------------------------------------- /agvs_gazebo/worlds/agvs_office.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 -9.8 6 | 7 | 8 | quick 9 | 20 10 | 1.0 11 | 12 | 13 | 0.0 14 | 0.2 15 | 100.0 16 | 0.001 17 | 18 | 19 | 0.001 20 | 21 | 22 | 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 23 | 24 | 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 25 | 26 | 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 27 | 28 | 29 | 0 0 1 30 | 400 400 31 | 32 | 33 | 34 | 35 | 36 | 50.000000 37 | 50.000000 38 | 0.000000 0.000000 0.000000 39 | 0.000000 40 | 0.000000 41 | 42 | 43 | 44 | 0.000000 45 | 100000.000000 46 | 47 | 48 | 49 | 0.000000 50 | 0.200000 51 | 1000000000.000000 52 | 1.000000 53 | 100.000000 54 | 0.001000 55 | 56 | 57 | 58 | 0.000000 59 | 60 | 61 | false 62 | 63 | 64 | 0 0 1 65 | 400 400 66 | 67 | 68 | 69 | 73 | 74 | 75 | 1 76 | 0 77 | 0 78 | 79 | 1 80 | 81 | 82 | 83 | model://sun 84 | 85 | 86 | 87 | model://willowgarage 88 | -16 -24 -0.05 0 0 0 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /agvs_robot_control/.rosinstall: -------------------------------------------------------------------------------- 1 | # Robotnik stacks 2 | 3 | - git: {local-name: robotnik_msgs, version: master, uri: 'https://github.com/RobotnikAutomation/robotnik_msgs.git'} 4 | -------------------------------------------------------------------------------- /agvs_robot_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package agvs_robot_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | 8 | 9 | 0.1.1 (2015-07-29) 10 | ------------------ 11 | * indigo-0.1.1 12 | * Redoing CHANGELOGS 13 | * agvs_robot_control: Adding build and run dependencies 14 | * Adding changelogs for the release 15 | * Adding the install macro to the CMakelists 16 | * Removing old msgs for AckermanDrive 17 | * Cleaning CMakelists and more 18 | * First indigo version commit 19 | * Contributors: Elena Gambaro, RomanRobotnik 20 | -------------------------------------------------------------------------------- /agvs_robot_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(agvs_robot_control) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | geometry_msgs 7 | robotnik_msgs 8 | roscpp 9 | sensor_msgs 10 | std_srvs 11 | std_msgs 12 | tf 13 | ackermann_msgs 14 | nav_msgs 15 | diagnostic_msgs 16 | diagnostic_updater 17 | ) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS 21 | roscpp 22 | robotnik_msgs 23 | sensor_msgs 24 | std_srvs 25 | std_msgs 26 | tf 27 | ackermann_msgs 28 | nav_msgs 29 | geometry_msgs 30 | diagnostic_msgs 31 | diagnostic_updater 32 | ) 33 | 34 | include_directories( 35 | ${catkin_INCLUDE_DIRS} 36 | ) 37 | 38 | add_executable(agvs_robot_control src/agvs_robot_control.cpp) 39 | add_dependencies(agvs_robot_control robotnik_msgs_generate_messages_cpp) 40 | 41 | target_link_libraries(agvs_robot_control 42 | ${catkin_LIBRARIES} 43 | ) 44 | 45 | install( 46 | TARGETS agvs_robot_control 47 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 48 | ) 49 | -------------------------------------------------------------------------------- /agvs_robot_control/launch/agvs_robot_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /agvs_robot_control/launch/agvs_robot_control.yaml: -------------------------------------------------------------------------------- 1 | agvs_robot_control: 2 | 3 | model: agvs 4 | 5 | # kinematic parameters 6 | agvs_wheel_diameter: 0.2195 7 | agvs_dist_to_center: 0.479 8 | 9 | publish_odom_tf: true 10 | imu_topic: /agvs/imu_data 11 | -------------------------------------------------------------------------------- /agvs_robot_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | agvs_robot_control 4 | 0.1.1 5 | The agvs_robot_control package. Robot controller that interacts with Gazebo motor controllers. 6 | 7 | Roberto Guzmán 8 | Román Navarro 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/agvs_robot_control 13 | https://github.com/RobotnikAutomation/agvs_sim 14 | https://github.com/RobotnikAutomation/agvs_sim/issues 15 | 16 | 17 | Roberto Guzmán 18 | 19 | 20 | catkin 21 | robotnik_msgs 22 | roscpp 23 | sensor_msgs 24 | std_srvs 25 | std_msgs 26 | tf 27 | message_generation 28 | ackermann_msgs 29 | nav_msgs 30 | geometry_msgs 31 | diagnostic_msgs 32 | diagnostic_updater 33 | robotnik_msgs 34 | roscpp 35 | sensor_msgs 36 | std_srvs 37 | std_msgs 38 | tf 39 | message_runtime 40 | ackermann_msgs 41 | nav_msgs 42 | geometry_msgs 43 | diagnostic_msgs 44 | diagnostic_updater 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /agvs_robot_control/src/agvs_robot_control.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * agvs_robot_control 3 | * Copyright (c) 2014, Robotnik Automation, SLL 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the Robotnik Automation, SLL. nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * \author Robotnik 31 | * \brief Controller for the AGVS robot Ackerman Drive 32 | * \brief (will include dual odometry measurement) 33 | */ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include "ackermann_msgs/AckermannDriveStamped.h" 48 | 49 | #include "diagnostic_msgs/DiagnosticStatus.h" 50 | #include "diagnostic_updater/diagnostic_updater.h" 51 | #include "diagnostic_updater/update_functions.h" 52 | #include "diagnostic_updater/DiagnosticStatusWrapper.h" 53 | #include "diagnostic_updater/publisher.h" 54 | #include 55 | 56 | 57 | #define PI 3.1415926535 58 | #define AGVS_MIN_COMMAND_REC_FREQ 5.0 59 | #define AGVS_MAX_COMMAND_REC_FREQ 150.0 60 | 61 | #define AGVS_WHEEL_DIAMETER 0.2195 // Default wheel diameter 62 | #define DEFAULT_DIST_CENTER_TO_WHEEL 0.479 // Default distance center to motorwheel 63 | 64 | #define MAX_ELEVATOR_POSITION 0.05 // meters 65 | 66 | using namespace std; 67 | 68 | class AGVSControllerClass { 69 | 70 | public: 71 | 72 | ros::NodeHandle node_handle_; 73 | ros::NodeHandle private_node_handle_; 74 | double desired_freq_; 75 | 76 | // Diagnostics 77 | diagnostic_updater::Updater diagnostic_; // General status diagnostic updater 78 | diagnostic_updater::FrequencyStatus freq_diag_; // Component frequency diagnostics 79 | diagnostic_updater::HeaderlessTopicDiagnostic *subs_command_freq; // Topic reception frequency diagnostics 80 | ros::Time last_command_time_; // Last moment when the component received a command 81 | diagnostic_updater::FunctionDiagnosticTask command_freq_; 82 | 83 | // Robot model 84 | std::string robot_model_; 85 | 86 | // Velocity and position references to low level controllers 87 | ros::Publisher ref_vel_fwd_; 88 | ros::Publisher ref_vel_bwd_; 89 | ros::Publisher ref_pos_fwd_; 90 | ros::Publisher ref_pos_bwd_; 91 | ros::Publisher ref_pos_elevator_; 92 | 93 | // Joint states published by the joint_state_controller of the Controller Manager 94 | ros::Subscriber joint_state_sub_; 95 | 96 | // High level robot command 97 | ros::Subscriber cmd_sub_; 98 | 99 | // Services 100 | ros::ServiceServer srv_SetOdometry_; 101 | ros::ServiceServer srv_SetMode_; 102 | ros::ServiceServer srv_GetMode_; 103 | ros::ServiceServer srv_RaiseElevator_; 104 | ros::ServiceServer srv_LowerElevator_; 105 | 106 | // Topics - Ackerman - velocity 107 | std::string fwd_vel_topic_; 108 | std::string bwd_vel_topic_; 109 | 110 | // Joint names - Ackerman - velocity 111 | std::string joint_front_wheel; 112 | std::string joint_back_wheel; 113 | 114 | // Topics - Ackerman - position 115 | std::string fwd_pos_topic_; 116 | std::string bwd_pos_topic_; 117 | std::string elevator_pos_topic_; 118 | 119 | std::string imu_topic_; 120 | 121 | // Joint names - Ackerman - position 122 | std::string joint_front_motor_wheel; 123 | std::string joint_back_motor_wheel; 124 | 125 | // Indexes to joint_states 126 | int fwd_vel_, bwd_vel_; 127 | int fwd_pos_, bwd_pos_; 128 | 129 | // Robot Speeds 130 | double linearSpeedXMps_; 131 | double linearSpeedYMps_; 132 | double angularSpeedRads_; 133 | 134 | // Robot Positions 135 | double robot_pose_px_; 136 | double robot_pose_py_; 137 | double robot_pose_pa_; 138 | double robot_pose_vx_; 139 | double robot_pose_vy_; 140 | 141 | // Robot Joint States 142 | sensor_msgs::JointState joint_state_; 143 | 144 | // Command reference 145 | // geometry_msgs::Twist base_vel_msg_; 146 | ackermann_msgs::AckermannDriveStamped base_vel_msg_; 147 | 148 | // External references 149 | double v_ref_; 150 | double a_ref_; 151 | 152 | double v_mps_; // Measured real robot speed traction wheel speed 153 | 154 | // Flag to indicate if joint_state has been read 155 | bool read_state_; 156 | 157 | // Robot configuration parameters 158 | double agvs_wheel_diameter_; 159 | double agvs_dist_to_center_; 160 | 161 | // IMU values 162 | double ang_vel_x_; 163 | double ang_vel_y_; 164 | double ang_vel_z_; 165 | 166 | double lin_acc_x_; 167 | double lin_acc_y_; 168 | double lin_acc_z_; 169 | 170 | double orientation_diff_x_; 171 | double orientation_diff_y_; 172 | double orientation_diff_z_; 173 | double orientation_diff_w_; 174 | 175 | // Parameter that defines if odom tf is published or not 176 | bool publish_odom_tf_; 177 | 178 | ros::Subscriber imu_sub_; 179 | 180 | // Publisher for odom topic 181 | ros::Publisher odom_pub_; 182 | 183 | // Broadcaster for odom tf 184 | tf::TransformBroadcaster odom_broadcaster; 185 | 186 | 187 | /*! \fn AGVSControllerClass::AGVSControllerClass() 188 | * \brief Public constructor 189 | */ 190 | AGVSControllerClass(ros::NodeHandle h) : diagnostic_(), 191 | node_handle_(h), private_node_handle_("~"), 192 | desired_freq_(100.0), 193 | freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05) ), 194 | command_freq_("Command frequency check", boost::bind(&AGVSControllerClass::check_command_subscriber, this, _1)) 195 | { 196 | 197 | ROS_INFO("agvs_robot_control_node - Init "); 198 | 199 | ros::NodeHandle agvs_robot_control_node_handle(node_handle_, "agvs_robot_control"); 200 | 201 | // Get robot model from the parameters 202 | if (!private_node_handle_.getParam("model", robot_model_)) { 203 | ROS_ERROR("Robot model not defined."); 204 | exit(-1); 205 | } 206 | else ROS_INFO("Robot Model : %s", robot_model_.c_str()); 207 | 208 | // Ackerman configuration - topics (control actions) 209 | private_node_handle_.param("fwd_vel_topic", fwd_vel_topic_, "/agvs/joint_front_wheel_controller/command"); 210 | private_node_handle_.param("bwd_vel_topic", bwd_vel_topic_, "/agvs/joint_back_wheel_controller/command"); 211 | private_node_handle_.param("fwd_pos_topic", fwd_pos_topic_, "/agvs/joint_front_motor_wheel_controller/command"); 212 | private_node_handle_.param("bwd_pos_topic", bwd_pos_topic_, "/agvs/joint_back_motor_wheel_controller/command"); 213 | private_node_handle_.param("elevator_pos_topic", elevator_pos_topic_, "/agvs/joint_elevator_controller/command"); 214 | private_node_handle_.param("imu_topic", imu_topic_, "/agvs/imu_data"); 215 | 216 | // Ackerman configuration - joint names 217 | private_node_handle_.param("joint_front_wheel", joint_front_wheel, "joint_front_wheel"); 218 | private_node_handle_.param("joint_back_wheel", joint_back_wheel, "joint_back_wheel"); 219 | private_node_handle_.param("joint_front_motor_wheel", joint_front_motor_wheel, "joint_front_motor_wheel"); 220 | private_node_handle_.param("joint_back_motor_wheel", joint_back_motor_wheel, "joint_back_motor_wheel"); 221 | 222 | // Robot kinematic parameters 223 | if (!private_node_handle_.getParam("agvs_wheel_diameter", agvs_wheel_diameter_)) 224 | agvs_wheel_diameter_ = AGVS_WHEEL_DIAMETER; 225 | if (!private_node_handle_.getParam("agvs_dist_to_center", agvs_dist_to_center_)) 226 | agvs_dist_to_center_ = DEFAULT_DIST_CENTER_TO_WHEEL; 227 | //ROS_INFO("agvs_wheel_diameter_ = %5.2f", agvs_wheel_diameter_); 228 | //ROS_INFO("agvs_dist_to_center_ = %5.2f", agvs_dist_to_center_); 229 | 230 | private_node_handle_.param("publish_odom_tf", publish_odom_tf_, true); 231 | if (publish_odom_tf_) ROS_INFO("PUBLISHING odom->base_footprint tf"); 232 | else ROS_INFO("NOT PUBLISHING odom->base_footprint tf"); 233 | 234 | // Robot Speeds 235 | linearSpeedXMps_ = 0.0; 236 | linearSpeedYMps_ = 0.0; 237 | angularSpeedRads_ = 0.0; 238 | 239 | // Robot Positions 240 | robot_pose_px_ = 0.0; 241 | robot_pose_py_ = 0.0; 242 | robot_pose_pa_ = 0.0; 243 | robot_pose_vx_ = 0.0; 244 | robot_pose_vy_ = 0.0; 245 | 246 | // External speed references 247 | v_ref_ = 0.0; 248 | a_ref_ = 0.0; 249 | 250 | // Imu variables 251 | ang_vel_x_ = 0.0; ang_vel_y_ = 0.0; ang_vel_z_ = 0.0; 252 | lin_acc_x_ = 0.0; lin_acc_y_ = 0.0; lin_acc_z_ = 0.0; 253 | orientation_diff_x_ = 0.0; orientation_diff_y_ = 0.0; orientation_diff_z_ = 0.0; orientation_diff_w_ = 1.0; 254 | 255 | // Advertise services 256 | srv_SetOdometry_ = private_node_handle_.advertiseService("set_odometry", &AGVSControllerClass::srvCallback_SetOdometry, this); 257 | srv_RaiseElevator_ = private_node_handle_.advertiseService("raise_elevator", &AGVSControllerClass::srvCallback_RaiseElevator, this); 258 | srv_LowerElevator_ = private_node_handle_.advertiseService("lower_elevator", &AGVSControllerClass::srvCallback_LowerElevator, this); 259 | 260 | // Subscribe to joint states topic 261 | joint_state_sub_ = agvs_robot_control_node_handle.subscribe("/agvs/joint_states", 1, &AGVSControllerClass::jointStateCallback, this); 262 | //joint_state_sub_ = private_node_handle_.subscribe("joint_states", 1, &AGVSControllerClass::jointStateCallback, this); 263 | //joint_state_sub_ = summit_xl_robot_control_node_handle.subscribe("/summit_xl/joint_states", 1, &SummitXLControllerClass::jointStateCallback, this); 264 | 265 | 266 | // Subscribe to imu data 267 | // imu_sub_ = agvs_robot_control_node_handle.subscribe("/agvs/imu_data", 1, &AGVSControllerClass::imuCallback, this); 268 | imu_sub_ = private_node_handle_.subscribe(imu_topic_, 1, &AGVSControllerClass::imuCallback, this); 269 | 270 | // Adevertise reference topics for the controllers 271 | ref_vel_fwd_ = private_node_handle_.advertise( fwd_vel_topic_, 50); 272 | ref_vel_bwd_ = private_node_handle_.advertise( bwd_vel_topic_, 50); 273 | ref_pos_fwd_ = private_node_handle_.advertise( fwd_pos_topic_, 50); 274 | ref_pos_bwd_ = private_node_handle_.advertise( bwd_pos_topic_, 50); 275 | ref_pos_elevator_ = private_node_handle_.advertise( elevator_pos_topic_, 50); 276 | 277 | // Subscribe to command topic 278 | cmd_sub_ = private_node_handle_.subscribe("command", 1, &AGVSControllerClass::commandCallback, this ); 279 | 280 | // TODO odom topic as parameter 281 | // Publish odometry 282 | odom_pub_ = private_node_handle_.advertise("odom", 1000); 283 | 284 | // Component frequency diagnostics 285 | diagnostic_.setHardwareID("agvs_robot_control - simulation"); 286 | diagnostic_.add( freq_diag_ ); 287 | diagnostic_.add( command_freq_ ); 288 | 289 | // Topics freq control 290 | // For /agvs_robot_control/command 291 | double min_freq = AGVS_MIN_COMMAND_REC_FREQ; // If you update these values, the 292 | double max_freq = AGVS_MAX_COMMAND_REC_FREQ; // HeaderlessTopicDiagnostic will use the new values. 293 | subs_command_freq = new diagnostic_updater::HeaderlessTopicDiagnostic("/agvs_robot_control/command", diagnostic_, 294 | diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10)); 295 | subs_command_freq->addTask(&command_freq_); // Adding an additional task to the control 296 | 297 | // Flag to indicate joint_state has been read 298 | read_state_ = false; 299 | 300 | // Robot ackermann measured speed. 301 | v_mps_ = 0; 302 | } 303 | 304 | /// Controller startup in realtime 305 | int starting() 306 | { 307 | 308 | ROS_INFO("AGVSControllerClass::starting"); 309 | 310 | // name: ['joint_back_motor_wheel', 'joint_back_wheel', 'joint_front_motor_wheel', 'joint_front_wheel'] 311 | // position: [6.283185307179586, -3.14159, 6.283185307179586, -3.14159] 312 | // velocity: [nan, nan, nan, nan] 313 | 314 | // Initialize joint indexes according to joint names 315 | if (read_state_) { 316 | vector joint_names = joint_state_.name; 317 | fwd_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_front_wheel)) - joint_names.begin(); 318 | bwd_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_back_wheel)) - joint_names.begin(); 319 | fwd_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_front_motor_wheel)) - joint_names.begin(); 320 | bwd_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_back_motor_wheel)) - joint_names.begin(); 321 | return 0; 322 | } 323 | else return -1; 324 | } 325 | 326 | /*! \fn void UpdateOdometry() 327 | * \brief Updates the values of the odometry 328 | * Ackerman's odometry calculation (using motor speed and position of the motor direction) 329 | */ 330 | void UpdateOdometry(){ 331 | 332 | // TODO UpdateOdometry differential drive 333 | 334 | // double v_left_mps, v_right_mps; 335 | // Calculate its own velocities for realize the motor control 336 | // v_left_mps = ((joint_state_.velocity[blw_vel_] + joint_state_.velocity[flw_vel_]) / 2.0) * (summit_xl_wheel_diameter_ / 2.0); 337 | // v_right_mps = -((joint_state_.velocity[brw_vel_] + joint_state_.velocity[frw_vel_]) / 2.0) * (summit_xl_wheel_diameter_ / 2.0); 338 | // sign according to urdf (if wheel model is not symetric, should be inverted) 339 | // angularSpeedRads_ = (v_right_mps - v_left_mps) / summit_xl_d_tracks_m_; // rad/s 340 | 341 | 342 | double fBetaRads = 0.0; 343 | double v_mps = 0.0; 344 | 345 | // Compute Position 346 | double fSamplePeriod = 1.0 / desired_freq_; 347 | 348 | // Linear speed of each wheel 349 | double v_fwd, v_bwd; 350 | v_fwd = joint_state_.velocity[fwd_vel_] * (agvs_wheel_diameter_ / 2.0); 351 | v_bwd = joint_state_.velocity[bwd_vel_] * (agvs_wheel_diameter_ / 2.0); 352 | //ROS_INFO("v_fwd = %.3lf, v_bwd = %.3lf", v_fwd, v_bwd); 353 | v_mps = -(v_fwd + v_bwd) / 2.0; 354 | 355 | // Angle of fwd and bwd motorwheels 356 | double a_fwd, a_bwd; 357 | a_fwd = radnorm2( joint_state_.position[fwd_pos_] ); 358 | a_bwd = radnorm2( joint_state_.position[bwd_pos_] ); 359 | fBetaRads = a_fwd; // consider to get a mean, but both angles are antisimetric 360 | 361 | // Filter noise 362 | if(fabs(v_mps) < 0.00001) v_mps = 0.0; 363 | 364 | // Compute Odometry 365 | double w = (v_mps / agvs_dist_to_center_) * sin(fBetaRads); 366 | robot_pose_pa_ += w * fSamplePeriod; 367 | 368 | // normalize 369 | radnorm(&robot_pose_pa_); 370 | //ROS_INFO("Orientation = %.3lf", robot_pose_pa_); 371 | // Velocities 372 | robot_pose_vx_ = v_mps * cos(fBetaRads) * cos(robot_pose_pa_); 373 | robot_pose_vy_ = v_mps * cos(fBetaRads) * sin(robot_pose_pa_); 374 | 375 | // Positions 376 | robot_pose_px_ += robot_pose_vx_ * fSamplePeriod; 377 | robot_pose_py_ += robot_pose_vy_ * fSamplePeriod; 378 | } 379 | 380 | // Publish robot odometry tf and topic depending 381 | void PublishOdometry() 382 | { 383 | ros::Time current_time = ros::Time::now(); 384 | 385 | //first, we'll publish the transform over tf 386 | geometry_msgs::TransformStamped odom_trans; 387 | odom_trans.header.stamp = current_time; 388 | odom_trans.header.frame_id = "odom"; 389 | odom_trans.child_frame_id = "base_footprint"; 390 | 391 | odom_trans.transform.translation.x = robot_pose_px_; 392 | odom_trans.transform.translation.y = robot_pose_py_; 393 | odom_trans.transform.translation.z = 0.0; 394 | 395 | // Convert theta from yaw (rads) to quaternion. note that this is only 2D !!! 396 | double theta = robot_pose_pa_; 397 | geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw( theta ); 398 | odom_trans.transform.rotation.x = quat.x; 399 | odom_trans.transform.rotation.y = quat.y; 400 | odom_trans.transform.rotation.z = quat.z; 401 | odom_trans.transform.rotation.w = quat.w; 402 | 403 | 404 | // send the transform over /tf 405 | // activate / deactivate with param 406 | // this tf in needed when not using robot_pose_ekf 407 | if (publish_odom_tf_) odom_broadcaster.sendTransform(odom_trans); 408 | 409 | //next, we'll publish the odometry message over ROS 410 | nav_msgs::Odometry odom; 411 | odom.header.stamp = current_time; 412 | odom.header.frame_id = "odom"; 413 | 414 | //set the position 415 | // Position 416 | odom.pose.pose.position.x = robot_pose_px_; 417 | odom.pose.pose.position.y = robot_pose_py_; 418 | odom.pose.pose.position.z = 0.0; 419 | // Orientation 420 | odom.pose.pose.orientation.x = orientation_diff_x_; 421 | odom.pose.pose.orientation.y = orientation_diff_y_; 422 | odom.pose.pose.orientation.z = orientation_diff_z_; 423 | odom.pose.pose.orientation.w = orientation_diff_w_; 424 | 425 | // Pose covariance 426 | for(int i = 0; i < 6; i++) 427 | odom.pose.covariance[i*6+i] = 0.1; // test 0.001 428 | 429 | //set the velocity 430 | odom.child_frame_id = "base_footprint"; 431 | // Linear velocities 432 | odom.twist.twist.linear.x = robot_pose_vx_; 433 | odom.twist.twist.linear.y = robot_pose_vy_; 434 | odom.twist.twist.linear.z = 0.0; 435 | // Angular velocities 436 | odom.twist.twist.angular.x = ang_vel_x_; 437 | odom.twist.twist.angular.y = ang_vel_y_; 438 | odom.twist.twist.angular.z = ang_vel_z_; 439 | // Twist covariance 440 | for(int i = 0; i < 6; i++) 441 | odom.twist.covariance[6*i+i] = 0.1; // test 0.001 442 | 443 | //publish the message 444 | odom_pub_.publish(odom); 445 | } 446 | 447 | void UpdateControl() 448 | { 449 | // Ackerman reference messages 450 | std_msgs::Float64 vel_ref_msg; 451 | std_msgs::Float64 pos_ref_msg; 452 | static double ev_ant = 0.0; 453 | 454 | // Note that the controllers are not in speed mode, but in effort. Therefore the speed ref will be Nm. 455 | // Open loop - works well but there is some inertia in the whole system and just setting torque to 0 does not stop the robot. 456 | // vel_ref_msg.data = -v_ref_ * 20.0; 457 | 458 | // If using a JointEffortController Try a velocity control loop 459 | // After 1.9.2 VelocityControllers in agvs_control should work 460 | /* 461 | double ev = v_mps_ - v_ref_; 462 | double Kp = 30.0; 463 | double Kd = 30.0; 464 | vel_ref_msg.data = Kp * ev; //+ Kd * (ev - ev_ant); 465 | ev_ant = ev; 466 | */ 467 | 468 | // Reference for velocity controllers 469 | double Kp = 10.0; // ref is in [m/s] while VelocityController expects ? 470 | vel_ref_msg.data = -v_ref_ * Kp; 471 | 472 | pos_ref_msg.data = a_ref_; 473 | 474 | // Publish references 475 | ref_vel_fwd_.publish( vel_ref_msg ); 476 | ref_vel_bwd_.publish( vel_ref_msg ); 477 | 478 | ref_pos_fwd_.publish( pos_ref_msg ); 479 | pos_ref_msg.data = -a_ref_; // symetric angle 480 | ref_pos_bwd_.publish( pos_ref_msg ); 481 | } 482 | 483 | // Sets the motor position to the desired value 484 | void SetElevatorPosition(double val){ 485 | 486 | std_msgs::Float64 ref_msg; 487 | 488 | ref_msg.data = val; 489 | 490 | ref_pos_elevator_.publish( ref_msg ); 491 | } 492 | 493 | /// Controller stopping 494 | void stopping() 495 | {} 496 | 497 | /* 498 | * \brief Checks that the robot is receiving at a correct frequency the command messages. Diagnostics 499 | * 500 | */ 501 | void check_command_subscriber(diagnostic_updater::DiagnosticStatusWrapper &stat) 502 | { 503 | ros::Time current_time = ros::Time::now(); 504 | 505 | double diff = (current_time - last_command_time_).toSec(); 506 | 507 | if(diff > 1.0){ 508 | stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Topic is not receiving commands"); 509 | //ROS_INFO("check_command_subscriber: %lf seconds without commands", diff); 510 | // TODO: Set Speed References to 0 511 | }else{ 512 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Topic receiving commands"); 513 | } 514 | } 515 | 516 | // Service SetOdometry 517 | bool srvCallback_SetOdometry(robotnik_msgs::set_odometry::Request &request, robotnik_msgs::set_odometry::Response &response ) 518 | { 519 | // ROS_INFO("summit_xl_odometry::set_odometry: request -> x = %f, y = %f, a = %f", req.x, req.y, req.orientation); 520 | robot_pose_px_ = request.x; 521 | robot_pose_py_ = request.y; 522 | robot_pose_pa_ = request.orientation; 523 | 524 | response.ret = true; 525 | return true; 526 | } 527 | 528 | // Service Raise Elevator 529 | bool srvCallback_RaiseElevator(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response ) 530 | { 531 | 532 | SetElevatorPosition(MAX_ELEVATOR_POSITION); 533 | 534 | return true; 535 | } 536 | 537 | // Service Lower Elevator 538 | bool srvCallback_LowerElevator(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response ) 539 | { 540 | SetElevatorPosition(0.0); 541 | 542 | return true; 543 | } 544 | 545 | 546 | // Topic command 547 | void jointStateCallback(const sensor_msgs::JointStateConstPtr& msg) 548 | { 549 | joint_state_ = *msg; 550 | read_state_ = true; 551 | } 552 | 553 | // Topic command 554 | void commandCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg) 555 | { 556 | // Safety check 557 | last_command_time_ = ros::Time::now(); 558 | subs_command_freq->tick(); // For diagnostics 559 | 560 | double speed_limit = 2.0; // m/s 561 | double angle_limit = PI; 562 | v_ref_ = saturation(msg->drive.speed, -speed_limit, speed_limit); 563 | a_ref_ = saturation(msg->drive.steering_angle, -angle_limit, angle_limit); 564 | } 565 | 566 | // Imu callback 567 | void imuCallback( const sensor_msgs::Imu& imu_msg){ 568 | 569 | orientation_diff_x_ = imu_msg.orientation.x; 570 | orientation_diff_y_ = imu_msg.orientation.y; 571 | orientation_diff_z_ = imu_msg.orientation.z; 572 | orientation_diff_w_ = imu_msg.orientation.w; 573 | 574 | ang_vel_x_ = imu_msg.angular_velocity.x; 575 | ang_vel_y_ = imu_msg.angular_velocity.y; 576 | ang_vel_z_ = imu_msg.angular_velocity.z; 577 | 578 | lin_acc_x_ = imu_msg.linear_acceleration.x; 579 | lin_acc_y_ = imu_msg.linear_acceleration.y; 580 | lin_acc_z_ = imu_msg.linear_acceleration.z; 581 | } 582 | 583 | double saturation(double u, double min, double max) 584 | { 585 | if (u>max) u=max; 586 | if (u= (PI)) { 596 | *radians -= 2.0 * PI; 597 | } 598 | while (*radians <= (-PI)) { 599 | *radians += 2.0 * PI; 600 | } 601 | } 602 | 603 | static inline double radnorm2( double value ) 604 | { 605 | while (value > 2.0*PI) value -= 2.0*PI; 606 | while (value < -2.0*PI) value += 2.0*PI; 607 | return value; 608 | } 609 | 610 | bool spin() 611 | { 612 | ROS_INFO("agvs_robot_control::spin()"); 613 | ros::Rate r(desired_freq_); // 50.0 614 | 615 | while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown. 616 | { 617 | if (starting() == 0) 618 | { 619 | while(ros::ok() && node_handle_.ok()) { 620 | UpdateControl(); 621 | UpdateOdometry(); 622 | PublishOdometry(); 623 | diagnostic_.update(); 624 | ros::spinOnce(); 625 | r.sleep(); 626 | } 627 | ROS_INFO("END OF ros::ok() !!!"); 628 | } else { 629 | // No need for diagnostic here since a broadcast occurs in start 630 | // when there is an error. 631 | usleep(1000000); 632 | ros::spinOnce(); 633 | } 634 | } 635 | 636 | ROS_INFO("agvs_robot_control::spin() - end"); 637 | return true; 638 | } 639 | 640 | }; // Class AGVSControllerClass 641 | 642 | int main(int argc, char** argv) 643 | { 644 | ros::init(argc, argv, "agvs_robot_control"); 645 | 646 | ros::NodeHandle n; 647 | AGVSControllerClass sxlrc(n); 648 | 649 | sxlrc.spin(); 650 | 651 | return (0); 652 | 653 | 654 | } 655 | 656 | -------------------------------------------------------------------------------- /agvs_sim/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package agvs_sim 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * agvs_sim: adding metapackage config 8 | * indigo-0.1.1 9 | * Redoing CHANGELOGS 10 | 11 | 0.1.1 (2015-07-29) 12 | ------------------ 13 | * Fixing error name on dependency 14 | * Adding changelogs for the release 15 | * Update package.xml 16 | * Update package.xml 17 | * Update CMakeLists.txt 18 | * First indigo version commit 19 | * Contributors: Elena Gambaro, ElenaFG, RomanRobotnik 20 | -------------------------------------------------------------------------------- /agvs_sim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(agvs_sim) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /agvs_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | agvs_sim 4 | 0.1.1 5 | agvs Gazebo simulation packages 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/agvs_sim 10 | https://github.com/RobotnikAutomation/agvs_sim/issues 11 | 12 | Roberto Guzmán 13 | Román Navarro 14 | 15 | Roberto Guzmán 16 | Román Navarro 17 | 18 | catkin 19 | 20 | agvs_gazebo 21 | agvs_robot_control 22 | agvs_control 23 | agvs_sim_bringup 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /agvs_sim_bringup/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package agvs_sim_bringup 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * indigo-0.1.1 8 | * Redoing CHANGELOGS 9 | 10 | 11 | 0.1.1 (2015-07-29) 12 | ------------------ 13 | * Adding changelogs for the release 14 | * Adding the install macro to the CMakelists 15 | * Cleaning CMakelists and more 16 | * First indigo version commit 17 | * Contributors: Elena Gambaro, RomanRobotnik 18 | -------------------------------------------------------------------------------- /agvs_sim_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(agvs_sim_bringup) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | 11 | install(DIRECTORY maps 12 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 13 | -------------------------------------------------------------------------------- /agvs_sim_bringup/launch/agvs_gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /agvs_sim_bringup/launch/agvs_hector_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 51 | 52 | 53 | 54 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /agvs_sim_bringup/launch/amcl_diff.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /agvs_sim_bringup/launch/amcl_diff_2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /agvs_sim_bringup/launch/map_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /agvs_sim_bringup/launch/mapping.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 | Splitter Ratio: 0.5 10 | Tree Height: 774 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: LaserScan 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | back_motor_wheel: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | back_wheel: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | base_footprint: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | base_link: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | elevator_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | front_motor_wheel: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | front_wheel: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | imu_link: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | laser_back: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | laser_front: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | logo_link: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | Value: true 114 | side_wheel_left: 115 | Alpha: 1 116 | Show Axes: false 117 | Show Trail: false 118 | Value: true 119 | side_wheel_right: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | Name: RobotModel 125 | Robot Description: robot_description 126 | TF Prefix: "" 127 | Update Interval: 0 128 | Value: true 129 | Visual Enabled: true 130 | - Alpha: 1 131 | Autocompute Intensity Bounds: true 132 | Autocompute Value Bounds: 133 | Max Value: 10 134 | Min Value: -10 135 | Value: true 136 | Axis: Z 137 | Channel Name: intensity 138 | Class: rviz/LaserScan 139 | Color: 255; 255; 255 140 | Color Transformer: Intensity 141 | Decay Time: 0 142 | Enabled: true 143 | Invert Rainbow: false 144 | Max Color: 255; 255; 255 145 | Max Intensity: 4.15992e-38 146 | Min Color: 0; 0; 0 147 | Min Intensity: 4.15992e-38 148 | Name: LaserScan 149 | Position Transformer: XYZ 150 | Queue Size: 10 151 | Selectable: true 152 | Size (Pixels): 3 153 | Size (m): 0.01 154 | Style: Flat Squares 155 | Topic: /scan 156 | Unreliable: false 157 | Use Fixed Frame: true 158 | Use rainbow: true 159 | Value: true 160 | - Alpha: 0.7 161 | Class: rviz/Map 162 | Color Scheme: map 163 | Draw Behind: false 164 | Enabled: true 165 | Name: Map 166 | Topic: /map 167 | Unreliable: false 168 | Value: true 169 | - Arrow Length: 0.3 170 | Class: rviz/PoseArray 171 | Color: 255; 25; 0 172 | Enabled: true 173 | Name: PoseArray 174 | Topic: /particlecloud 175 | Unreliable: false 176 | Value: true 177 | Enabled: true 178 | Global Options: 179 | Background Color: 48; 48; 48 180 | Fixed Frame: map 181 | Frame Rate: 30 182 | Name: root 183 | Tools: 184 | - Class: rviz/Interact 185 | Hide Inactive Objects: true 186 | - Class: rviz/MoveCamera 187 | - Class: rviz/Select 188 | - Class: rviz/FocusCamera 189 | - Class: rviz/Measure 190 | - Class: rviz/SetInitialPose 191 | Topic: /initialpose 192 | - Class: rviz/SetGoal 193 | Topic: /move_base_simple/goal 194 | - Class: rviz/PublishPoint 195 | Single click: true 196 | Topic: /clicked_point 197 | Value: true 198 | Views: 199 | Current: 200 | Angle: 0 201 | Class: rviz/TopDownOrtho 202 | Enable Stereo Rendering: 203 | Stereo Eye Separation: 0.06 204 | Stereo Focal Distance: 1 205 | Swap Stereo Eyes: false 206 | Value: false 207 | Name: Current View 208 | Near Clip Distance: 0.01 209 | Scale: 30.9822 210 | Target Frame: 211 | Value: TopDownOrtho (rviz) 212 | X: 0 213 | Y: 0 214 | Saved: ~ 215 | Window Geometry: 216 | Displays: 217 | collapsed: false 218 | Height: 1056 219 | Hide Left Dock: false 220 | Hide Right Dock: false 221 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000395fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000395000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000395fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000395000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003ffc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 222 | Selection: 223 | collapsed: false 224 | Time: 225 | collapsed: false 226 | Tool Properties: 227 | collapsed: false 228 | Views: 229 | collapsed: false 230 | Width: 1855 231 | X: 1985 232 | Y: 24 233 | -------------------------------------------------------------------------------- /agvs_sim_bringup/launch/purepursuit_nav.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 | - /InteractiveMarkers1 10 | - /PoseArray1 11 | Splitter Ratio: 0.5 12 | Tree Height: 669 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LaserScan 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.03 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 | - Alpha: 1 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | back_motor_wheel: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | back_wheel: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | base_footprint: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | base_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | elevator_link: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | front_motor_wheel: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | front_wheel: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | imu_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | laser_back: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | laser_front: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | side_wheel_left: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Value: true 116 | side_wheel_right: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | Name: RobotModel 122 | Robot Description: robot_description 123 | TF Prefix: "" 124 | Update Interval: 0 125 | Value: true 126 | Visual Enabled: true 127 | - Alpha: 0.7 128 | Class: rviz/Map 129 | Color Scheme: map 130 | Draw Behind: false 131 | Enabled: true 132 | Name: Map 133 | Topic: /map 134 | Value: true 135 | - Alpha: 1 136 | Autocompute Intensity Bounds: true 137 | Autocompute Value Bounds: 138 | Max Value: 10 139 | Min Value: -10 140 | Value: true 141 | Axis: Z 142 | Channel Name: intensity 143 | Class: rviz/LaserScan 144 | Color: 255; 255; 255 145 | Color Transformer: Intensity 146 | Decay Time: 0 147 | Enabled: true 148 | Invert Rainbow: false 149 | Max Color: 255; 255; 255 150 | Max Intensity: 2.70451e-43 151 | Min Color: 0; 0; 0 152 | Min Intensity: 0 153 | Name: LaserScan 154 | Position Transformer: XYZ 155 | Queue Size: 10 156 | Selectable: true 157 | Size (Pixels): 3 158 | Size (m): 0.01 159 | Style: Flat Squares 160 | Topic: /scan 161 | Use Fixed Frame: true 162 | Use rainbow: true 163 | Value: true 164 | - Class: rviz/InteractiveMarkers 165 | Enable Transparency: true 166 | Enabled: true 167 | Name: InteractiveMarkers 168 | Show Axes: false 169 | Show Descriptions: true 170 | Show Visual Aids: false 171 | Update Topic: /path_marker/update 172 | Value: true 173 | - Arrow Length: 0.3 174 | Class: rviz/PoseArray 175 | Color: 255; 25; 0 176 | Enabled: true 177 | Name: PoseArray 178 | Topic: /particlecloud 179 | Value: true 180 | Enabled: true 181 | Global Options: 182 | Background Color: 48; 48; 48 183 | Fixed Frame: map 184 | Frame Rate: 30 185 | Name: root 186 | Tools: 187 | - Class: rviz/Interact 188 | Hide Inactive Objects: true 189 | - Class: rviz/MoveCamera 190 | - Class: rviz/Select 191 | - Class: rviz/FocusCamera 192 | - Class: rviz/Measure 193 | - Class: rviz/SetInitialPose 194 | Topic: /initialpose 195 | - Class: rviz/SetGoal 196 | Topic: /move_base_simple/goal 197 | - Class: rviz/PublishPoint 198 | Single click: true 199 | Topic: /clicked_point 200 | Value: true 201 | Views: 202 | Current: 203 | Class: rviz/Orbit 204 | Distance: 10.1054 205 | Enable Stereo Rendering: 206 | Stereo Eye Separation: 0.06 207 | Stereo Focal Distance: 1 208 | Swap Stereo Eyes: false 209 | Value: false 210 | Focal Point: 211 | X: 0 212 | Y: 0 213 | Z: 0 214 | Name: Current View 215 | Near Clip Distance: 0.01 216 | Pitch: 0.645398 217 | Target Frame: 218 | Value: Orbit (rviz) 219 | Yaw: 3.1954 220 | Saved: ~ 221 | Window Geometry: 222 | Displays: 223 | collapsed: false 224 | Height: 950 225 | Hide Left Dock: false 226 | Hide Right Dock: false 227 | QMainWindow State: 000000ff00000000fd00000004000000000000013c0000032cfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000032c000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000028f000000c50000000000000000000000010000010f0000032cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000032c000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006af0000003efc0100000002fb0000000800540069006d00650100000000000006af000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004580000032c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 228 | Selection: 229 | collapsed: false 230 | Time: 231 | collapsed: false 232 | Tool Properties: 233 | collapsed: false 234 | Views: 235 | collapsed: false 236 | Width: 1711 237 | X: 58 238 | Y: 60 239 | -------------------------------------------------------------------------------- /agvs_sim_bringup/launch/robot_pose_ekf.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 | -------------------------------------------------------------------------------- /agvs_sim_bringup/maps/agvs_floor_1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotnikAutomation/agvs_sim/446805025c739ce26c029cc7ab857993306381e5/agvs_sim_bringup/maps/agvs_floor_1.pgm -------------------------------------------------------------------------------- /agvs_sim_bringup/maps/agvs_floor_1.yaml: -------------------------------------------------------------------------------- 1 | image: agvs_floor_1.pgm 2 | resolution: 0.050000 3 | origin: [-100.000000, -100.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /agvs_sim_bringup/maps/willow.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotnikAutomation/agvs_sim/446805025c739ce26c029cc7ab857993306381e5/agvs_sim_bringup/maps/willow.pgm -------------------------------------------------------------------------------- /agvs_sim_bringup/maps/willow.yaml: -------------------------------------------------------------------------------- 1 | image: willow.pgm 2 | resolution: 0.050000 3 | origin: [-100.000000, -100.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /agvs_sim_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | agvs_sim_bringup 4 | 0.1.1 5 | The agvs_sim_bringup package. It contains multiple launch files to perform different tasks, from creating a map with gmapping to launching amcl. 6 | 7 | 8 | Roberto Guzmán 9 | Román Navarro 10 | 11 | BSD 12 | 13 | 14 | http://wiki.ros.org/agvs_sim_bringup 15 | https://github.com/RobotnikAutomation/agvs_sim 16 | https://github.com/RobotnikAutomation/agvs_sim/issues 17 | 18 | 19 | Román Navarro 20 | 21 | catkin 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /agvs_sim_bringup/scripts/save_map.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # parameter $1: name of the map 3 | 4 | rosrun map_server map_saver -f $1 5 | --------------------------------------------------------------------------------