├── LICENSE ├── README.md ├── a.gitignore ├── dashgo_bringup ├── CMakeLists.txt ├── config │ └── my_dashgo_params.yaml ├── launch │ ├── minimal.launch │ ├── odom_ekf.launch │ └── robot_pose_ekf.launch ├── nodes │ ├── dashgo_driver.py │ └── dashgo_driver.py_bak ├── package.xml ├── scripts │ ├── c.py │ ├── check_angular.py │ ├── check_linear.py │ ├── get_angular.py │ ├── get_angular_combined.py │ ├── get_angular_odom.py │ ├── move_base_square.py │ ├── nav_grid.py │ ├── nav_square.py │ ├── odom_ekf.py │ └── odom_out_back.py └── startup │ ├── dashgo-restart │ ├── dashgo-start │ ├── dashgo-stop │ ├── dashgo.service │ └── readme.txt ├── dashgo_description ├── CMakeLists.txt ├── launch │ └── dashgo_description.launch ├── package.xml └── urdf │ ├── dashgobase │ ├── base.urdf.xacro │ ├── dashgobase.xacro │ ├── materials.urdf.xacro │ └── torso.urdf.xacro │ └── readme.txt └── dashgo_nav ├── CMakeLists.txt ├── config ├── dashgo │ ├── base_local_planner_params.yaml │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ └── local_costmap_params.yaml ├── nav_obstacles_params.yaml └── nav_test_params.yaml ├── launch ├── move_base.launch ├── move_base_blank_map.launch └── move_base_obstacle.launch ├── maps ├── blank_map.pgm ├── blank_map.yaml ├── blank_map_with_obstacle.pgm ├── blank_map_with_obstacle.yaml ├── tb_condo_2.pgm ├── tb_condo_2.yaml ├── test_map.pgm └── test_map.yaml └── package.xml /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | EAI dashgo D1 2 | -------------------------------------------------------------------------------- /a.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *pyc 3 | *.swap 4 | # Object files 5 | *.o 6 | *.ko 7 | *.obj 8 | *.elf 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Libraries 15 | *.lib 16 | *.a 17 | *.la 18 | *.lo 19 | 20 | # Shared objects (inc. Windows DLLs) 21 | *.dll 22 | *.so 23 | *.so.* 24 | *.dylib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | *.i*86 31 | *.x86_64 32 | *.hex 33 | 34 | # Debug files 35 | *.dSYM/ 36 | *.su 37 | -------------------------------------------------------------------------------- /dashgo_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dashgo_bringup) 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 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES dashgo_bringup 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(dashgo_bringup 122 | # src/${PROJECT_NAME}/dashgo_bringup.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(dashgo_bringup ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(dashgo_bringup_node src/dashgo_bringup_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(dashgo_bringup_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(dashgo_bringup_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS dashgo_bringup dashgo_bringup_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_dashgo_bringup.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /dashgo_bringup/config/my_dashgo_params.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/dashgo 2 | baud: 115200 3 | timeout: 0.1 4 | rate: 50 5 | sensorstate_rate: 10 6 | 7 | use_base_controller: True 8 | base_controller_rate: 10 9 | 10 | # For a robot that uses base_footprint, change base_frame to base_footprint 11 | #base_frame: base_link 12 | base_frame: base_footprint 13 | 14 | # === Robot drivetrain parameters 15 | wheel_diameter: 0.1264 16 | wheel_track: 0.3550 17 | encoder_resolution: 1200 # from Pololu for 13*34*4 motors 18 | gear_reduction: 1.0 19 | motors_reversed: False 20 | 21 | # === PID parameters 22 | Kp: 50 23 | Kd: 20 24 | Ki: 0 25 | Ko: 50 26 | accel_limit: 1.0 27 | -------------------------------------------------------------------------------- /dashgo_bringup/launch/minimal.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /dashgo_bringup/launch/odom_ekf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /dashgo_bringup/launch/robot_pose_ekf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /dashgo_bringup/nodes/dashgo_driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | A ROS Node for the Arduino microcontroller 5 | 6 | Created for the Pi Robot Project: http://www.pirobot.org 7 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 8 | 9 | This program is free software; you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation; either version 2 of the License, or 12 | (at your option) any later version. 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details at: 18 | 19 | http://www.gnu.org/licenses/gpl.html 20 | """ 21 | 22 | import rospy 23 | from geometry_msgs.msg import Twist 24 | import os, time 25 | import thread 26 | 27 | from math import pi as PI, degrees, radians, sin, cos 28 | import os 29 | import time 30 | import sys, traceback 31 | from serial.serialutil import SerialException 32 | from serial import Serial 33 | 34 | import roslib 35 | 36 | from geometry_msgs.msg import Quaternion, Twist, Pose 37 | from nav_msgs.msg import Odometry 38 | from std_msgs.msg import Int16 39 | from tf.broadcaster import TransformBroadcaster 40 | 41 | ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 42 | 0, 1e-3, 0, 0, 0, 0, 43 | 0, 0, 1e6, 0, 0, 0, 44 | 0, 0, 0, 1e6, 0, 0, 45 | 0, 0, 0, 0, 1e6, 0, 46 | 0, 0, 0, 0, 0, 1e3] 47 | ODOM_POSE_COVARIANCE2 = [1e-9, 0, 0, 0, 0, 0, 48 | 0, 1e-3, 1e-9, 0, 0, 0, 49 | 0, 0, 1e6, 0, 0, 0, 50 | 0, 0, 0, 1e6, 0, 0, 51 | 0, 0, 0, 0, 1e6, 0, 52 | 0, 0, 0, 0, 0, 1e-9] 53 | 54 | ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 55 | 0, 1e-3, 0, 0, 0, 0, 56 | 0, 0, 1e6, 0, 0, 0, 57 | 0, 0, 0, 1e6, 0, 0, 58 | 0, 0, 0, 0, 1e6, 0, 59 | 0, 0, 0, 0, 0, 1e3] 60 | ODOM_TWIST_COVARIANCE2 = [1e-9, 0, 0, 0, 0, 0, 61 | 0, 1e-3, 1e-9, 0, 0, 0, 62 | 0, 0, 1e6, 0, 0, 0, 63 | 0, 0, 0, 1e6, 0, 0, 64 | 0, 0, 0, 0, 1e6, 0, 65 | 0, 0, 0, 0, 0, 1e-9] 66 | 67 | 68 | SERVO_MAX = 180 69 | SERVO_MIN = 0 70 | 71 | class Arduino: 72 | ''' Configuration Parameters 73 | ''' 74 | N_ANALOG_PORTS = 6 75 | N_DIGITAL_PORTS = 12 76 | 77 | def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): 78 | 79 | self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller. 80 | self.PID_INTERVAL = 1000 / 30 81 | 82 | self.port = port 83 | self.baudrate = baudrate 84 | self.timeout = timeout 85 | self.encoder_count = 0 86 | self.writeTimeout = timeout 87 | self.interCharTimeout = timeout / 30. 88 | 89 | # Keep things thread safe 90 | self.mutex = thread.allocate_lock() 91 | 92 | # An array to cache analog sensor readings 93 | self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS 94 | 95 | # An array to cache digital sensor readings 96 | self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS 97 | 98 | def connect(self): 99 | try: 100 | print "Connecting to Arduino on port", self.port, "..." 101 | self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) 102 | # The next line is necessary to give the firmware time to wake up. 103 | time.sleep(1) 104 | test = self.get_baud() 105 | if test != self.baudrate: 106 | time.sleep(1) 107 | test = self.get_baud() 108 | if test != self.baudrate: 109 | raise SerialException 110 | print "Connected at", self.baudrate 111 | print "Arduino is ready." 112 | 113 | except SerialException: 114 | print "Serial Exception:" 115 | print sys.exc_info() 116 | print "Traceback follows:" 117 | traceback.print_exc(file=sys.stdout) 118 | print "Cannot connect to Arduino!" 119 | os._exit(1) 120 | 121 | def open(self): 122 | ''' Open the serial port. 123 | ''' 124 | self.port.open() 125 | 126 | def close(self): 127 | ''' Close the serial port. 128 | ''' 129 | self.port.close() 130 | 131 | def send(self, cmd): 132 | ''' This command should not be used on its own: it is called by the execute commands 133 | below in a thread safe manner. 134 | ''' 135 | self.port.write(cmd + '\r') 136 | 137 | def recv(self, timeout=0.5): 138 | timeout = min(timeout, self.timeout) 139 | ''' This command should not be used on its own: it is called by the execute commands 140 | below in a thread safe manner. Note: we use read() instead of readline() since 141 | readline() tends to return garbage characters from the Arduino 142 | ''' 143 | c = '' 144 | value = '' 145 | attempts = 0 146 | while c != '\r': 147 | c = self.port.read(1) 148 | value += c 149 | attempts += 1 150 | if attempts * self.interCharTimeout > timeout: 151 | return None 152 | 153 | value = value.strip('\r') 154 | 155 | return value 156 | 157 | def recv_ack(self): 158 | ''' This command should not be used on its own: it is called by the execute commands 159 | below in a thread safe manner. 160 | ''' 161 | ack = self.recv(self.timeout) 162 | return ack == 'OK' 163 | 164 | def recv_int(self): 165 | ''' This command should not be used on its own: it is called by the execute commands 166 | below in a thread safe manner. 167 | ''' 168 | value = self.recv(self.timeout) 169 | try: 170 | return int(value) 171 | except: 172 | return None 173 | 174 | def recv_array(self): 175 | ''' This command should not be used on its own: it is called by the execute commands 176 | below in a thread safe manner. 177 | ''' 178 | try: 179 | values = self.recv(self.timeout * self.N_ANALOG_PORTS).split() 180 | return map(int, values) 181 | except: 182 | return [] 183 | 184 | def execute(self, cmd): 185 | ''' Thread safe execution of "cmd" on the Arduino returning a single integer value. 186 | ''' 187 | self.mutex.acquire() 188 | 189 | try: 190 | self.port.flushInput() 191 | except: 192 | pass 193 | 194 | ntries = 1 195 | attempts = 0 196 | 197 | try: 198 | self.port.write(cmd + '\r') 199 | value = self.recv(self.timeout) 200 | while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): 201 | try: 202 | self.port.flushInput() 203 | self.port.write(cmd + '\r') 204 | value = self.recv(self.timeout) 205 | except: 206 | print "Exception executing command: " + cmd 207 | attempts += 1 208 | except: 209 | self.mutex.release() 210 | print "Exception executing command: " + cmd 211 | value = None 212 | 213 | self.mutex.release() 214 | return int(value) 215 | 216 | def execute_array(self, cmd): 217 | ''' Thread safe execution of "cmd" on the Arduino returning an array. 218 | ''' 219 | self.mutex.acquire() 220 | 221 | try: 222 | self.port.flushInput() 223 | except: 224 | pass 225 | 226 | ntries = 1 227 | attempts = 0 228 | 229 | try: 230 | self.port.write(cmd + '\r') 231 | values = self.recv_array() 232 | while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): 233 | try: 234 | self.port.flushInput() 235 | self.port.write(cmd + '\r') 236 | values = self.recv_array() 237 | except: 238 | print("Exception executing command: " + cmd) 239 | attempts += 1 240 | except: 241 | self.mutex.release() 242 | print "Exception executing command: " + cmd 243 | raise SerialException 244 | return [] 245 | 246 | try: 247 | values = map(int, values) 248 | except: 249 | values = [] 250 | 251 | self.mutex.release() 252 | return values 253 | 254 | def execute_ack(self, cmd): 255 | ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. 256 | ''' 257 | self.mutex.acquire() 258 | 259 | try: 260 | self.port.flushInput() 261 | except: 262 | pass 263 | 264 | ntries = 1 265 | attempts = 0 266 | 267 | try: 268 | self.port.write(cmd + '\r') 269 | ack = self.recv(self.timeout) 270 | while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): 271 | try: 272 | self.port.flushInput() 273 | self.port.write(cmd + '\r') 274 | ack = self.recv(self.timeout) 275 | except: 276 | print "Exception executing command: " + cmd 277 | attempts += 1 278 | except: 279 | self.mutex.release() 280 | print "execute_ack exception when executing", cmd 281 | print sys.exc_info() 282 | return 0 283 | 284 | self.mutex.release() 285 | return ack == 'OK' 286 | 287 | def update_pid(self, Kp, Kd, Ki, Ko): 288 | ''' Set the PID parameters on the Arduino 289 | ''' 290 | print "Updating PID parameters" 291 | cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko) 292 | self.execute_ack(cmd) 293 | 294 | def get_baud(self): 295 | ''' Get the current baud rate on the serial port. 296 | ''' 297 | return int(self.execute('b')); 298 | 299 | def get_encoder_counts(self): 300 | values = self.execute_array('e') 301 | if len(values) != 2: 302 | print "Encoder count was not 2" 303 | raise SerialException 304 | return None 305 | else: 306 | return values 307 | 308 | def reset_encoders(self): 309 | ''' Reset the encoder counts to 0 310 | ''' 311 | return self.execute_ack('r') 312 | 313 | def drive(self, right, left): 314 | ''' Speeds are given in encoder ticks per PID interval 315 | ''' 316 | return self.execute_ack('m %d %d' %(right, left)) 317 | 318 | def drive_m_per_s(self, right, left): 319 | ''' Set the motor speeds in meters per second. 320 | ''' 321 | left_revs_per_second = float(left) / (self.wheel_diameter * PI) 322 | right_revs_per_second = float(right) / (self.wheel_diameter * PI) 323 | 324 | left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) 325 | right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) 326 | 327 | self.drive(right_ticks_per_loop , left_ticks_per_loop ) 328 | 329 | def stop(self): 330 | ''' Stop both motors. 331 | ''' 332 | self.drive(0, 0) 333 | 334 | def ping(self, pin): 335 | ''' The srf05/Ping command queries an SRF05/Ping sonar sensor 336 | connected to the General Purpose I/O line pinId for a distance, 337 | and returns the range in cm. Sonar distance resolution is integer based. 338 | ''' 339 | return self.execute('p %d' %pin); 340 | 341 | def get_pidin(self): 342 | values = self.execute_array('i') 343 | if len(values) != 2: 344 | print "get_pidin count was not 2" 345 | raise SerialException 346 | return None 347 | else: 348 | return values 349 | 350 | def get_pidout(self): 351 | values = self.execute_array('f') 352 | if len(values) != 2: 353 | print "get_pidout count was not 2" 354 | raise SerialException 355 | return None 356 | else: 357 | return values 358 | 359 | 360 | """ Class to receive Twist commands and publish Odometry data """ 361 | class BaseController: 362 | def __init__(self, arduino, base_frame): 363 | self.arduino = arduino 364 | self.base_frame = base_frame 365 | self.rate = float(rospy.get_param("~base_controller_rate", 10)) 366 | self.timeout = rospy.get_param("~base_controller_timeout", 1.0) 367 | self.stopped = False 368 | 369 | pid_params = dict() 370 | pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 371 | pid_params['wheel_track'] = rospy.get_param("~wheel_track", "") 372 | pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 373 | pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0) 374 | pid_params['Kp'] = rospy.get_param("~Kp", 20) 375 | pid_params['Kd'] = rospy.get_param("~Kd", 12) 376 | pid_params['Ki'] = rospy.get_param("~Ki", 0) 377 | pid_params['Ko'] = rospy.get_param("~Ko", 50) 378 | 379 | self.accel_limit = rospy.get_param('~accel_limit', 0.1) 380 | self.motors_reversed = rospy.get_param("~motors_reversed", False) 381 | 382 | # Set up PID parameters and check for missing values 383 | self.setup_pid(pid_params) 384 | 385 | # How many encoder ticks are there per meter? 386 | self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * PI) 387 | 388 | # What is the maximum acceleration we will tolerate when changing wheel speeds? 389 | self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate 390 | 391 | # Track how often we get a bad encoder count (if any) 392 | self.bad_encoder_count = 0 393 | 394 | self.encoder_min = rospy.get_param('encoder_min', -32768) 395 | self.encoder_max = rospy.get_param('encoder_max', 32768) 396 | self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min ) 397 | self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min ) 398 | self.l_wheel_mult = 0 399 | self.r_wheel_mult = 0 400 | 401 | now = rospy.Time.now() 402 | self.then = now # time for determining dx/dy 403 | self.t_delta = rospy.Duration(1.0 / self.rate) 404 | self.t_next = now + self.t_delta 405 | 406 | # Internal data 407 | self.enc_left = None # encoder readings 408 | self.enc_right = None 409 | self.x = 0 # position in xy plane 410 | self.y = 0 411 | self.th = 0 # rotation in radians 412 | self.v_left = 0 413 | self.v_right = 0 414 | self.v_des_left = 0 # cmd_vel setpoint 415 | self.v_des_right = 0 416 | self.last_cmd_vel = now 417 | 418 | # Subscriptions 419 | rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback) 420 | 421 | # Clear any old odometry info 422 | self.arduino.reset_encoders() 423 | 424 | # Set up the odometry broadcaster 425 | self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5) 426 | self.odomBroadcaster = TransformBroadcaster() 427 | 428 | rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev") 429 | rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame") 430 | 431 | self.lEncoderPub = rospy.Publisher('Lencoder', Int16) 432 | self.rEncoderPub = rospy.Publisher('Rencoder', Int16) 433 | self.lPidoutPub = rospy.Publisher('Lpidout', Int16) 434 | self.rPidoutPub = rospy.Publisher('Rpidout', Int16) 435 | self.lVelPub = rospy.Publisher('Lvel', Int16) 436 | self.rVelPub = rospy.Publisher('Rvel', Int16) 437 | 438 | def setup_pid(self, pid_params): 439 | # Check to see if any PID parameters are missing 440 | missing_params = False 441 | for param in pid_params: 442 | if pid_params[param] == "": 443 | print("*** PID Parameter " + param + " is missing. ***") 444 | missing_params = True 445 | 446 | if missing_params: 447 | os._exit(1) 448 | 449 | self.wheel_diameter = pid_params['wheel_diameter'] 450 | self.wheel_track = pid_params['wheel_track'] 451 | self.encoder_resolution = pid_params['encoder_resolution'] 452 | self.gear_reduction = pid_params['gear_reduction'] 453 | 454 | self.Kp = pid_params['Kp'] 455 | self.Kd = pid_params['Kd'] 456 | self.Ki = pid_params['Ki'] 457 | self.Ko = pid_params['Ko'] 458 | 459 | self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko) 460 | 461 | def poll(self): 462 | now = rospy.Time.now() 463 | if now > self.t_next: 464 | try: 465 | left_pidin, right_pidin = self.arduino.get_pidin() 466 | except: 467 | rospy.logerr("getpidout exception count: ") 468 | return 469 | 470 | self.lEncoderPub.publish(left_pidin) 471 | self.rEncoderPub.publish(right_pidin) 472 | try: 473 | left_pidout, right_pidout = self.arduino.get_pidout() 474 | except: 475 | rospy.logerr("getpidout exception count: ") 476 | return 477 | self.lPidoutPub.publish(left_pidout) 478 | self.rPidoutPub.publish(right_pidout) 479 | # Read the encoders 480 | try: 481 | left_enc, right_enc = self.arduino.get_encoder_counts() 482 | #rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc)) 483 | except: 484 | self.bad_encoder_count += 1 485 | rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) 486 | return 487 | 488 | dt = now - self.then 489 | self.then = now 490 | dt = dt.to_sec() 491 | 492 | # Calculate odometry 493 | if self.enc_left == None: 494 | dright = 0 495 | dleft = 0 496 | else: 497 | if (left_enc < self.encoder_low_wrap and self.enc_left > self.encoder_high_wrap) : 498 | self.l_wheel_mult = self.l_wheel_mult + 1 499 | elif (left_enc > self.encoder_high_wrap and self.enc_left < self.encoder_low_wrap) : 500 | self.l_wheel_mult = self.l_wheel_mult - 1 501 | else: 502 | self.l_wheel_mult = 0 503 | if (right_enc < self.encoder_low_wrap and self.enc_right > self.encoder_high_wrap) : 504 | self.r_wheel_mult = self.r_wheel_mult + 1 505 | elif (right_enc > self.encoder_high_wrap and self.enc_right < self.encoder_low_wrap) : 506 | self.r_wheel_mult = self.r_wheel_mult - 1 507 | else: 508 | self.r_wheel_mult = 0 509 | #dright = (right_enc - self.enc_right) / self.ticks_per_meter 510 | #dleft = (left_enc - self.enc_left) / self.ticks_per_meter 511 | dleft = 1.0 * (left_enc + self.l_wheel_mult * (self.encoder_max - self.encoder_min)-self.enc_left) / self.ticks_per_meter 512 | dright = 1.0 * (right_enc + self.r_wheel_mult * (self.encoder_max - self.encoder_min)-self.enc_right) / self.ticks_per_meter 513 | 514 | self.enc_right = right_enc 515 | self.enc_left = left_enc 516 | 517 | dxy_ave = (dright + dleft) / 2.0 518 | dth = (dright - dleft) / self.wheel_track 519 | vxy = dxy_ave / dt 520 | vth = dth / dt 521 | 522 | if (dxy_ave != 0): 523 | dx = cos(dth) * dxy_ave 524 | dy = -sin(dth) * dxy_ave 525 | self.x += (cos(self.th) * dx - sin(self.th) * dy) 526 | self.y += (sin(self.th) * dx + cos(self.th) * dy) 527 | 528 | if (dth != 0): 529 | self.th += dth 530 | 531 | quaternion = Quaternion() 532 | quaternion.x = 0.0 533 | quaternion.y = 0.0 534 | quaternion.z = sin(self.th / 2.0) 535 | quaternion.w = cos(self.th / 2.0) 536 | 537 | # Create the odometry transform frame broadcaster. 538 | self.odomBroadcaster.sendTransform( 539 | (self.x, self.y, 0), 540 | (quaternion.x, quaternion.y, quaternion.z, quaternion.w), 541 | rospy.Time.now(), 542 | self.base_frame, 543 | "odom" 544 | ) 545 | 546 | odom = Odometry() 547 | odom.header.frame_id = "odom" 548 | odom.child_frame_id = self.base_frame 549 | odom.header.stamp = now 550 | odom.pose.pose.position.x = self.x 551 | odom.pose.pose.position.y = self.y 552 | odom.pose.pose.position.z = 0 553 | odom.pose.pose.orientation = quaternion 554 | odom.twist.twist.linear.x = vxy 555 | odom.twist.twist.linear.y = 0 556 | odom.twist.twist.angular.z = vth 557 | 558 | # todo sensor_state.distance == 0 559 | if self.v_des_left == 0 and self.v_des_right == 0: 560 | odom.pose.covariance = ODOM_POSE_COVARIANCE2 561 | odom.twist.covariance = ODOM_TWIST_COVARIANCE2 562 | else: 563 | odom.pose.covariance = ODOM_POSE_COVARIANCE 564 | odom.twist.covariance = ODOM_TWIST_COVARIANCE 565 | 566 | self.odomPub.publish(odom) 567 | 568 | if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): 569 | self.v_des_left = 0 570 | self.v_des_right = 0 571 | 572 | if self.v_left < self.v_des_left: 573 | self.v_left += self.max_accel 574 | if self.v_left > self.v_des_left: 575 | self.v_left = self.v_des_left 576 | else: 577 | self.v_left -= self.max_accel 578 | if self.v_left < self.v_des_left: 579 | self.v_left = self.v_des_left 580 | 581 | if self.v_right < self.v_des_right: 582 | self.v_right += self.max_accel 583 | if self.v_right > self.v_des_right: 584 | self.v_right = self.v_des_right 585 | else: 586 | self.v_right -= self.max_accel 587 | if self.v_right < self.v_des_right: 588 | self.v_right = self.v_des_right 589 | self.lVelPub.publish(self.v_left) 590 | self.rVelPub.publish(self.v_right) 591 | 592 | # Set motor speeds in encoder ticks per PID loop 593 | if not self.stopped: 594 | self.arduino.drive(self.v_left, self.v_right) 595 | 596 | self.t_next = now + self.t_delta 597 | 598 | def stop(self): 599 | self.stopped = True 600 | self.arduino.drive(0, 0) 601 | 602 | def cmdVelCallback(self, req): 603 | # Handle velocity-based movement requests 604 | self.last_cmd_vel = rospy.Time.now() 605 | 606 | x = req.linear.x # m/s 607 | th = req.angular.z # rad/s 608 | 609 | if x == 0: 610 | # Turn in place 611 | right = th * self.wheel_track * self.gear_reduction / 2.0 612 | left = -right 613 | elif th == 0: 614 | # Pure forward/backward motion 615 | left = right = x 616 | else: 617 | # Rotation about a point in space 618 | left = x - th * self.wheel_track * self.gear_reduction / 2.0 619 | right = x + th * self.wheel_track * self.gear_reduction / 2.0 620 | 621 | self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE) 622 | self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE) 623 | 624 | class ArduinoROS(): 625 | def __init__(self): 626 | rospy.init_node('Arduino', log_level=rospy.DEBUG) 627 | 628 | # Cleanup when termniating the node 629 | rospy.on_shutdown(self.shutdown) 630 | 631 | self.port = rospy.get_param("~port", "/dev/ttyACM0") 632 | self.baud = int(rospy.get_param("~baud", 57600)) 633 | self.timeout = rospy.get_param("~timeout", 0.5) 634 | self.base_frame = rospy.get_param("~base_frame", 'base_link') 635 | 636 | # Overall loop rate: should be faster than fastest sensor rate 637 | self.rate = int(rospy.get_param("~rate", 50)) 638 | r = rospy.Rate(self.rate) 639 | 640 | # Rate at which summary SensorState message is published. Individual sensors publish 641 | # at their own rates. 642 | self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) 643 | 644 | self.use_base_controller = rospy.get_param("~use_base_controller", False) 645 | 646 | # Set up the time for publishing the next SensorState message 647 | now = rospy.Time.now() 648 | self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate) 649 | self.t_next_sensors = now + self.t_delta_sensors 650 | 651 | # Initialize a Twist message 652 | self.cmd_vel = Twist() 653 | 654 | # A cmd_vel publisher so we can stop the robot when shutting down 655 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 656 | 657 | # Initialize the controlller 658 | self.controller = Arduino(self.port, self.baud, self.timeout) 659 | 660 | # Make the connection 661 | self.controller.connect() 662 | 663 | rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") 664 | 665 | # Reserve a thread lock 666 | mutex = thread.allocate_lock() 667 | 668 | # Initialize the base controller if used 669 | if self.use_base_controller: 670 | self.myBaseController = BaseController(self.controller, self.base_frame) 671 | 672 | # Start polling the sensors and base controller 673 | while not rospy.is_shutdown(): 674 | 675 | if self.use_base_controller: 676 | mutex.acquire() 677 | self.myBaseController.poll() 678 | mutex.release() 679 | r.sleep() 680 | 681 | def shutdown(self): 682 | # Stop the robot 683 | try: 684 | rospy.loginfo("Stopping the robot...") 685 | self.cmd_vel_pub.Publish(Twist()) 686 | rospy.sleep(2) 687 | except: 688 | pass 689 | rospy.loginfo("Shutting down Arduino Node...") 690 | 691 | if __name__ == '__main__': 692 | myArduino = ArduinoROS() 693 | 694 | -------------------------------------------------------------------------------- /dashgo_bringup/nodes/dashgo_driver.py_bak: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | A ROS Node for the Arduino microcontroller 5 | 6 | Created for the Pi Robot Project: http://www.pirobot.org 7 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 8 | 9 | This program is free software; you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation; either version 2 of the License, or 12 | (at your option) any later version. 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details at: 18 | 19 | http://www.gnu.org/licenses/gpl.html 20 | """ 21 | 22 | import rospy 23 | from geometry_msgs.msg import Twist 24 | import os, time 25 | import thread 26 | 27 | from math import pi as PI, degrees, radians, sin, cos 28 | import os 29 | import time 30 | import sys, traceback 31 | from serial.serialutil import SerialException 32 | from serial import Serial 33 | 34 | import roslib 35 | 36 | from geometry_msgs.msg import Quaternion, Twist, Pose 37 | from nav_msgs.msg import Odometry 38 | from std_msgs.msg import Int32 39 | from tf.broadcaster import TransformBroadcaster 40 | 41 | ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 42 | 0, 1e-3, 0, 0, 0, 0, 43 | 0, 0, 1e6, 0, 0, 0, 44 | 0, 0, 0, 1e6, 0, 0, 45 | 0, 0, 0, 0, 1e6, 0, 46 | 0, 0, 0, 0, 0, 1e3] 47 | ODOM_POSE_COVARIANCE2 = [1e-9, 0, 0, 0, 0, 0, 48 | 0, 1e-3, 1e-9, 0, 0, 0, 49 | 0, 0, 1e6, 0, 0, 0, 50 | 0, 0, 0, 1e6, 0, 0, 51 | 0, 0, 0, 0, 1e6, 0, 52 | 0, 0, 0, 0, 0, 1e-9] 53 | 54 | ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 55 | 0, 1e-3, 0, 0, 0, 0, 56 | 0, 0, 1e6, 0, 0, 0, 57 | 0, 0, 0, 1e6, 0, 0, 58 | 0, 0, 0, 0, 1e6, 0, 59 | 0, 0, 0, 0, 0, 1e3] 60 | ODOM_TWIST_COVARIANCE2 = [1e-9, 0, 0, 0, 0, 0, 61 | 0, 1e-3, 1e-9, 0, 0, 0, 62 | 0, 0, 1e6, 0, 0, 0, 63 | 0, 0, 0, 1e6, 0, 0, 64 | 0, 0, 0, 0, 1e6, 0, 65 | 0, 0, 0, 0, 0, 1e-9] 66 | 67 | 68 | SERVO_MAX = 180 69 | SERVO_MIN = 0 70 | 71 | class Arduino: 72 | ''' Configuration Parameters 73 | ''' 74 | N_ANALOG_PORTS = 6 75 | N_DIGITAL_PORTS = 12 76 | 77 | def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): 78 | 79 | self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller. 80 | self.PID_INTERVAL = 1000 / 30 81 | 82 | self.port = port 83 | self.baudrate = baudrate 84 | self.timeout = timeout 85 | self.encoder_count = 0 86 | self.writeTimeout = timeout 87 | self.interCharTimeout = timeout / 30. 88 | 89 | # Keep things thread safe 90 | self.mutex = thread.allocate_lock() 91 | 92 | # An array to cache analog sensor readings 93 | self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS 94 | 95 | # An array to cache digital sensor readings 96 | self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS 97 | 98 | def connect(self): 99 | try: 100 | print "Connecting to Arduino on port", self.port, "..." 101 | self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) 102 | # The next line is necessary to give the firmware time to wake up. 103 | time.sleep(1) 104 | test = self.get_baud() 105 | if test != self.baudrate: 106 | time.sleep(1) 107 | test = self.get_baud() 108 | if test != self.baudrate: 109 | raise SerialException 110 | print "Connected at", self.baudrate 111 | print "Arduino is ready." 112 | 113 | except SerialException: 114 | print "Serial Exception:" 115 | print sys.exc_info() 116 | print "Traceback follows:" 117 | traceback.print_exc(file=sys.stdout) 118 | print "Cannot connect to Arduino!" 119 | os._exit(1) 120 | 121 | def open(self): 122 | ''' Open the serial port. 123 | ''' 124 | self.port.open() 125 | 126 | def close(self): 127 | ''' Close the serial port. 128 | ''' 129 | self.port.close() 130 | 131 | def send(self, cmd): 132 | ''' This command should not be used on its own: it is called by the execute commands 133 | below in a thread safe manner. 134 | ''' 135 | self.port.write(cmd + '\r') 136 | 137 | def recv(self, timeout=0.5): 138 | timeout = min(timeout, self.timeout) 139 | ''' This command should not be used on its own: it is called by the execute commands 140 | below in a thread safe manner. Note: we use read() instead of readline() since 141 | readline() tends to return garbage characters from the Arduino 142 | ''' 143 | c = '' 144 | value = '' 145 | attempts = 0 146 | while c != '\r': 147 | c = self.port.read(1) 148 | value += c 149 | attempts += 1 150 | if attempts * self.interCharTimeout > timeout: 151 | return None 152 | 153 | value = value.strip('\r') 154 | 155 | return value 156 | 157 | def recv_ack(self): 158 | ''' This command should not be used on its own: it is called by the execute commands 159 | below in a thread safe manner. 160 | ''' 161 | ack = self.recv(self.timeout) 162 | return ack == 'OK' 163 | 164 | def recv_int(self): 165 | ''' This command should not be used on its own: it is called by the execute commands 166 | below in a thread safe manner. 167 | ''' 168 | value = self.recv(self.timeout) 169 | try: 170 | return int(value) 171 | except: 172 | return None 173 | 174 | def recv_array(self): 175 | ''' This command should not be used on its own: it is called by the execute commands 176 | below in a thread safe manner. 177 | ''' 178 | try: 179 | values = self.recv(self.timeout * self.N_ANALOG_PORTS).split() 180 | return map(int, values) 181 | except: 182 | return [] 183 | 184 | def execute(self, cmd): 185 | ''' Thread safe execution of "cmd" on the Arduino returning a single integer value. 186 | ''' 187 | self.mutex.acquire() 188 | 189 | try: 190 | self.port.flushInput() 191 | except: 192 | pass 193 | 194 | ntries = 1 195 | attempts = 0 196 | 197 | try: 198 | self.port.write(cmd + '\r') 199 | value = self.recv(self.timeout) 200 | while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): 201 | try: 202 | self.port.flushInput() 203 | self.port.write(cmd + '\r') 204 | value = self.recv(self.timeout) 205 | except: 206 | print "Exception executing command: " + cmd 207 | attempts += 1 208 | except: 209 | self.mutex.release() 210 | print "Exception executing command: " + cmd 211 | value = None 212 | 213 | self.mutex.release() 214 | return int(value) 215 | 216 | def execute_array(self, cmd): 217 | ''' Thread safe execution of "cmd" on the Arduino returning an array. 218 | ''' 219 | self.mutex.acquire() 220 | 221 | try: 222 | self.port.flushInput() 223 | except: 224 | pass 225 | 226 | ntries = 1 227 | attempts = 0 228 | 229 | try: 230 | self.port.write(cmd + '\r') 231 | values = self.recv_array() 232 | while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): 233 | try: 234 | self.port.flushInput() 235 | self.port.write(cmd + '\r') 236 | values = self.recv_array() 237 | except: 238 | print("Exception executing command: " + cmd) 239 | attempts += 1 240 | except: 241 | self.mutex.release() 242 | print "Exception executing command: " + cmd 243 | raise SerialException 244 | return [] 245 | 246 | try: 247 | values = map(int, values) 248 | except: 249 | values = [] 250 | 251 | self.mutex.release() 252 | return values 253 | 254 | def execute_ack(self, cmd): 255 | ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. 256 | ''' 257 | self.mutex.acquire() 258 | 259 | try: 260 | self.port.flushInput() 261 | except: 262 | pass 263 | 264 | ntries = 1 265 | attempts = 0 266 | 267 | try: 268 | self.port.write(cmd + '\r') 269 | ack = self.recv(self.timeout) 270 | while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): 271 | try: 272 | self.port.flushInput() 273 | self.port.write(cmd + '\r') 274 | ack = self.recv(self.timeout) 275 | except: 276 | print "Exception executing command: " + cmd 277 | attempts += 1 278 | except: 279 | self.mutex.release() 280 | print "execute_ack exception when executing", cmd 281 | print sys.exc_info() 282 | return 0 283 | 284 | self.mutex.release() 285 | return ack == 'OK' 286 | 287 | def update_pid(self, Kp, Kd, Ki, Ko): 288 | ''' Set the PID parameters on the Arduino 289 | ''' 290 | print "Updating PID parameters" 291 | cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko) 292 | self.execute_ack(cmd) 293 | 294 | def get_baud(self): 295 | ''' Get the current baud rate on the serial port. 296 | ''' 297 | return int(self.execute('b')); 298 | 299 | def get_encoder_counts(self): 300 | values = self.execute_array('e') 301 | if len(values) != 2: 302 | print "Encoder count was not 2" 303 | raise SerialException 304 | return None 305 | else: 306 | return values 307 | 308 | def reset_encoders(self): 309 | ''' Reset the encoder counts to 0 310 | ''' 311 | return self.execute_ack('r') 312 | 313 | def drive(self, right, left): 314 | ''' Speeds are given in encoder ticks per PID interval 315 | ''' 316 | return self.execute_ack('m %d %d' %(right, left)) 317 | 318 | def drive_m_per_s(self, right, left): 319 | ''' Set the motor speeds in meters per second. 320 | ''' 321 | left_revs_per_second = float(left) / (self.wheel_diameter * PI) 322 | right_revs_per_second = float(right) / (self.wheel_diameter * PI) 323 | 324 | left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) 325 | right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) 326 | 327 | self.drive(right_ticks_per_loop , left_ticks_per_loop ) 328 | 329 | def stop(self): 330 | ''' Stop both motors. 331 | ''' 332 | self.drive(0, 0) 333 | 334 | def analog_read(self, pin): 335 | return self.execute('a %d' %pin) 336 | 337 | def analog_write(self, pin, value): 338 | return self.execute_ack('x %d %d' %(pin, value)) 339 | 340 | def digital_read(self, pin): 341 | return self.execute('d %d' %pin) 342 | 343 | def digital_write(self, pin, value): 344 | return self.execute_ack('w %d %d' %(pin, value)) 345 | 346 | def pin_mode(self, pin, mode): 347 | return self.execute_ack('c %d %d' %(pin, mode)) 348 | 349 | def servo_write(self, id, pos): 350 | ''' Usage: servo_write(id, pos) 351 | Position is given in radians and converted to degrees before sending 352 | ''' 353 | return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos))))) 354 | 355 | def servo_read(self, id): 356 | ''' Usage: servo_read(id) 357 | The returned position is converted from degrees to radians 358 | ''' 359 | return radians(self.execute('t %d' %id)) 360 | 361 | def ping(self, pin): 362 | ''' The srf05/Ping command queries an SRF05/Ping sonar sensor 363 | connected to the General Purpose I/O line pinId for a distance, 364 | and returns the range in cm. Sonar distance resolution is integer based. 365 | ''' 366 | return self.execute('p %d' %pin); 367 | 368 | def get_pidin(self): 369 | values = self.execute_array('i') 370 | if len(values) != 2: 371 | print "get_pidin count was not 2" 372 | raise SerialException 373 | return None 374 | else: 375 | return values 376 | 377 | def get_pidout(self): 378 | values = self.execute_array('f') 379 | if len(values) != 2: 380 | print "get_pidout count was not 2" 381 | raise SerialException 382 | return None 383 | else: 384 | return values 385 | 386 | 387 | 388 | """ Class to receive Twist commands and publish Odometry data """ 389 | class BaseController: 390 | def __init__(self, arduino, base_frame): 391 | self.arduino = arduino 392 | self.base_frame = base_frame 393 | self.rate = float(rospy.get_param("~base_controller_rate", 10)) 394 | self.timeout = rospy.get_param("~base_controller_timeout", 1.0) 395 | self.stopped = False 396 | 397 | pid_params = dict() 398 | pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 399 | pid_params['wheel_track'] = rospy.get_param("~wheel_track", "") 400 | pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 401 | pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0) 402 | pid_params['Kp'] = rospy.get_param("~Kp", 20) 403 | pid_params['Kd'] = rospy.get_param("~Kd", 12) 404 | pid_params['Ki'] = rospy.get_param("~Ki", 0) 405 | pid_params['Ko'] = rospy.get_param("~Ko", 50) 406 | 407 | self.accel_limit = rospy.get_param('~accel_limit', 0.1) 408 | self.motors_reversed = rospy.get_param("~motors_reversed", False) 409 | 410 | # Set up PID parameters and check for missing values 411 | self.setup_pid(pid_params) 412 | 413 | # How many encoder ticks are there per meter? 414 | self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * PI) 415 | #self.l_ticks_per_meter = 1440 * self.gear_reduction / (self.wheel_diameter * pi) 416 | #self.r_ticks_per_meter = 1410 * self.gear_reduction / (self.wheel_diameter * pi) 417 | 418 | # What is the maximum acceleration we will tolerate when changing wheel speeds? 419 | self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate 420 | 421 | # Track how often we get a bad encoder count (if any) 422 | self.bad_encoder_count = 0 423 | 424 | self.encoder_min = rospy.get_param('encoder_min', -32768) 425 | self.encoder_max = rospy.get_param('encoder_max', 32768) 426 | self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min ) 427 | self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min ) 428 | self.l_wheel_mult = 0 429 | self.r_wheel_mult = 0 430 | 431 | now = rospy.Time.now() 432 | self.then = now # time for determining dx/dy 433 | self.t_delta = rospy.Duration(1.0 / self.rate) 434 | self.t_next = now + self.t_delta 435 | 436 | # Internal data 437 | self.enc_left = None # encoder readings 438 | self.enc_right = None 439 | self.x = 0 # position in xy plane 440 | self.y = 0 441 | self.th = 0 # rotation in radians 442 | self.v_left = 0 443 | self.v_right = 0 444 | self.v_des_left = 0 # cmd_vel setpoint 445 | self.v_des_right = 0 446 | self.last_cmd_vel = now 447 | 448 | # Subscriptions 449 | rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback) 450 | 451 | # Clear any old odometry info 452 | self.arduino.reset_encoders() 453 | 454 | # Set up the odometry broadcaster 455 | self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5) 456 | self.odomBroadcaster = TransformBroadcaster() 457 | 458 | rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev") 459 | rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame") 460 | 461 | self.lEncoderPub = rospy.Publisher('Lencoder', Int32) 462 | self.rEncoderPub = rospy.Publisher('Rencoder', Int32) 463 | self.lPidoutPub = rospy.Publisher('Lpidout', Int32) 464 | self.rPidoutPub = rospy.Publisher('Rpidout', Int32) 465 | self.lVelPub = rospy.Publisher('Lvel', Int32) 466 | self.rVelPub = rospy.Publisher('Rvel', Int32) 467 | 468 | def setup_pid(self, pid_params): 469 | # Check to see if any PID parameters are missing 470 | missing_params = False 471 | for param in pid_params: 472 | if pid_params[param] == "": 473 | print("*** PID Parameter " + param + " is missing. ***") 474 | missing_params = True 475 | 476 | if missing_params: 477 | os._exit(1) 478 | 479 | self.wheel_diameter = pid_params['wheel_diameter'] 480 | self.wheel_track = pid_params['wheel_track'] 481 | self.encoder_resolution = pid_params['encoder_resolution'] 482 | self.gear_reduction = pid_params['gear_reduction'] 483 | 484 | self.Kp = pid_params['Kp'] 485 | self.Kd = pid_params['Kd'] 486 | self.Ki = pid_params['Ki'] 487 | self.Ko = pid_params['Ko'] 488 | 489 | self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko) 490 | 491 | def poll(self): 492 | now = rospy.Time.now() 493 | if now > self.t_next: 494 | try: 495 | left_pidin, right_pidin = self.arduino.get_pidin() 496 | except: 497 | rospy.logerr("getpidout exception count: ") 498 | return 499 | 500 | self.lEncoderPub.publish(left_pidin) 501 | self.rEncoderPub.publish(right_pidin) 502 | try: 503 | left_pidout, right_pidout = self.arduino.get_pidout() 504 | except: 505 | rospy.logerr("getpidout exception count: ") 506 | return 507 | self.lPidoutPub.publish(left_pidout) 508 | self.rPidoutPub.publish(right_pidout) 509 | # Read the encoders 510 | try: 511 | left_enc, right_enc = self.arduino.get_encoder_counts() 512 | rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc)) 513 | except: 514 | self.bad_encoder_count += 1 515 | rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) 516 | return 517 | 518 | dt = now - self.then 519 | self.then = now 520 | dt = dt.to_sec() 521 | 522 | # Calculate odometry 523 | if self.enc_left == None: 524 | dright = 0 525 | dleft = 0 526 | else: 527 | if (left_enc < self.encoder_low_wrap and self.enc_left > self.encoder_high_wrap) : 528 | self.l_wheel_mult = self.l_wheel_mult + 1 529 | elif (left_enc > self.encoder_high_wrap and self.enc_left < self.encoder_low_wrap) : 530 | self.l_wheel_mult = self.l_wheel_mult - 1 531 | else: 532 | self.l_wheel_mult = 0 533 | if (right_enc < self.encoder_low_wrap and self.enc_right > self.encoder_high_wrap) : 534 | self.r_wheel_mult = self.r_wheel_mult + 1 535 | elif (right_enc > self.encoder_high_wrap and self.enc_right < self.encoder_low_wrap) : 536 | self.r_wheel_mult = self.r_wheel_mult - 1 537 | else: 538 | self.r_wheel_mult = 0 539 | #dright = (right_enc - self.enc_right) / self.ticks_per_meter 540 | #dleft = (left_enc - self.enc_left) / self.ticks_per_meter 541 | dleft = 1.0 * (left_enc + self.l_wheel_mult * (self.encoder_max - self.encoder_min)-self.enc_left) / self.ticks_per_meter 542 | dright = 1.0 * (right_enc + self.r_wheel_mult * (self.encoder_max - self.encoder_min)-self.enc_right) / self.ticks_per_meter 543 | 544 | self.enc_right = right_enc 545 | self.enc_left = left_enc 546 | 547 | dxy_ave = (dright + dleft) / 2.0 548 | dth = (dright - dleft) / self.wheel_track 549 | vxy = dxy_ave / dt 550 | vth = dth / dt 551 | 552 | if (dxy_ave != 0): 553 | dx = cos(dth) * dxy_ave 554 | dy = -sin(dth) * dxy_ave 555 | self.x += (cos(self.th) * dx - sin(self.th) * dy) 556 | self.y += (sin(self.th) * dx + cos(self.th) * dy) 557 | 558 | if (dth != 0): 559 | self.th += dth 560 | 561 | quaternion = Quaternion() 562 | quaternion.x = 0.0 563 | quaternion.y = 0.0 564 | quaternion.z = sin(self.th / 2.0) 565 | quaternion.w = cos(self.th / 2.0) 566 | 567 | # Create the odometry transform frame broadcaster. 568 | self.odomBroadcaster.sendTransform( 569 | (self.x, self.y, 0), 570 | (quaternion.x, quaternion.y, quaternion.z, quaternion.w), 571 | rospy.Time.now(), 572 | self.base_frame, 573 | "odom" 574 | ) 575 | 576 | odom = Odometry() 577 | odom.header.frame_id = "odom" 578 | odom.child_frame_id = self.base_frame 579 | odom.header.stamp = now 580 | odom.pose.pose.position.x = self.x 581 | odom.pose.pose.position.y = self.y 582 | odom.pose.pose.position.z = 0 583 | odom.pose.pose.orientation = quaternion 584 | odom.twist.twist.linear.x = vxy 585 | odom.twist.twist.linear.y = 0 586 | odom.twist.twist.angular.z = vth 587 | 588 | # todo sensor_state.distance == 0 589 | if self.v_des_left == 0 and self.v_des_right == 0: 590 | odom.pose.covariance = ODOM_POSE_COVARIANCE2 591 | odom.twist.covariance = ODOM_TWIST_COVARIANCE2 592 | else: 593 | odom.pose.covariance = ODOM_POSE_COVARIANCE 594 | odom.twist.covariance = ODOM_TWIST_COVARIANCE 595 | 596 | self.odomPub.publish(odom) 597 | 598 | if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): 599 | self.v_des_left = 0 600 | self.v_des_right = 0 601 | 602 | if self.v_left < self.v_des_left: 603 | self.v_left += self.max_accel 604 | if self.v_left > self.v_des_left: 605 | self.v_left = self.v_des_left 606 | else: 607 | self.v_left -= self.max_accel 608 | if self.v_left < self.v_des_left: 609 | self.v_left = self.v_des_left 610 | 611 | if self.v_right < self.v_des_right: 612 | self.v_right += self.max_accel 613 | if self.v_right > self.v_des_right: 614 | self.v_right = self.v_des_right 615 | else: 616 | self.v_right -= self.max_accel 617 | if self.v_right < self.v_des_right: 618 | self.v_right = self.v_des_right 619 | self.lVelPub.publish(self.v_left) 620 | self.rVelPub.publish(self.v_right) 621 | 622 | # Set motor speeds in encoder ticks per PID loop 623 | if not self.stopped: 624 | self.arduino.drive(self.v_left, self.v_right) 625 | 626 | self.t_next = now + self.t_delta 627 | 628 | def stop(self): 629 | self.stopped = True 630 | self.arduino.drive(0, 0) 631 | 632 | def cmdVelCallback(self, req): 633 | # Handle velocity-based movement requests 634 | self.last_cmd_vel = rospy.Time.now() 635 | 636 | x = req.linear.x # m/s 637 | th = req.angular.z # rad/s 638 | 639 | if x == 0: 640 | # Turn in place 641 | right = th * self.wheel_track * self.gear_reduction / 2.0 642 | left = -right 643 | elif th == 0: 644 | # Pure forward/backward motion 645 | left = right = x 646 | else: 647 | # Rotation about a point in space 648 | left = x - th * self.wheel_track * self.gear_reduction / 2.0 649 | right = x + th * self.wheel_track * self.gear_reduction / 2.0 650 | 651 | self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE) 652 | self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE) 653 | 654 | class ArduinoROS(): 655 | def __init__(self): 656 | rospy.init_node('Arduino', log_level=rospy.DEBUG) 657 | 658 | # Cleanup when termniating the node 659 | rospy.on_shutdown(self.shutdown) 660 | 661 | self.port = rospy.get_param("~port", "/dev/ttyACM0") 662 | self.baud = int(rospy.get_param("~baud", 57600)) 663 | self.timeout = rospy.get_param("~timeout", 0.5) 664 | self.base_frame = rospy.get_param("~base_frame", 'base_link') 665 | 666 | # Overall loop rate: should be faster than fastest sensor rate 667 | self.rate = int(rospy.get_param("~rate", 50)) 668 | r = rospy.Rate(self.rate) 669 | 670 | # Rate at which summary SensorState message is published. Individual sensors publish 671 | # at their own rates. 672 | self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) 673 | 674 | self.use_base_controller = rospy.get_param("~use_base_controller", False) 675 | 676 | # Set up the time for publishing the next SensorState message 677 | now = rospy.Time.now() 678 | self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate) 679 | self.t_next_sensors = now + self.t_delta_sensors 680 | 681 | # Initialize a Twist message 682 | self.cmd_vel = Twist() 683 | 684 | # A cmd_vel publisher so we can stop the robot when shutting down 685 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 686 | 687 | # Initialize the controlller 688 | self.controller = Arduino(self.port, self.baud, self.timeout) 689 | 690 | # Make the connection 691 | self.controller.connect() 692 | 693 | rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") 694 | 695 | # Reserve a thread lock 696 | mutex = thread.allocate_lock() 697 | 698 | # Initialize the base controller if used 699 | if self.use_base_controller: 700 | self.myBaseController = BaseController(self.controller, self.base_frame) 701 | 702 | # Start polling the sensors and base controller 703 | while not rospy.is_shutdown(): 704 | 705 | if self.use_base_controller: 706 | mutex.acquire() 707 | self.myBaseController.poll() 708 | mutex.release() 709 | r.sleep() 710 | 711 | def shutdown(self): 712 | # Stop the robot 713 | try: 714 | rospy.loginfo("Stopping the robot...") 715 | self.cmd_vel_pub.Publish(Twist()) 716 | rospy.sleep(2) 717 | except: 718 | pass 719 | rospy.loginfo("Shutting down Arduino Node...") 720 | 721 | if __name__ == '__main__': 722 | myArduino = ArduinoROS() 723 | 724 | -------------------------------------------------------------------------------- /dashgo_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dashgo_bringup 4 | 0.0.0 5 | The dashgo_bringup package 6 | 7 | 8 | 9 | 10 | harney 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/c.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | 3 | import PyKDL 4 | import rospy 5 | from sensor_msgs.msg import Imu 6 | from nav_msgs.msg import Odometry 7 | from geometry_msgs.msg import Twist 8 | from math import * 9 | import threading 10 | import os 11 | import subprocess 12 | import yaml 13 | 14 | 15 | def quat_to_angle(quat): 16 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 17 | return rot.GetRPY()[2] 18 | def normalize_angle(angle): 19 | res = angle 20 | while res > pi: 21 | res -= 2.0*pi 22 | while res < -pi: 23 | res += 2.0*pi 24 | return res 25 | 26 | 27 | class CalibrateRobot: 28 | def __init__(self): 29 | self.lock = threading.Lock() 30 | self.sub_imu = rospy.Subscriber('imu', Imu, self.imu_cb) 31 | self.last_imu_angle = 0 32 | self.imu_angle = 0 33 | while not rospy.is_shutdown(): 34 | self.last_imu_angle = self.imu_angle 35 | rospy.sleep(10) 36 | rospy.loginfo("imu_drift:"+str((self.imu_angle-self.last_imu_angle)*180/3.1415926)) 37 | 38 | 39 | def imu_cb(self, msg): 40 | with self.lock: 41 | angle = quat_to_angle(msg.orientation) 42 | self.imu_angle = angle 43 | self.imu_time = msg.header.stamp 44 | 45 | def main(): 46 | rospy.init_node('scan_to_angle') 47 | CalibrateRobot() 48 | 49 | 50 | if __name__ == '__main__': 51 | main() 52 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/check_angular.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist, Quaternion 5 | from nav_msgs.msg import Odometry 6 | import tf 7 | from math import radians, copysign 8 | import PyKDL 9 | from math import pi 10 | 11 | 12 | def quat_to_angle(quat): 13 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 14 | return rot.GetRPY()[2] 15 | 16 | def normalize_angle(angle): 17 | res = angle 18 | while res > pi: 19 | res -= 2.0 * pi 20 | while res < -pi: 21 | res += 2.0 * pi 22 | return res 23 | 24 | class CalibrateAngular(): 25 | def __init__(self): 26 | # Give the node a name 27 | rospy.init_node('calibrate_angular', anonymous=False) 28 | 29 | # Set rospy to execute a shutdown function when terminating the script 30 | rospy.on_shutdown(self.shutdown) 31 | 32 | # How fast will we check the odometry values? 33 | self.rate = rospy.get_param('~rate', 20) 34 | r = rospy.Rate(self.rate) 35 | 36 | # The test angle is 360 degrees 37 | self.test_angle = radians(rospy.get_param('~test_angle', 360.0)) 38 | 39 | self.speed = rospy.get_param('~speed', 0.5) # radians per second 40 | self.tolerance = radians(rospy.get_param('tolerance', 1)) # degrees converted to radians 41 | self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0) 42 | self.start_test = rospy.get_param('~start_test', True) 43 | 44 | # Publisher to control the robot's speed 45 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 46 | 47 | # The base frame is usually base_link or base_footprint 48 | self.base_frame = rospy.get_param('~base_frame', '/base_footprint') 49 | 50 | # The odom frame is usually just /odom 51 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 52 | 53 | # Initialize the tf listener 54 | self.tf_listener = tf.TransformListener() 55 | 56 | # Give tf some time to fill its buffer 57 | rospy.sleep(2) 58 | 59 | # Make sure we see the odom and base frames 60 | self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) 61 | 62 | rospy.loginfo("Bring up rqt_reconfigure to control the test.") 63 | 64 | reverse = 1 65 | 66 | while not rospy.is_shutdown(): 67 | if self.start_test: 68 | # Get the current rotation angle from tf 69 | self.odom_angle = self.get_odom_angle() 70 | 71 | last_angle = self.odom_angle 72 | turn_angle = 0 73 | self.test_angle *= reverse 74 | error = self.test_angle - turn_angle 75 | 76 | # Alternate directions between tests 77 | reverse = -reverse 78 | 79 | while abs(error) > self.tolerance and self.start_test: 80 | if rospy.is_shutdown(): 81 | return 82 | 83 | # Rotate the robot to reduce the error 84 | move_cmd = Twist() 85 | move_cmd.angular.z = copysign(self.speed, error) 86 | self.cmd_vel.publish(move_cmd) 87 | r.sleep() 88 | 89 | # Get the current rotation angle from tf 90 | self.odom_angle = self.get_odom_angle() 91 | 92 | # Compute how far we have gone since the last measurement 93 | delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle) 94 | 95 | # Add to our total angle so far 96 | turn_angle += delta_angle 97 | 98 | # Compute the new error 99 | error = self.test_angle - turn_angle 100 | 101 | # Store the current angle for the next comparison 102 | last_angle = self.odom_angle 103 | 104 | # Stop the robot 105 | self.cmd_vel.publish(Twist()) 106 | 107 | # Update the status flag 108 | self.start_test = False 109 | params = {'start_test': False} 110 | 111 | rospy.sleep(0.5) 112 | 113 | # Stop the robot 114 | self.cmd_vel.publish(Twist()) 115 | 116 | def get_odom_angle(self): 117 | # Get the current transform between the odom and base frames 118 | try: 119 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 120 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 121 | rospy.loginfo("TF Exception") 122 | return 123 | 124 | # Convert the rotation from a quaternion to an Euler angle 125 | return quat_to_angle(Quaternion(*rot)) 126 | 127 | 128 | def shutdown(self): 129 | # Always stop the robot when shutting down the node 130 | rospy.loginfo("Stopping the robot...") 131 | self.cmd_vel.publish(Twist()) 132 | rospy.sleep(1) 133 | 134 | if __name__ == '__main__': 135 | try: 136 | CalibrateAngular() 137 | except: 138 | rospy.loginfo("Calibration terminated.") 139 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/check_linear.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist, Point 5 | from math import copysign, sqrt, pow 6 | import tf 7 | 8 | class CalibrateLinear(): 9 | def __init__(self): 10 | # Give the node a name 11 | rospy.init_node('calibrate_linear', anonymous=False) 12 | 13 | # Set rospy to execute a shutdown function when terminating the script 14 | rospy.on_shutdown(self.shutdown) 15 | 16 | # How fast will we check the odometry values? 17 | self.rate = rospy.get_param('~rate', 20) 18 | r = rospy.Rate(self.rate) 19 | 20 | # Set the distance to travel 21 | self.test_distance = rospy.get_param('~test_distance', 1.0) # meters 22 | self.speed = rospy.get_param('~speed', 0.15) # meters per second 23 | self.tolerance = rospy.get_param('~tolerance', 0.01) # meters 24 | self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0) 25 | self.start_test = rospy.get_param('~start_test', True) 26 | 27 | # Publisher to control the robot's speed 28 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 29 | 30 | 31 | # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot 32 | self.base_frame = rospy.get_param('~base_frame', '/base_footprint') 33 | 34 | # The odom frame is usually just /odom 35 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 36 | 37 | # Initialize the tf listener 38 | self.tf_listener = tf.TransformListener() 39 | 40 | # Give tf some time to fill its buffer 41 | rospy.sleep(2) 42 | 43 | # Make sure we see the odom and base frames 44 | self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) 45 | 46 | rospy.loginfo("Bring up rqt_reconfigure to control the test.") 47 | 48 | self.position = Point() 49 | 50 | # Get the starting position from the tf transform between the odom and base frames 51 | self.position = self.get_position() 52 | 53 | x_start = self.position.x 54 | y_start = self.position.y 55 | 56 | move_cmd = Twist() 57 | 58 | while not rospy.is_shutdown(): 59 | # Stop the robot by default 60 | move_cmd = Twist() 61 | 62 | if self.start_test: 63 | # Get the current position from the tf transform between the odom and base frames 64 | self.position = self.get_position() 65 | 66 | # Compute the Euclidean distance from the target point 67 | distance = sqrt(pow((self.position.x - x_start), 2) + 68 | pow((self.position.y - y_start), 2)) 69 | 70 | # Correct the estimated distance by the correction factor 71 | distance *= self.odom_linear_scale_correction 72 | 73 | # How close are we? 74 | error = distance - self.test_distance 75 | 76 | # Are we close enough? 77 | if not self.start_test or abs(error) < self.tolerance: 78 | self.start_test = False 79 | params = {'start_test': False} 80 | rospy.loginfo(params) 81 | else: 82 | # If not, move in the appropriate direction 83 | move_cmd.linear.x = copysign(self.speed, -1 * error) 84 | else: 85 | self.position = self.get_position() 86 | x_start = self.position.x 87 | y_start = self.position.y 88 | 89 | self.cmd_vel.publish(move_cmd) 90 | r.sleep() 91 | 92 | # Stop the robot 93 | self.cmd_vel.publish(Twist()) 94 | 95 | def get_position(self): 96 | # Get the current transform between the odom and base frames 97 | try: 98 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 99 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 100 | rospy.loginfo("TF Exception") 101 | return 102 | 103 | return Point(*trans) 104 | 105 | def shutdown(self): 106 | # Always stop the robot when shutting down the node 107 | rospy.loginfo("Stopping the robot...") 108 | self.cmd_vel.publish(Twist()) 109 | rospy.sleep(1) 110 | 111 | if __name__ == '__main__': 112 | try: 113 | CalibrateLinear() 114 | rospy.spin() 115 | except: 116 | rospy.loginfo("Calibration terminated.") 117 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/get_angular.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | 3 | import PyKDL 4 | import rospy 5 | from sensor_msgs.msg import Imu 6 | from nav_msgs.msg import Odometry 7 | from geometry_msgs.msg import Twist 8 | from math import * 9 | import threading 10 | import os 11 | import subprocess 12 | import yaml 13 | 14 | 15 | def quat_to_angle(quat): 16 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 17 | return rot.GetRPY()[2] 18 | def normalize_angle(angle): 19 | res = angle 20 | while res > pi: 21 | res -= 2.0*pi 22 | while res < -pi: 23 | res += 2.0*pi 24 | return res 25 | 26 | 27 | class CalibrateRobot: 28 | def __init__(self): 29 | self.lock = threading.Lock() 30 | self.sub_imu = rospy.Subscriber('imu', Imu, self.imu_cb) 31 | self.last_imu_angle = 0 32 | self.imu_angle = 0 33 | while not rospy.is_shutdown(): 34 | rospy.sleep(0.3) 35 | rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926)) 36 | 37 | 38 | def imu_cb(self, msg): 39 | with self.lock: 40 | angle = quat_to_angle(msg.orientation) 41 | self.imu_angle = angle 42 | self.imu_time = msg.header.stamp 43 | 44 | def main(): 45 | rospy.init_node('scan_to_angle') 46 | CalibrateRobot() 47 | 48 | 49 | if __name__ == '__main__': 50 | main() 51 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/get_angular_combined.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | 3 | import PyKDL 4 | import rospy 5 | from sensor_msgs.msg import Imu 6 | from nav_msgs.msg import Odometry 7 | from geometry_msgs.msg import Twist, PoseWithCovarianceStamped 8 | from math import * 9 | import threading 10 | import os 11 | import subprocess 12 | import yaml 13 | 14 | 15 | def quat_to_angle(quat): 16 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 17 | return rot.GetRPY()[2] 18 | def normalize_angle(angle): 19 | res = angle 20 | while res > pi: 21 | res -= 2.0*pi 22 | while res < -pi: 23 | res += 2.0*pi 24 | return res 25 | 26 | 27 | class CalibrateRobot: 28 | def __init__(self): 29 | self.lock = threading.Lock() 30 | self.sub_imu = rospy.Subscriber('robot_pose_ekf/odom_combined', PoseWithCovarianceStamped, self.imu_cb) 31 | self.last_imu_angle = 0 32 | self.imu_angle = 0 33 | while not rospy.is_shutdown(): 34 | rospy.sleep(0.1) 35 | rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926)) 36 | 37 | def imu_cb(self, msg): 38 | with self.lock: 39 | angle = quat_to_angle(msg.pose.orientation) 40 | self.imu_angle = angle 41 | self.imu_time = msg.header.stamp 42 | 43 | def main(): 44 | rospy.init_node('scan_to_angle') 45 | CalibrateRobot() 46 | 47 | 48 | if __name__ == '__main__': 49 | main() 50 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/get_angular_odom.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | 3 | import PyKDL 4 | import rospy 5 | from sensor_msgs.msg import Imu 6 | from nav_msgs.msg import Odometry 7 | from geometry_msgs.msg import Twist 8 | from math import * 9 | import threading 10 | import os 11 | import subprocess 12 | import yaml 13 | 14 | 15 | def quat_to_angle(quat): 16 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 17 | return rot.GetRPY()[2] 18 | def normalize_angle(angle): 19 | res = angle 20 | while res > pi: 21 | res -= 2.0*pi 22 | while res < -pi: 23 | res += 2.0*pi 24 | return res 25 | 26 | 27 | class CalibrateRobot1: 28 | def __init__(self): 29 | self.lock = threading.Lock() 30 | self.sub_imu = rospy.Subscriber('odom', Odometry, self.imu_cb) 31 | self.last_imu_angle = 0 32 | self.imu_angle = 0 33 | while not rospy.is_shutdown(): 34 | rospy.sleep(0.3) 35 | rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926)) 36 | 37 | 38 | def imu_cb(self, msg): 39 | with self.lock: 40 | angle = quat_to_angle(msg.pose.pose.orientation) 41 | self.imu_angle = angle 42 | self.imu_time = msg.header.stamp 43 | 44 | def main(): 45 | rospy.init_node('scan_to_angle1') 46 | CalibrateRobot1() 47 | 48 | 49 | if __name__ == '__main__': 50 | main() 51 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/move_base_square.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ move_base_square.py - Version 1.1 2013-12-20 4 | 5 | Command a robot to move in a square using move_base actions.. 6 | 7 | Created for the Pi Robot Project: http://www.pirobot.org 8 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 9 | 10 | This program is free software; you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation; either version 2 of the License, or 13 | (at your option) any later version.5 14 | 15 | This program is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details at: 19 | 20 | http://www.gnu.org/licenses/gpl.htmlPoint 21 | 22 | """ 23 | 24 | import rospy 25 | import actionlib 26 | from actionlib_msgs.msg import * 27 | from geometry_msgs.msg import Pose, Point, Quaternion, Twist 28 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 29 | from tf.transformations import quaternion_from_euler 30 | from visualization_msgs.msg import Marker 31 | from math import radians, pi 32 | 33 | class MoveBaseSquare(): 34 | def __init__(self): 35 | rospy.init_node('nav_test', anonymous=False) 36 | 37 | rospy.on_shutdown(self.shutdown) 38 | 39 | # How big is the square we want the robot to navigate? 40 | square_size = rospy.get_param("~square_size", 1.0) # meters 41 | 42 | # Create a list to hold the target quaternions (orientations) 43 | quaternions = list() 44 | 45 | # First define the corner orientations as Euler angles 46 | euler_angles = (pi/2, pi, 3*pi/2, 0) 47 | 48 | # Then convert the angles to quaternions 49 | for angle in euler_angles: 50 | q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') 51 | q = Quaternion(*q_angle) 52 | quaternions.append(q) 53 | 54 | # Create a list to hold the waypoint poses 55 | waypoints = list() 56 | 57 | # Append each of the four waypoints to the list. Each waypoint 58 | # is a pose consisting of a position and orientation in the map frame. 59 | waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0])) 60 | waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1])) 61 | waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2])) 62 | waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) 63 | 64 | # Initialize the visualization markers for RViz 65 | self.init_markers() 66 | 67 | # Set a visualization marker at each waypoint 68 | for waypoint in waypoints: 69 | p = Point() 70 | p = waypoint.position 71 | self.markers.points.append(p) 72 | 73 | # Publisher to manually control the robot (e.g. to stop it, queue_size=5) 74 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 75 | 76 | # Subscribe to the move_base action server 77 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 78 | 79 | rospy.loginfo("Waiting for move_base action server...") 80 | 81 | # Wait 60 seconds for the action server to become available 82 | self.move_base.wait_for_server(rospy.Duration(60)) 83 | 84 | rospy.loginfo("Connected to move base server") 85 | rospy.loginfo("Starting navigation test") 86 | 87 | # Initialize a counter to track waypoints 88 | i = 0 89 | 90 | # Cycle through the four waypoints 91 | while i < 4 and not rospy.is_shutdown(): 92 | # Update the marker display 93 | self.marker_pub.publish(self.markers) 94 | 95 | # Intialize the waypoint goal 96 | goal = MoveBaseGoal() 97 | 98 | # Use the map frame to define goal poses 99 | goal.target_pose.header.frame_id = 'map' 100 | 101 | # Set the time stamp to "now" 102 | goal.target_pose.header.stamp = rospy.Time.now() 103 | 104 | # Set the goal pose to the i-th waypoint 105 | goal.target_pose.pose = waypoints[i] 106 | 107 | # Start the robot moving toward the goal 108 | self.move(goal) 109 | 110 | i += 1 111 | 112 | def move(self, goal): 113 | # Send the goal pose to the MoveBaseAction server 114 | self.move_base.send_goal(goal) 115 | 116 | # Allow 1 minute to get there 117 | finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) 118 | 119 | # If we don't get there in time, abort the goal 120 | if not finished_within_time: 121 | self.move_base.cancel_goal() 122 | rospy.loginfo("Timed out achieving goal") 123 | else: 124 | # We made it! 125 | state = self.move_base.get_state() 126 | if state == GoalStatus.SUCCEEDED: 127 | rospy.loginfo("Goal succeeded!") 128 | 129 | def init_markers(self): 130 | # Set up our waypoint markers 131 | marker_scale = 0.2 132 | marker_lifetime = 0 # 0 is forever 133 | marker_ns = 'waypoints' 134 | marker_id = 0 135 | marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} 136 | 137 | # Define a marker publisher. 138 | self.marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=5) 139 | 140 | # Initialize the marker points list. 141 | self.markers = Marker() 142 | self.markers.ns = marker_ns 143 | self.markers.id = marker_id 144 | self.markers.type = Marker.CUBE_LIST 145 | self.markers.action = Marker.ADD 146 | self.markers.lifetime = rospy.Duration(marker_lifetime) 147 | self.markers.scale.x = marker_scale 148 | self.markers.scale.y = marker_scale 149 | self.markers.color.r = marker_color['r'] 150 | self.markers.color.g = marker_color['g'] 151 | self.markers.color.b = marker_color['b'] 152 | self.markers.color.a = marker_color['a'] 153 | 154 | self.markers.header.frame_id = 'odom' 155 | self.markers.header.stamp = rospy.Time.now() 156 | self.markers.points = list() 157 | 158 | def shutdown(self): 159 | rospy.loginfo("Stopping the robot...") 160 | # Cancel any active goals 161 | self.move_base.cancel_goal() 162 | rospy.sleep(2) 163 | # Stop the robot 164 | self.cmd_vel_pub.publish(Twist()) 165 | rospy.sleep(1) 166 | 167 | if __name__ == '__main__': 168 | try: 169 | MoveBaseSquare() 170 | except rospy.ROSInterruptException: 171 | rospy.loginfo("Navigation test finished.") 172 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/nav_grid.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ nav_square.py - Version 1.1 2013-12-20 4 | 5 | A basic demo of the using odometry data to move the robot 6 | along a square trajectory. 7 | 8 | Created for the Pi Robot Project: http://www.pirobot.org 9 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 10 | 11 | This program is free software; you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation; either version 2 of the License, or 14 | (at your option) any later version.5 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details at: 20 | 21 | http://www.gnu.org/licenses/gpl.html 22 | 23 | """ 24 | 25 | import rospy 26 | from geometry_msgs.msg import Twist, Point, Quaternion 27 | import tf 28 | from math import radians, copysign, sqrt, pow, pi 29 | import PyKDL 30 | 31 | 32 | def quat_to_angle(quat): 33 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 34 | return rot.GetRPY()[2] 35 | 36 | def normalize_angle(angle): 37 | res = angle 38 | while res > pi: 39 | res -= 2.0 * pi 40 | while res < -pi: 41 | res += 2.0 * pi 42 | return res 43 | 44 | 45 | class NavSquare(): 46 | def __init__(self): 47 | # Give the node a name 48 | rospy.init_node('nav_square', anonymous=False) 49 | 50 | # Set rospy to execute a shutdown function when terminating the script 51 | rospy.on_shutdown(self.shutdown) 52 | 53 | # How fast will we check the odometry values? 54 | rate = 20 55 | 56 | # Set the equivalent ROS rate variable 57 | r = rospy.Rate(rate) 58 | 59 | # Set the parameters for the target square 60 | goal_distance = rospy.get_param("~goal_distance", 1.0) # meters 61 | goal_angle = radians(rospy.get_param("~goal_angle", 90)) # degrees converted to radians 62 | linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second 63 | angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second 64 | angular_tolerance = radians(rospy.get_param("~angular_tolerance", 2)) # degrees to radians 65 | 66 | # Publisher to control the robot's speed 67 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 68 | 69 | # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot 70 | self.base_frame = rospy.get_param('~base_frame', '/base_footprint') 71 | 72 | # The odom frame is usually just /odom 73 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 74 | 75 | # Initialize the tf listener 76 | self.tf_listener = tf.TransformListener() 77 | 78 | # Give tf some time to fill its buffer 79 | rospy.sleep(2) 80 | 81 | # Set the odom frame 82 | self.odom_frame = '/odom' 83 | 84 | # Find out if the robot uses /base_link or /base_footprint 85 | try: 86 | self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) 87 | self.base_frame = '/base_footprint' 88 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 89 | try: 90 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) 91 | self.base_frame = '/base_link' 92 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 93 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 94 | rospy.signal_shutdown("tf Exception") 95 | 96 | # Initialize the position variable as a Point type 97 | position = Point() 98 | 99 | # Cycle through the four sides of the square 100 | for i in range(8): 101 | # Initialize the movement command 102 | move_cmd = Twist() 103 | 104 | # Set the movement command to forward motion 105 | move_cmd.linear.x = linear_speed 106 | 107 | # Get the starting position values 108 | (position, rotation) = self.get_odom() 109 | 110 | x_start = position.x 111 | y_start = position.y 112 | 113 | # Keep track of the distance traveled 114 | distance = 0 115 | if (i%2==1): 116 | goal_distance = 0.2 117 | else: 118 | goal_distance = 1.0 119 | # Enter the loop to move along a side 120 | while distance < goal_distance and not rospy.is_shutdown(): 121 | # Publish the Twist message and sleep 1 cycle 122 | self.cmd_vel.publish(move_cmd) 123 | 124 | r.sleep() 125 | 126 | # Get the current position 127 | (position, rotation) = self.get_odom() 128 | 129 | # Compute the Euclidean distance from the start 130 | distance = sqrt(pow((position.x - x_start), 2) + 131 | pow((position.y - y_start), 2)) 132 | 133 | # Stop the robot before rotating 134 | move_cmd = Twist() 135 | self.cmd_vel.publish(move_cmd) 136 | rospy.sleep(1.0) 137 | 138 | # Set the movement command to a rotation 139 | if (i%4<=1): 140 | move_cmd.angular.z = angular_speed 141 | else: 142 | move_cmd.angular.z = -1*angular_speed 143 | 144 | # Track the last angle measured 145 | last_angle = rotation 146 | 147 | # Track how far we have turned 148 | turn_angle = 0 149 | 150 | # Begin the rotation 151 | while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): 152 | # Publish the Twist message and sleep 1 cycle 153 | self.cmd_vel.publish(move_cmd) 154 | 155 | r.sleep() 156 | 157 | # Get the current rotation 158 | (position, rotation) = self.get_odom() 159 | 160 | # Compute the amount of rotation since the last lopp 161 | delta_angle = normalize_angle(rotation - last_angle) 162 | 163 | turn_angle += delta_angle 164 | last_angle = rotation 165 | 166 | move_cmd = Twist() 167 | self.cmd_vel.publish(move_cmd) 168 | rospy.sleep(1.0) 169 | 170 | # Stop the robot when we are done 171 | self.cmd_vel.publish(Twist()) 172 | 173 | def get_odom(self): 174 | # Get the current transform between the odom and base frames 175 | try: 176 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 177 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 178 | rospy.loginfo("TF Exception") 179 | return 180 | 181 | return (Point(*trans), quat_to_angle(Quaternion(*rot))) 182 | 183 | def shutdown(self): 184 | # Always stop the robot when shutting down the node 185 | rospy.loginfo("Stopping the robot...") 186 | self.cmd_vel.publish(Twist()) 187 | rospy.sleep(1) 188 | 189 | if __name__ == '__main__': 190 | try: 191 | NavSquare() 192 | except rospy.ROSInterruptException: 193 | rospy.loginfo("Navigation terminated.") 194 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/nav_square.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ nav_square.py - Version 1.1 2013-12-20 4 | 5 | A basic demo of the using odometry data to move the robot 6 | along a square trajectory. 7 | 8 | Created for the Pi Robot Project: http://www.pirobot.org 9 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 10 | 11 | This program is free software; you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation; either version 2 of the License, or 14 | (at your option) any later version.5 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details at: 20 | 21 | http://www.gnu.org/licenses/gpl.html 22 | 23 | """ 24 | 25 | import rospy 26 | from geometry_msgs.msg import Twist, Point, Quaternion 27 | import tf 28 | from math import radians, copysign, sqrt, pow, pi 29 | import PyKDL 30 | 31 | 32 | def quat_to_angle(quat): 33 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 34 | return rot.GetRPY()[2] 35 | 36 | def normalize_angle(angle): 37 | res = angle 38 | while res > pi: 39 | res -= 2.0 * pi 40 | while res < -pi: 41 | res += 2.0 * pi 42 | return res 43 | 44 | 45 | class NavSquare(): 46 | def __init__(self): 47 | # Give the node a name 48 | rospy.init_node('nav_square', anonymous=False) 49 | 50 | # Set rospy to execute a shutdown function when terminating the script 51 | rospy.on_shutdown(self.shutdown) 52 | 53 | # How fast will we check the odometry values? 54 | rate = 20 55 | 56 | # Set the equivalent ROS rate variable 57 | r = rospy.Rate(rate) 58 | 59 | # Set the parameters for the target square 60 | goal_distance = rospy.get_param("~goal_distance", 1.0) # meters 61 | goal_angle = radians(rospy.get_param("~goal_angle", 90)) # degrees converted to radians 62 | linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second 63 | angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second 64 | angular_tolerance = radians(rospy.get_param("~angular_tolerance", 2)) # degrees to radians 65 | 66 | # Publisher to control the robot's speed 67 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 68 | 69 | # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot 70 | self.base_frame = rospy.get_param('~base_frame', '/base_footprint') 71 | 72 | # The odom frame is usually just /odom 73 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 74 | 75 | # Initialize the tf listener 76 | self.tf_listener = tf.TransformListener() 77 | 78 | # Give tf some time to fill its buffer 79 | rospy.sleep(2) 80 | 81 | # Set the odom frame 82 | self.odom_frame = '/odom' 83 | 84 | # Find out if the robot uses /base_link or /base_footprint 85 | try: 86 | self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) 87 | self.base_frame = '/base_footprint' 88 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 89 | try: 90 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) 91 | self.base_frame = '/base_link' 92 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 93 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 94 | rospy.signal_shutdown("tf Exception") 95 | 96 | # Initialize the position variable as a Point type 97 | position = Point() 98 | 99 | # Cycle through the four sides of the square 100 | for i in range(4): 101 | # Initialize the movement command 102 | move_cmd = Twist() 103 | 104 | # Set the movement command to forward motion 105 | move_cmd.linear.x = linear_speed 106 | 107 | # Get the starting position values 108 | (position, rotation) = self.get_odom() 109 | 110 | x_start = position.x 111 | y_start = position.y 112 | 113 | # Keep track of the distance traveled 114 | distance = 0 115 | 116 | # Enter the loop to move along a side 117 | while distance < goal_distance and not rospy.is_shutdown(): 118 | # Publish the Twist message and sleep 1 cycle 119 | self.cmd_vel.publish(move_cmd) 120 | 121 | r.sleep() 122 | 123 | # Get the current position 124 | (position, rotation) = self.get_odom() 125 | 126 | # Compute the Euclidean distance from the start 127 | distance = sqrt(pow((position.x - x_start), 2) + 128 | pow((position.y - y_start), 2)) 129 | 130 | # Stop the robot before rotating 131 | move_cmd = Twist() 132 | self.cmd_vel.publish(move_cmd) 133 | rospy.sleep(1.0) 134 | 135 | # Set the movement command to a rotation 136 | move_cmd.angular.z = angular_speed 137 | 138 | # Track the last angle measured 139 | last_angle = rotation 140 | 141 | # Track how far we have turned 142 | turn_angle = 0 143 | 144 | # Begin the rotation 145 | while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): 146 | # Publish the Twist message and sleep 1 cycle 147 | self.cmd_vel.publish(move_cmd) 148 | 149 | r.sleep() 150 | 151 | # Get the current rotation 152 | (position, rotation) = self.get_odom() 153 | 154 | # Compute the amount of rotation since the last lopp 155 | delta_angle = normalize_angle(rotation - last_angle) 156 | 157 | turn_angle += delta_angle 158 | last_angle = rotation 159 | 160 | move_cmd = Twist() 161 | self.cmd_vel.publish(move_cmd) 162 | rospy.sleep(1.0) 163 | 164 | # Stop the robot when we are done 165 | self.cmd_vel.publish(Twist()) 166 | 167 | def get_odom(self): 168 | # Get the current transform between the odom and base frames 169 | try: 170 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 171 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 172 | rospy.loginfo("TF Exception") 173 | return 174 | 175 | return (Point(*trans), quat_to_angle(Quaternion(*rot))) 176 | 177 | def shutdown(self): 178 | # Always stop the robot when shutting down the node 179 | rospy.loginfo("Stopping the robot...") 180 | self.cmd_vel.publish(Twist()) 181 | rospy.sleep(1) 182 | 183 | if __name__ == '__main__': 184 | try: 185 | NavSquare() 186 | except rospy.ROSInterruptException: 187 | rospy.loginfo("Navigation terminated.") 188 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/odom_ekf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ odom_ekf.py - Version 1.1 2013-12-20 4 | 5 | Republish the /robot_pose_ekf/odom_combined topic which is of type 6 | geometry_msgs/PoseWithCovarianceStamped as an equivalent message of 7 | type nav_msgs/Odometry so we can view it in RViz. 8 | 9 | Created for the Pi Robot Project: http://www.pirobot.org 10 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 11 | 12 | This program is free software; you can redistribute it and/or modify 13 | it under the terms of the GNU General Public License as published by 14 | the Free Software Foundation; either version 2 of the License, or 15 | (at your option) any later version.5 16 | 17 | This program is distributed in the hope that it will be useful, 18 | but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | GNU General Public License for more details at: 21 | 22 | http://www.gnu.org/licenses/gpl.html 23 | 24 | """ 25 | 26 | import rospy 27 | from geometry_msgs.msg import PoseWithCovarianceStamped 28 | from nav_msgs.msg import Odometry 29 | 30 | class OdomEKF(): 31 | def __init__(self): 32 | # Give the node a name 33 | rospy.init_node('odom_ekf', anonymous=False) 34 | 35 | # Publisher of type nav_msgs/Odometry 36 | self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=5) 37 | 38 | # Wait for the /odom_combined topic to become available 39 | rospy.wait_for_message('input', PoseWithCovarianceStamped) 40 | 41 | # Subscribe to the /odom_combined topic 42 | rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom) 43 | 44 | rospy.loginfo("Publishing combined odometry on /odom_ekf") 45 | 46 | def pub_ekf_odom(self, msg): 47 | odom = Odometry() 48 | odom.header = msg.header 49 | odom.header.frame_id = '/odom' 50 | odom.child_frame_id = 'base_footprint' 51 | odom.pose = msg.pose 52 | 53 | self.ekf_pub.publish(odom) 54 | 55 | if __name__ == '__main__': 56 | try: 57 | OdomEKF() 58 | rospy.spin() 59 | except: 60 | pass 61 | 62 | -------------------------------------------------------------------------------- /dashgo_bringup/scripts/odom_out_back.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ odom_out_and_back.py - Version 1.1 2013-12-20 4 | 5 | A basic demo of using the /odom topic to move a robot a given distance 6 | or rotate through a given angle. 7 | 8 | Created for the Pi Robot Project: http://www.pirobot.org 9 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 10 | 11 | This program is free software; you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation; either version 2 of the License, or 14 | (at your option) any later version.5 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details at: 20 | 21 | http://www.gnu.org/licenses/gpl.html 22 | 23 | """ 24 | 25 | import rospy 26 | from geometry_msgs.msg import Twist, Point, Quaternion 27 | import tf 28 | from math import radians, copysign, sqrt, pow, pi 29 | import PyKDL 30 | 31 | def quat_to_angle(quat): 32 | rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w) 33 | return rot.GetRPY()[2] 34 | 35 | def normalize_angle(angle): 36 | res = angle 37 | while res > pi: 38 | res -= 2.0 * pi 39 | while res < -pi: 40 | res += 2.0 * pi 41 | return res 42 | 43 | 44 | 45 | class OutAndBack(): 46 | def __init__(self): 47 | # Give the node a name 48 | rospy.init_node('out_and_back', anonymous=False) 49 | 50 | # Set rospy to execute a shutdown function when exiting 51 | rospy.on_shutdown(self.shutdown) 52 | 53 | # Publisher to control the robot's speed 54 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 55 | 56 | # How fast will we update the robot's movement? 57 | rate = 20 58 | 59 | # Set the equivalent ROS rate variable 60 | r = rospy.Rate(rate) 61 | 62 | # Set the forward linear speed to 0.15 meters per second 63 | linear_speed = 0.15 64 | 65 | # Set the travel distance in meters 66 | goal_distance = 1.0 67 | 68 | # Set the rotation speed in radians per second 69 | angular_speed = 0.5 70 | 71 | # Set the angular tolerance in degrees converted to radians 72 | angular_tolerance = radians(1.0) 73 | 74 | # Set the rotation angle to Pi radians (180 degrees) 75 | goal_angle = pi 76 | 77 | # Initialize the tf listener 78 | self.tf_listener = tf.TransformListener() 79 | 80 | # Give tf some time to fill its buffer 81 | rospy.sleep(2) 82 | 83 | # Set the odom frame 84 | self.odom_frame = '/odom' 85 | 86 | # Find out if the robot uses /base_link or /base_footprint 87 | try: 88 | self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) 89 | self.base_frame = '/base_footprint' 90 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 91 | try: 92 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) 93 | self.base_frame = '/base_link' 94 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 95 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 96 | rospy.signal_shutdown("tf Exception") 97 | 98 | # Initialize the position variable as a Point type 99 | position = Point() 100 | 101 | # Loop once for each leg of the trip 102 | for i in range(2): 103 | # Initialize the movement command 104 | move_cmd = Twist() 105 | 106 | # Set the movement command to forward motion 107 | move_cmd.linear.x = linear_speed 108 | 109 | # Get the starting position values 110 | (position, rotation) = self.get_odom() 111 | 112 | x_start = position.x 113 | y_start = position.y 114 | 115 | # Keep track of the distance traveled 116 | distance = 0 117 | 118 | # Enter the loop to move along a side 119 | while distance < goal_distance and not rospy.is_shutdown(): 120 | # Publish the Twist message and sleep 1 cycle 121 | self.cmd_vel.publish(move_cmd) 122 | 123 | r.sleep() 124 | 125 | # Get the current position 126 | (position, rotation) = self.get_odom() 127 | 128 | # Compute the Euclidean distance from the start 129 | distance = sqrt(pow((position.x - x_start), 2) + 130 | pow((position.y - y_start), 2)) 131 | 132 | # Stop the robot before the rotation 133 | move_cmd = Twist() 134 | self.cmd_vel.publish(move_cmd) 135 | rospy.sleep(1) 136 | 137 | # Set the movement command to a rotation 138 | move_cmd.angular.z = angular_speed 139 | 140 | # Track the last angle measured 141 | last_angle = rotation 142 | 143 | # Track how far we have turned 144 | turn_angle = 0 145 | 146 | while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): 147 | # Publish the Twist message and sleep 1 cycle 148 | self.cmd_vel.publish(move_cmd) 149 | r.sleep() 150 | 151 | # Get the current rotation 152 | (position, rotation) = self.get_odom() 153 | 154 | # Compute the amount of rotation since the last loop 155 | delta_angle = normalize_angle(rotation - last_angle) 156 | 157 | # Add to the running total 158 | turn_angle += delta_angle 159 | last_angle = rotation 160 | 161 | # Stop the robot before the next leg 162 | move_cmd = Twist() 163 | self.cmd_vel.publish(move_cmd) 164 | rospy.sleep(1) 165 | 166 | # Stop the robot for good 167 | self.cmd_vel.publish(Twist()) 168 | 169 | def get_odom(self): 170 | # Get the current transform between the odom and base frames 171 | try: 172 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 173 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 174 | rospy.loginfo("TF Exception") 175 | return 176 | 177 | return (Point(*trans), quat_to_angle(Quaternion(*rot))) 178 | 179 | def shutdown(self): 180 | # Always stop the robot when shutting down the node. 181 | rospy.loginfo("Stopping the robot...") 182 | self.cmd_vel.publish(Twist()) 183 | rospy.sleep(1) 184 | 185 | if __name__ == '__main__': 186 | try: 187 | OutAndBack() 188 | except: 189 | rospy.loginfo("Out-and-Back node terminated.") 190 | -------------------------------------------------------------------------------- /dashgo_bringup/startup/dashgo-restart: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | log_file=/tmp/dashgo-upstart.log 4 | DATE=`date` 5 | echo "$DATE: dashgo-stop" >> $log_file 6 | 7 | /usr/sbin/dashgo-stop 8 | sleep 2 9 | /usr/sbin/dashgo-start 10 | -------------------------------------------------------------------------------- /dashgo_bringup/startup/dashgo-start: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | log_file=/tmp/dashgo-upstart.log 4 | DATE=`date` 5 | 6 | source /opt/ros/indigo/setup.bash 7 | source /home/pi/dashgo_ws/devel/setup.bash 8 | 9 | interface=wlan0 10 | echo "$DATE: dashgo-start on interface $interface" >> $log_file 11 | 12 | export ROS_IP=`ifconfig $interface | grep -o 'inet addr:[^ ]*' | cut -d: -f2` 13 | 14 | echo "$DATE: dashgo-start setting ROS_IP=$ROS_IP" >> $log_file 15 | 16 | if [ "$ROS_IP" = "" ]; then 17 | echo "$DATE: dashgo-start can't run with empty ROS_IP." >> $log_file 18 | exit 1 19 | fi 20 | 21 | roslaunch dashgo_bringup minimal.launch 22 | -------------------------------------------------------------------------------- /dashgo_bringup/startup/dashgo-stop: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | log_file=/tmp/dashgo-upstart.log 4 | DATE=`date` 5 | echo "$DATE: dashgo-stop" >> $log_file 6 | 7 | source /opt/ros/indigo/setup.bash 8 | source /home/pi/dashgo_ws/devel/setup.bash 9 | 10 | for i in $( rosnode list ); do 11 | rosnode kill $i; 12 | done 13 | 14 | killall roslaunch 15 | -------------------------------------------------------------------------------- /dashgo_bringup/startup/dashgo.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=dashgo startup 3 | 4 | [Service] 5 | Restart=always 6 | RestartSec=30 7 | ExecStart=/usr/sbin/dashgo-start 8 | ExecStop=/usr/sbin/dashgo-stop 9 | ExecRestart=/usr/sbin/dashgo-restart 10 | 11 | [Install] 12 | WantedBy=multi-user.target 13 | -------------------------------------------------------------------------------- /dashgo_bringup/startup/readme.txt: -------------------------------------------------------------------------------- 1 | cd 2 | sudo cp dashgo-start /usr/sbin/ 3 | sudo chmod +x /usr/sbin/dashgo-start 4 | 5 | 6 | sudo cp dashgo-stop /usr/sbin/ 7 | sudo chmod +x /usr/sbin/dashgo-stop 8 | 9 | 10 | sudo cp dashgo-restart /usr/sbin/ 11 | sudo chmod +x /usr/sbin/dashgo-restart 12 | 13 | sudo cp dashgo.service /lib/systemd/system/ 14 | 15 | 16 | ---------- 17 | sudo systemctl start dashgo.service 18 | sudo systemctl stop dashgo.service 19 | 20 | systemctl is-enabled dashgo.service 21 | 22 | sudo systemctl enable dashgo.service 23 | sudo systemctl disable dashgo.service 24 | -------------------------------------------------------------------------------- /dashgo_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dashgo_description) 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 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES dashgo_description 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(dashgo_description 122 | # src/${PROJECT_NAME}/dashgo_description.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(dashgo_description ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(dashgo_description_node src/dashgo_description_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(dashgo_description_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(dashgo_description_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS dashgo_description dashgo_description_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_dashgo_description.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /dashgo_description/launch/dashgo_description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /dashgo_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dashgo_description 4 | 0.0.0 5 | The dashgo_description package 6 | 7 | 8 | 9 | 10 | pi 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /dashgo_description/urdf/dashgobase/base.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /dashgo_description/urdf/dashgobase/dashgobase.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /dashgo_description/urdf/dashgobase/materials.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /dashgo_description/urdf/dashgobase/torso.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /dashgo_description/urdf/readme.txt: -------------------------------------------------------------------------------- 1 | https://github.com/turtlebot/turtlebot/tree/indigo/turtlebot_description 2 | -------------------------------------------------------------------------------- /dashgo_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dashgo_nav) 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 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES dashgo_nav 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(dashgo_nav 122 | # src/${PROJECT_NAME}/dashgo_nav.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(dashgo_nav ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(dashgo_nav_node src/dashgo_nav_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(dashgo_nav_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(dashgo_nav_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS dashgo_nav dashgo_nav_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_dashgo_nav.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /dashgo_nav/config/dashgo/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | controller_frequency: 3.0 2 | recovery_behavior_enabled: false 3 | clearing_rotation_allowed: false 4 | 5 | TrajectoryPlannerROS: 6 | max_vel_x: 0.3 7 | min_vel_x: 0.05 8 | max_vel_y: 0.0 # zero for a differential drive robot 9 | min_vel_y: 0.0 10 | min_in_place_vel_theta: 0.5 11 | escape_vel: -0.1 12 | acc_lim_x: 2.5 13 | acc_lim_y: 0.0 # zero for a differential drive robot 14 | acc_lim_theta: 3.2 15 | 16 | holonomic_robot: false 17 | yaw_goal_tolerance: 0.1 # about 6 degrees 18 | xy_goal_tolerance: 0.1 # 10 cm 19 | latch_xy_goal_tolerance: false 20 | pdist_scale: 0.8 21 | gdist_scale: 0.6 22 | meter_scoring: true 23 | 24 | heading_lookahead: 0.325 25 | heading_scoring: false 26 | heading_scoring_timestep: 0.8 27 | occdist_scale: 0.1 28 | oscillation_reset_dist: 0.05 29 | publish_cost_grid_pc: false 30 | prune_plan: true 31 | 32 | sim_time: 1.0 33 | sim_granularity: 0.025 34 | angular_sim_granularity: 0.025 35 | vx_samples: 8 36 | vy_samples: 0 # zero for a differential drive robot 37 | vtheta_samples: 20 38 | dwa: true 39 | simple_attractor: false 40 | -------------------------------------------------------------------------------- /dashgo_nav/config/dashgo/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | robot_radius: 0.165 4 | inflation_radius: 0.3 5 | max_obstacle_height: 0.6 6 | min_obstacle_height: 0.0 7 | observation_sources: scan 8 | scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0} 9 | -------------------------------------------------------------------------------- /dashgo_nav/config/dashgo/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 0 6 | static_map: true 7 | rolling_window: false 8 | resolution: 0.01 9 | transform_tolerance: 1.0 10 | map_type: costmap 11 | -------------------------------------------------------------------------------- /dashgo_nav/config/dashgo/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_footprint 4 | update_frequency: 3.0 5 | publish_frequency: 1.0 6 | static_map: false 7 | rolling_window: true 8 | width: 6.0 9 | height: 6.0 10 | resolution: 0.01 11 | transform_tolerance: 1.0 12 | map_type: costmap 13 | -------------------------------------------------------------------------------- /dashgo_nav/config/nav_obstacles_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | max_vel_x: 0.3 3 | pdist_scale: 0.8 4 | gdist_scale: 0.4 5 | -------------------------------------------------------------------------------- /dashgo_nav/config/nav_test_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | yaw_goal_tolerance: 6.28 # We don't care about orientation 3 | xy_goal_tolerance: 0.1 # 10 cm 4 | pdist_scale: 0.8 5 | gdist_scale: 0.4 6 | occdist_scale: 0.1 7 | -------------------------------------------------------------------------------- /dashgo_nav/launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /dashgo_nav/launch/move_base_blank_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /dashgo_nav/launch/move_base_obstacle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /dashgo_nav/maps/blank_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EAIBOT/dashgo/026ed64b7cd3c9c3d13cd55f2928b46fe94b5d76/dashgo_nav/maps/blank_map.pgm -------------------------------------------------------------------------------- /dashgo_nav/maps/blank_map.yaml: -------------------------------------------------------------------------------- 1 | image: blank_map.pgm 2 | resolution: 0.01 3 | origin: [-2.5, -2.5, 0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 # Taken from the Willow Garage map in the turtlebot_navigation package 6 | negate: 0 7 | -------------------------------------------------------------------------------- /dashgo_nav/maps/blank_map_with_obstacle.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EAIBOT/dashgo/026ed64b7cd3c9c3d13cd55f2928b46fe94b5d76/dashgo_nav/maps/blank_map_with_obstacle.pgm -------------------------------------------------------------------------------- /dashgo_nav/maps/blank_map_with_obstacle.yaml: -------------------------------------------------------------------------------- 1 | image: blank_map_with_obstacle.pgm 2 | resolution: 0.01 3 | origin: [-2.5, -2.5, 0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.195 # Taken from the Willow Garage map in the turtlebot_navigation package 6 | negate: 0 7 | -------------------------------------------------------------------------------- /dashgo_nav/maps/tb_condo_2.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EAIBOT/dashgo/026ed64b7cd3c9c3d13cd55f2928b46fe94b5d76/dashgo_nav/maps/tb_condo_2.pgm -------------------------------------------------------------------------------- /dashgo_nav/maps/tb_condo_2.yaml: -------------------------------------------------------------------------------- 1 | image: tb_condo_2.pgm 2 | resolution: 0.050000 3 | origin: [-12.200000, -13.800000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /dashgo_nav/maps/test_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EAIBOT/dashgo/026ed64b7cd3c9c3d13cd55f2928b46fe94b5d76/dashgo_nav/maps/test_map.pgm -------------------------------------------------------------------------------- /dashgo_nav/maps/test_map.yaml: -------------------------------------------------------------------------------- 1 | image: test_map.pgm 2 | resolution: 0.050000 3 | origin: [-13.800000, -12.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.9 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /dashgo_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dashgo_nav 4 | 0.0.0 5 | The dashgo_nav package 6 | 7 | 8 | 9 | 10 | pi 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | --------------------------------------------------------------------------------