├── src └── odrive_ros │ ├── __init__.py │ ├── odrive_simulator.py │ ├── odrive_interface.py │ └── odrive_node.py ├── nodes └── odrive_node ├── setup.py ├── package.xml ├── launch └── odrive.launch ├── LICENSE ├── .gitignore ├── CHANGELOG ├── README.md └── CMakeLists.txt /src/odrive_ros/__init__.py: -------------------------------------------------------------------------------- 1 | from odrive_node import ODriveNode, start_odrive -------------------------------------------------------------------------------- /nodes/odrive_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | 4 | import rospy 5 | import odrive_ros 6 | 7 | if __name__ == '__main__': 8 | try: 9 | odrive_ros.start_odrive() 10 | except rospy.ROSInterruptException: 11 | pass 12 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | setup_args = generate_distutils_setup( 7 | packages=['odrive_ros'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**setup_args) -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | odrive_ros 4 | 0.8.1 5 | ODrive motor controller driver for ROS 6 | Modified BSD 7 | 8 | Josh Marshall, Tecevo 9 | https://github.com/neomanic/odrive_ros 10 | 11 | catkin 12 | rospy 13 | rospy 14 | 15 | rospy 16 | std_msgs 17 | geometry_msgs 18 | nav_msgs 19 | std_srvs 20 | diagnostic_msgs 21 | diagnostic_updater 22 | 23 | ODrive 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/odrive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Modified BSD License 2 | 3 | Copyright 2018 Tecevo Pty Ltd 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 10 | 11 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 12 | 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | -------------------------------------------------------------------------------- /CHANGELOG: -------------------------------------------------------------------------------- 1 | ## 0.8.1 2019-09-25 2 | 3 | Add i2t calculation to diagnostics. 4 | 5 | ## 0.8.0 2019-09-24 6 | 7 | Publish diagnostics, including status from 0.7.0 and motor state. 8 | 9 | ## 0.7.0 2019-09-06 10 | 11 | Backed out automatic preroll, it's now done manually under user control. 12 | Status publishing on /odrive/status topic, so you can check connection/preroll/ready state externally. 13 | Temperature reading. 14 | 15 | ## 0.6.0 2019-07-24 16 | 17 | Added more careful error checking, in particular looking for USB comms loss vs other errors 18 | Watchdog timer now used. 19 | 20 | ## 0.5.0 2019-07-15 21 | 22 | Added facility to import odrive_interface into an odrivetool shell for testing. 23 | More error checking and recovery. 24 | Initial work towards providing a simulation mode for use in gazebo. 25 | 26 | ## 0.4.0 2019-03-19 27 | 28 | Reworked all ODrive comms into a fast+slow thread. 29 | Added error recovery on comms failure. 30 | Added auto preroll on first drive. 31 | 32 | ## 0.3.1 2018-11-01 33 | 34 | Max speed and angular velocity parameter. 35 | 36 | ## 0.3.0 2018-10-15 37 | 38 | Now uses the ODrive `preroll` branch to allow use of encoders with no index. 39 | Set ROS param `use_preroll` to False for old behaviour. 40 | 41 | Added diff-drive odometry calculation and publishing to both ROS topic and tf 42 | Added current calculation 43 | 44 | Pulls encoder counts from driver 45 | 46 | ## 0.2.1 2018-10-03 47 | 48 | Bug in startup sequence (calibrated twice) 49 | 50 | ## 0.2.0 2018-09-25 51 | 52 | Various services for connecting, calibrating, engage/release, etc. 53 | Parameters to set whether connection and calibration happens on startup. 54 | 55 | ## 0.1.2 2018-09-12 56 | 57 | Abstracted left/right motors up into the node, so axis0 and axis1 can be assigned to either. 58 | Fixed dumb bug in cleaning up node class. 59 | 60 | ## 0.1.1 2018-09-10 61 | 62 | - Switched to modified BSD license from MIT, same as ROS proper. 63 | 64 | ## 0.1.0 2018-09-07 65 | 66 | - Turned into a proper ROS package: just need to checkout repository into your workspace's src/ dir. 67 | 68 | ## 2018-07-31 69 | 70 | - Initial commit 71 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ARCHIVE NOTICE 2021-02-04 2 | I have taken on a new position no longer have access to an ODrive for testing. Happy to add a maintainer role if anyone wants to step up to it, otherwise this repo will just stay up here as a reference for other implementations. 3 | 4 | # odrive_ros 5 | ROS driver for the [ODrive motor driver](https://odriverobotics.com/) 6 | 7 | This is a basic pass at a ROS driver to operate the ODrive as a differential drive. It's Python-based, so not super-fast, but it'll get you started. Maybe in time this will have an optimised C++ version, but I woudn't count on it anytime soon. ;) 8 | 9 | Future plans include adding a simulation mode so you can continue to use this driver in a simulation mode within gazebo. 10 | 11 | Feedback, issues and pull requests welcome. 12 | 13 | ## Usage 14 | 15 | You will need the main ODrive Python tools installed. 16 | 17 | To install: 18 | ```sh 19 | git clone https://github.com/madcowswe/ODrive 20 | cd ODrive/tools 21 | sudo pip install monotonic # required for py < 3 22 | 23 | # sudo python setup.py install # doesn't work due to weird setup process, so do the following: 24 | python setup.py sdist 25 | sudo pip install dist/odrive-xxx.tar.gz 26 | ``` 27 | 28 | then `rosrun odrive_ros odrive_node` will get you on your way. 29 | 30 | There is a sample launch file showing the use of the wheelbase, tyre circumference, and encoder count parameters too, try `roslaunch odrive_ros odrive.launch` or copy it into your own package and edit the values to correspond to your particular setup. 31 | 32 | If you want to test out the driver separate from ROS, you can also install as a regular Python package. 33 | 34 | ```sh 35 | roscd odrive_ros 36 | sudo pip install -e . 37 | ``` 38 | 39 | If you want to use a bare Python shell, see below for the import which exposes the ODrive with setup and drive functions. You can use either ODriveInterfaceAPI (uses USB) or ODriveInterfaceSerial (requires the /dev/ttyUSBx corresponding to the ODrive as parameter, but not tested recently). 40 | 41 | ```python 42 | from odrive_ros import odrive_interface 43 | od = odrive_interface.ODriveInterfaceAPI() 44 | od.connect() 45 | od.calibrate() # does a calibration 46 | od.engage() # get ready to drive 47 | od.drive(1000,1000) # drive axis0 and axis1 48 | od.release() # done 49 | ``` 50 | 51 | You can now (as of v0.5) use this wrapper from within an `odrivetool shell`. Once the shell has launched and connected to your ODrive, run the following, then you can use the commands as if in a bare Python interactive session above, and still get the help provided by odrivetool. 52 | 53 | ```python 54 | import odrive_interface 55 | od = odrive_interface.ODriveInterfaceAPI(active_odrive=odrv0) 56 | ``` 57 | 58 | 59 | ## Acknowledgements 60 | 61 | Thanks to the ODrive team for a great little bit of hardware and the active community support. Hope this helps! 62 | 63 | - [ODrive homepage](https://odriverobotics.com) 64 | - [ODrive getting started](https://docs.odriverobotics.com) 65 | - [ODrive main repo](https://github.com/madcowswe/ODrive) 66 | 67 | Thanks to Simon Birrell for his tutorial on [how to package a Python-based ROS package](http://www.artificialhumancompanions.com/structure-python-based-ros-package/). 68 | 69 | -------------------------------------------------------------------------------- /src/odrive_ros/odrive_simulator.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import time 3 | import logging 4 | import traceback 5 | 6 | default_logger = logging.getLogger(__name__) 7 | default_logger.setLevel(logging.DEBUG) 8 | 9 | # create console handler and set level to debug 10 | ch = logging.StreamHandler() 11 | ch.setLevel(logging.DEBUG) 12 | 13 | default_logger.addHandler(ch) 14 | 15 | class ODriveInterfaceSimulator(object): 16 | encoder_cpr = 4096 17 | connected = False 18 | engaged = False 19 | 20 | right_axis_vel = 0 # units: encoder counts/s 21 | left_axis_vel = 0 22 | right_axis_pos = 0 # go from 0 up to encoder_cpr-1 23 | left_axis_pos = 0 24 | last_time_update = None 25 | 26 | 27 | def __init__(self, logger=None): 28 | self.logger = logger if logger else default_logger 29 | 30 | def update_time(self, curr_time): 31 | # provided so simulator can update position 32 | if last_time_update is None: 33 | last_time_update = curr_time 34 | return 35 | 36 | dt = curr_time - last_time_update 37 | left_axis_pos = floor(left_axis_pos + left_axis_vel *dt) % self.encoder_cpr 38 | right_axis_pos = floor(right_axis_pos + right_axis_vel*dt) % self.encoder_cpr 39 | last_time_update = curr_time 40 | 41 | def connect(self, port=None, right_axis=0, timeout=30): 42 | if self.connected: 43 | self.logger.info("Already connected. Simulating disc/reconnect.") 44 | 45 | self.encoder_cpr = 4096 46 | self.logger.info("Connected to simulated ODrive.") 47 | return True 48 | 49 | def disconnect(self): 50 | self.connected = False 51 | return True 52 | 53 | def calibrate(self): 54 | if not self.connected: 55 | self.logger.error("Not connected.") 56 | return False 57 | self.logger.info("Calibrated.") 58 | return True 59 | 60 | def preroll(self, wait=True, reverse=False): 61 | if not self.connected: 62 | self.logger.error("Not connected.") 63 | return False 64 | 65 | return True 66 | 67 | def ensure_prerolled(self): 68 | return True 69 | 70 | def engaged(self): 71 | return self.engaged 72 | 73 | def engage(self): 74 | if not self.connected: 75 | self.logger.error("Not connected.") 76 | return False 77 | self.engaged = True 78 | return True 79 | 80 | def release(self): 81 | if not self.connected: 82 | self.logger.error("Not connected.") 83 | return False 84 | self.engaged = False 85 | return True 86 | 87 | def drive(self, left_motor_val, right_motor_val): 88 | if not self.connnected: 89 | self.logger.error("Not connected.") 90 | return 91 | self.left_axis.controller.vel_setpoint = left_motor_val 92 | self.right_axis.controller.vel_setpoint = -right_motor_val 93 | 94 | return True 95 | 96 | def get_errors(self, clear=True): 97 | if not self.driver: 98 | return None 99 | return "Simulated ODrive, no errors." 100 | 101 | def left_vel_estimate(self): return self.left_axis_vel 102 | def right_vel_estimate(self): return self.right_axis_vel 103 | def left_pos(self): return self.left_axis_pos 104 | def right_pos(self): return self.right_axis_pos 105 | 106 | def left_current(self): return 0 107 | def right_current(self): return 0 108 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(odrive_ros) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | geometry_msgs 14 | diagnostic_msgs 15 | diagnostic_updater 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # std_msgs # Or other packages containing msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES odrive_ros 110 | # CATKIN_DEPENDS rospy 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | # add_library(${PROJECT_NAME} 127 | # src/${PROJECT_NAME}/odrive_ros.cpp 128 | # ) 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | ## Declare a C++ executable 136 | ## With catkin_make all packages are built within a single CMake context 137 | ## The recommended prefix ensures that target names across packages don't collide 138 | # add_executable(${PROJECT_NAME}_node src/odrive_ros_node.cpp) 139 | 140 | ## Rename C++ executable without prefix 141 | ## The above recommended prefix causes long target names, the following renames the 142 | ## target back to the shorter version for ease of user use 143 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 144 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(${PROJECT_NAME}_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | install(PROGRAMS 165 | nodes/odrive_node 166 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | ) 168 | 169 | ## Mark executables and/or libraries for installation 170 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 171 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark cpp header files for installation 177 | # install(DIRECTORY include/${PROJECT_NAME}/ 178 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 179 | # FILES_MATCHING PATTERN "*.h" 180 | # PATTERN ".svn" EXCLUDE 181 | # ) 182 | 183 | ## Mark other files for installation (e.g. launch and bag files, etc.) 184 | # install(FILES 185 | # # myfile1 186 | # # myfile2 187 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 188 | # ) 189 | 190 | ############# 191 | ## Testing ## 192 | ############# 193 | 194 | ## Add gtest based cpp test target and link libraries 195 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_odrive_ros.cpp) 196 | # if(TARGET ${PROJECT_NAME}-test) 197 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 198 | # endif() 199 | 200 | ## Add folders to be run by python nosetests 201 | # catkin_add_nosetests(test) 202 | -------------------------------------------------------------------------------- /src/odrive_ros/odrive_interface.py: -------------------------------------------------------------------------------- 1 | import serial 2 | from serial.serialutil import SerialException 3 | 4 | import sys 5 | import time 6 | import logging 7 | import traceback 8 | 9 | import odrive 10 | from odrive.enums import * 11 | 12 | import fibre 13 | from fibre import ChannelBrokenException, ChannelDamagedException 14 | 15 | default_logger = logging.getLogger(__name__) 16 | default_logger.setLevel(logging.DEBUG) 17 | 18 | # create console handler and set level to debug 19 | ch = logging.StreamHandler() 20 | ch.setLevel(logging.DEBUG) 21 | 22 | default_logger.addHandler(ch) 23 | 24 | class ODriveFailure(Exception): 25 | pass 26 | 27 | class ODriveInterfaceAPI(object): 28 | driver = None 29 | encoder_cpr = 4096 30 | right_axis = None 31 | left_axis = None 32 | connected = False 33 | _preroll_started = False 34 | _preroll_completed = False 35 | #engaged = False 36 | 37 | def __init__(self, logger=None, active_odrive=None): 38 | self.logger = logger if logger else default_logger 39 | 40 | if active_odrive: # pass in the odrv0 object from odrivetool shell to use it directly. 41 | self.driver = active_odrive 42 | self.axes = (self.driver.axis0, self.driver.axis1) 43 | self.right_axis = self.driver.axis0 44 | self.left_axis = self.driver.axis1 45 | self.logger.info("Loaded pre-existing ODrive interface. Check index search status.") 46 | self.encoder_cpr = self.driver.axis0.encoder.config.cpr 47 | self.connected = True 48 | self._preroll_started = False 49 | self._preroll_completed = True 50 | 51 | def __del__(self): 52 | self.disconnect() 53 | 54 | def update_time(self, curr_time): 55 | # provided so simulator can update position 56 | pass 57 | 58 | def connect(self, port=None, right_axis=0, timeout=30): 59 | if self.driver: 60 | self.logger.info("Already connected. Disconnecting and reconnecting.") 61 | try: 62 | self.driver = odrive.find_any(timeout=timeout, logger=self.logger) 63 | self.axes = (self.driver.axis0, self.driver.axis1) 64 | except: 65 | self.logger.error("No ODrive found. Is device powered?") 66 | return False 67 | 68 | # save some parameters for easy access 69 | self.right_axis = self.driver.axis0 if right_axis == 0 else self.driver.axis1 70 | self.left_axis = self.driver.axis1 if right_axis == 0 else self.driver.axis0 71 | 72 | # check for no errors 73 | for axis in [self.right_axis, self.left_axis]: 74 | if axis.error != 0: 75 | error_str = "Had error on startup, rebooting. Axis error 0x%x, motor error 0x%x, encoder error 0x%x. Rebooting." % (axis.error, axis.motor.error, axis.encoder.error) 76 | self.logger.error(error_str) 77 | self.reboot() 78 | return False 79 | 80 | self.encoder_cpr = self.driver.axis0.encoder.config.cpr 81 | 82 | self.connected = True 83 | self.logger.info("Connected to ODrive. " + self.get_version_string()) 84 | 85 | self._preroll_started = False 86 | self._preroll_completed = False 87 | 88 | return True 89 | 90 | def disconnect(self): 91 | self.connected = False 92 | self.right_axis = None 93 | self.left_axis = None 94 | 95 | #self.engaged = False 96 | 97 | if not self.driver: 98 | self.logger.error("Not connected.") 99 | return False 100 | 101 | try: 102 | self.release() 103 | except: 104 | self.logger.error("Error in timer: " + traceback.format_exc()) 105 | return False 106 | finally: 107 | self.driver = None 108 | return True 109 | 110 | def get_version_string(self): 111 | if not self.driver or not self.connected: 112 | return "Not connected." 113 | return "ODrive %s, hw v%d.%d-%d, fw v%d.%d.%d%s, sdk v%s" % ( 114 | str(self.driver.serial_number), 115 | self.driver.hw_version_major, self.driver.hw_version_minor, self.driver.hw_version_variant, 116 | self.driver.fw_version_major, self.driver.fw_version_minor, self.driver.fw_version_revision, 117 | "-dev" if self.driver.fw_version_unreleased else "", 118 | odrive.version.get_version_str()) 119 | 120 | 121 | def reboot(self): 122 | if not self.driver: 123 | self.logger.error("Not connected.") 124 | return False 125 | try: 126 | self.driver.reboot() 127 | except KeyError: 128 | self.logger.error("Rebooted ODrive.") 129 | except: 130 | self.logger.error("Failed to reboot: " + traceback.format_exc()) 131 | finally: 132 | self.driver = None 133 | return True 134 | 135 | def calibrate(self): 136 | if not self.driver: 137 | self.logger.error("Not connected.") 138 | return False 139 | 140 | self.logger.info("Vbus %.2fV" % self.driver.vbus_voltage) 141 | 142 | for i, axis in enumerate(self.axes): 143 | self.logger.info("Calibrating axis %d..." % i) 144 | axis.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE 145 | time.sleep(1) 146 | while axis.current_state != AXIS_STATE_IDLE: 147 | time.sleep(0.1) 148 | if axis.error != 0: 149 | self.logger.error("Failed calibration with axis error 0x%x, motor error 0x%x" % (axis.error, axis.motor.error)) 150 | return False 151 | 152 | return True 153 | 154 | def preroll(self, wait=True): 155 | if not self.driver: 156 | self.logger.error("Not connected.") 157 | return False 158 | 159 | if self._preroll_started: # must be prerolling or already prerolled 160 | return False 161 | self._preroll_started = True 162 | self._preroll_completed = False 163 | 164 | #self.logger.info("Vbus %.2fV" % self.driver.vbus_voltage) 165 | 166 | for i, axis in enumerate(self.axes): 167 | self.logger.info("Index search preroll axis %d..." % i) 168 | axis.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH 169 | 170 | if wait: 171 | for i, axis in enumerate(self.axes): 172 | while axis.current_state != AXIS_STATE_IDLE: 173 | time.sleep(0.1) 174 | self._preroll_started = False 175 | for i, axis in enumerate(self.axes): 176 | if axis.error != 0: 177 | self.logger.error("Failed preroll with left_axis error 0x%x, motor error 0x%x" % (axis.error, axis.motor.error)) 178 | return False 179 | self._preroll_completed = True 180 | self.logger.info("Index search preroll complete.") 181 | return True 182 | else: 183 | return False 184 | 185 | # def prerolling(self): 186 | # return self.axes[0].current_state == AXIS_STATE_ENCODER_INDEX_SEARCH or self.axes[1].current_state == AXIS_STATE_ENCODER_INDEX_SEARCH 187 | # 188 | # def prerolled(self): # 189 | # return self._prerolled and not self.prerolling() 190 | 191 | def ensure_prerolled(self): 192 | # preroll success 193 | if self._preroll_completed: 194 | return True 195 | # started, not completed 196 | elif self._preroll_started: 197 | #self.logger.info("Checking for preroll complete.") 198 | if self.axes[0].current_state != AXIS_STATE_ENCODER_INDEX_SEARCH and self.axes[1].current_state != AXIS_STATE_ENCODER_INDEX_SEARCH: 199 | # completed, check for errors before marking complete 200 | for i, axis in enumerate(self.axes): 201 | if axis.error != 0: 202 | self._preroll_started = False 203 | error_str = "Failed index search preroll with axis error 0x%x, motor error 0x%x, encoder error 0x%x. Rebooting." % (axis.error, axis.motor.error, axis.encoder.error) 204 | #self.reboot() 205 | self.logger.error(error_str) 206 | raise Exception(error_str) 207 | # no errors, success 208 | self._preroll_started = False 209 | self._preroll_completed = True 210 | self.logger.info("Index search preroll complete. Ready to drive.") 211 | return True 212 | else: 213 | # still prerolling 214 | return False 215 | else: # start preroll 216 | #self.logger.info("Preroll started.") 217 | self.preroll(wait=False) 218 | return False 219 | 220 | def has_prerolled(self): 221 | return self._preroll_completed 222 | 223 | def engaged(self): 224 | if self.driver and hasattr(self, 'axes'): 225 | return self.axes[0].current_state == AXIS_STATE_CLOSED_LOOP_CONTROL and self.axes[1].current_state == AXIS_STATE_CLOSED_LOOP_CONTROL 226 | else: 227 | return False 228 | 229 | def idle(self): 230 | if self.driver and hasattr(self, 'axes'): 231 | return self.axes[0].current_state == AXIS_STATE_IDLE and self.axes[1].current_state == AXIS_STATE_IDLE 232 | else: 233 | return False 234 | 235 | def engage(self): 236 | if not self.driver: 237 | self.logger.error("Not connected.") 238 | return False 239 | 240 | #self.logger.debug("Setting drive mode.") 241 | for axis in self.axes: 242 | axis.controller.vel_setpoint = 0 243 | axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL 244 | axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL 245 | 246 | #self.engaged = True 247 | return True 248 | 249 | def release(self): 250 | if not self.driver: 251 | self.logger.error("Not connected.") 252 | return False 253 | #self.logger.debug("Releasing.") 254 | for axis in self.axes: 255 | axis.requested_state = AXIS_STATE_IDLE 256 | 257 | #self.engaged = False 258 | return True 259 | 260 | def drive(self, left_motor_val, right_motor_val): 261 | if not self.driver: 262 | self.logger.error("Not connected.") 263 | return 264 | #try: 265 | self.left_axis.controller.vel_setpoint = left_motor_val 266 | self.right_axis.controller.vel_setpoint = -right_motor_val 267 | #except (fibre.protocol.ChannelBrokenException, AttributeError) as e: 268 | # raise ODriveFailure(str(e)) 269 | 270 | def feed_watchdog(self): 271 | self.left_axis.watchdog_feed() 272 | self.right_axis.watchdog_feed() 273 | 274 | def get_errors(self, clear=True): 275 | # TODO: add error parsing, see: https://github.com/madcowswe/ODrive/blob/master/tools/odrive/utils.py#L34 276 | if not self.driver: 277 | return None 278 | 279 | axis_error = self.axes[0].error or self.axes[1].error 280 | 281 | if axis_error: 282 | error_string = "Errors(hex): L: a%x m%x e%x c%x, R: a%x m%x e%x c%x" % ( 283 | self.left_axis.error, self.left_axis.motor.error, self.left_axis.encoder.error, self.left_axis.controller.error, 284 | self.right_axis.error, self.right_axis.motor.error, self.right_axis.encoder.error, self.right_axis.controller.error, 285 | ) 286 | 287 | if clear: 288 | for axis in self.axes: 289 | axis.error = 0 290 | axis.motor.error = 0 291 | axis.encoder.error = 0 292 | axis.controller.error = 0 293 | 294 | if axis_error: 295 | return error_string 296 | 297 | def left_vel_estimate(self): return self.left_axis.encoder.vel_estimate if self.left_axis else 0 # units: encoder counts/s 298 | def right_vel_estimate(self): return self.right_axis.encoder.vel_estimate if self.right_axis else 0 # neg is forward for right 299 | def left_pos(self): return self.left_axis.encoder.pos_cpr if self.left_axis else 0 # units: encoder counts 300 | def right_pos(self): return self.right_axis.encoder.pos_cpr if self.right_axis else 0 # sign! 301 | 302 | # TODO check these match the right motors, but it doesn't matter for now 303 | def left_temperature(self): return self.left_axis.motor.get_inverter_temp() if self.left_axis else 0. 304 | def right_temperature(self): return self.right_axis.motor.get_inverter_temp() if self.right_axis else 0. 305 | 306 | def left_current(self): return self.left_axis.motor.current_control.Ibus if self.left_axis and self.left_axis.current_state > 1 else 0. 307 | def right_current(self): return self.right_axis.motor.current_control.Ibus if self.right_axis and self.right_axis.current_state > 1 else 0. 308 | 309 | # from axis.hpp: https://github.com/madcowswe/ODrive/blob/767a2762f9b294b687d761029ef39e742bdf4539/Firmware/MotorControl/axis.hpp#L26 310 | MOTOR_STATES = [ 311 | "UNDEFINED", # motor speed conversion 67 | wheel_track = None 68 | tyre_circumference = None 69 | encoder_counts_per_rev = None 70 | m_s_to_value = 1.0 71 | axis_for_right = 0 72 | encoder_cpr = 4096 73 | 74 | # Startup parameters 75 | connect_on_startup = False 76 | calibrate_on_startup = False 77 | engage_on_startup = False 78 | 79 | publish_joint_angles = True 80 | # Simulation mode 81 | # When enabled, output simulated odometry and joint angles (TODO: do joint angles anyway from ?) 82 | sim_mode = False 83 | 84 | def __init__(self): 85 | self.sim_mode = get_param('simulation_mode', False) 86 | self.publish_joint_angles = get_param('publish_joint_angles', True) # if self.sim_mode else False 87 | self.publish_temperatures = get_param('publish_temperatures', True) 88 | 89 | self.axis_for_right = float(get_param('~axis_for_right', 0)) # if right calibrates first, this should be 0, else 1 90 | self.wheel_track = float(get_param('~wheel_track', 0.285)) # m, distance between wheel centres 91 | self.tyre_circumference = float(get_param('~tyre_circumference', 0.341)) # used to translate velocity commands in m/s into motor rpm 92 | 93 | self.connect_on_startup = get_param('~connect_on_startup', False) 94 | #self.calibrate_on_startup = get_param('~calibrate_on_startup', False) 95 | #self.engage_on_startup = get_param('~engage_on_startup', False) 96 | 97 | self.has_preroll = get_param('~use_preroll', True) 98 | 99 | self.publish_current = get_param('~publish_current', True) 100 | self.publish_raw_odom =get_param('~publish_raw_odom', True) 101 | 102 | self.publish_odom = get_param('~publish_odom', True) 103 | self.publish_tf = get_param('~publish_odom_tf', False) 104 | self.odom_topic = get_param('~odom_topic', "odom") 105 | self.odom_frame = get_param('~odom_frame', "odom") 106 | self.base_frame = get_param('~base_frame', "base_link") 107 | self.odom_calc_hz = get_param('~odom_calc_hz', 10) 108 | 109 | rospy.on_shutdown(self.terminate) 110 | 111 | rospy.Service('connect_driver', std_srvs.srv.Trigger, self.connect_driver) 112 | rospy.Service('disconnect_driver', std_srvs.srv.Trigger, self.disconnect_driver) 113 | 114 | rospy.Service('calibrate_motors', std_srvs.srv.Trigger, self.calibrate_motor) 115 | rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor) 116 | rospy.Service('release_motors', std_srvs.srv.Trigger, self.release_motor) 117 | 118 | # odometry update, disable during preroll, whenever wheels off ground 119 | self.odometry_update_enabled = True 120 | rospy.Service('enable_odometry_updates', std_srvs.srv.SetBool, self.enable_odometry_update_service) 121 | 122 | self.status_pub = rospy.Publisher('status', std_msgs.msg.String, latch=True, queue_size=2) 123 | self.status = "disconnected" 124 | self.status_pub.publish(self.status) 125 | 126 | self.command_queue = Queue.Queue(maxsize=5) 127 | self.vel_subscribe = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=2) 128 | 129 | self.publish_diagnostics = True 130 | if self.publish_diagnostics: 131 | self.diagnostic_updater = diagnostic_updater.Updater() 132 | self.diagnostic_updater.setHardwareID("Not connected, unknown") 133 | self.diagnostic_updater.add("ODrive Diagnostics", self.pub_diagnostics) 134 | 135 | if self.publish_temperatures: 136 | self.temperature_publisher_left = rospy.Publisher('left/temperature', Float64, queue_size=2) 137 | self.temperature_publisher_right = rospy.Publisher('right/temperature', Float64, queue_size=2) 138 | 139 | self.i2t_error_latch = False 140 | if self.publish_current: 141 | #self.current_loop_count = 0 142 | #self.left_current_accumulator = 0.0 143 | #self.right_current_accumulator = 0.0 144 | self.current_publisher_left = rospy.Publisher('left/current', Float64, queue_size=2) 145 | self.current_publisher_right = rospy.Publisher('right/current', Float64, queue_size=2) 146 | self.i2t_publisher_left = rospy.Publisher('left/i2t', Float64, queue_size=2) 147 | self.i2t_publisher_right = rospy.Publisher('right/i2t', Float64, queue_size=2) 148 | 149 | rospy.logdebug("ODrive will publish motor currents.") 150 | 151 | self.i2t_resume_threshold = get_param('~i2t_resume_threshold', 222) 152 | self.i2t_warning_threshold = get_param('~i2t_warning_threshold', 333) 153 | self.i2t_error_threshold = get_param('~i2t_error_threshold', 666) 154 | 155 | self.last_cmd_vel_time = rospy.Time.now() 156 | 157 | if self.publish_raw_odom: 158 | self.raw_odom_publisher_encoder_left = rospy.Publisher('left/raw_odom/encoder', Int32, queue_size=2) if self.publish_raw_odom else None 159 | self.raw_odom_publisher_encoder_right = rospy.Publisher('right/raw_odom/encoder', Int32, queue_size=2) if self.publish_raw_odom else None 160 | self.raw_odom_publisher_vel_left = rospy.Publisher('left/raw_odom/velocity', Int32, queue_size=2) if self.publish_raw_odom else None 161 | self.raw_odom_publisher_vel_right = rospy.Publisher('right/raw_odom/velocity', Int32, queue_size=2) if self.publish_raw_odom else None 162 | 163 | if self.publish_odom: 164 | rospy.Service('reset_odometry', std_srvs.srv.Trigger, self.reset_odometry) 165 | self.old_pos_l = 0 166 | self.old_pos_r = 0 167 | 168 | self.odom_publisher = rospy.Publisher(self.odom_topic, Odometry, tcp_nodelay=True, queue_size=2) 169 | # setup message 170 | self.odom_msg = Odometry() 171 | #print(dir(self.odom_msg)) 172 | self.odom_msg.header.frame_id = self.odom_frame 173 | self.odom_msg.child_frame_id = self.base_frame 174 | self.odom_msg.pose.pose.position.x = 0.0 175 | self.odom_msg.pose.pose.position.y = 0.0 176 | self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope 177 | self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical 178 | self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical 179 | self.odom_msg.pose.pose.orientation.z = 0.0 180 | self.odom_msg.pose.pose.orientation.w = 1.0 181 | self.odom_msg.twist.twist.linear.x = 0.0 182 | self.odom_msg.twist.twist.linear.y = 0.0 # no sideways 183 | self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward 184 | self.odom_msg.twist.twist.angular.x = 0.0 # or roll 185 | self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw 186 | self.odom_msg.twist.twist.angular.z = 0.0 187 | 188 | # store current location to be updated. 189 | self.x = 0.0 190 | self.y = 0.0 191 | self.theta = 0.0 192 | 193 | # setup transform 194 | self.tf_publisher = tf2_ros.TransformBroadcaster() 195 | self.tf_msg = TransformStamped() 196 | self.tf_msg.header.frame_id = self.odom_frame 197 | self.tf_msg.child_frame_id = self.base_frame 198 | self.tf_msg.transform.translation.x = 0.0 199 | self.tf_msg.transform.translation.y = 0.0 200 | self.tf_msg.transform.translation.z = 0.0 201 | self.tf_msg.transform.rotation.x = 0.0 202 | self.tf_msg.transform.rotation.y = 0.0 203 | self.tf_msg.transform.rotation.w = 0.0 204 | self.tf_msg.transform.rotation.z = 1.0 205 | 206 | if self.publish_joint_angles: 207 | self.joint_state_publisher = rospy.Publisher('/odrive/joint_states', JointState, queue_size=2) 208 | 209 | jsm = JointState() 210 | self.joint_state_msg = jsm 211 | #jsm.name.resize(2) 212 | #jsm.position.resize(2) 213 | jsm.name = ['joint_left_wheel','joint_right_wheel'] 214 | jsm.position = [0.0, 0.0] 215 | 216 | 217 | def main_loop(self): 218 | # Main control, handle startup and error handling 219 | # while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs 220 | main_rate = rospy.Rate(1) # hz 221 | # Start timer to run high-rate comms 222 | self.fast_timer = rospy.Timer(rospy.Duration(1/float(self.odom_calc_hz)), self.fast_timer) 223 | 224 | self.fast_timer_comms_active = False 225 | 226 | while not rospy.is_shutdown(): 227 | try: 228 | main_rate.sleep() 229 | except rospy.ROSInterruptException: # shutdown / stop ODrive?? 230 | break; 231 | 232 | # fast timer running, so do nothing and wait for any errors 233 | if self.fast_timer_comms_active: 234 | continue 235 | 236 | # check for errors 237 | if self.driver: 238 | try: 239 | # driver connected, but fast_comms not active -> must be an error? 240 | # TODO: try resetting errors and recalibrating, not just a full disconnection 241 | error_string = self.driver.get_errors(clear=True) 242 | if error_string: 243 | rospy.logerr("Had errors, disconnecting and retrying connection.") 244 | rospy.logerr(error_string) 245 | self.driver.disconnect() 246 | self.status = "disconnected" 247 | self.status_pub.publish(self.status) 248 | self.driver = None 249 | else: 250 | # must have called connect service from another node 251 | self.fast_timer_comms_active = True 252 | except (ChannelBrokenException, ChannelDamagedException, AttributeError): 253 | rospy.logerr("ODrive USB connection failure in main_loop.") 254 | self.status = "disconnected" 255 | self.status_pub.publish(self.status) 256 | self.driver = None 257 | except: 258 | rospy.logerr("Unknown errors accessing ODrive:" + traceback.format_exc()) 259 | self.status = "disconnected" 260 | self.status_pub.publish(self.status) 261 | self.driver = None 262 | 263 | if not self.driver: 264 | if not self.connect_on_startup: 265 | #rospy.loginfo("ODrive node started, but not connected.") 266 | continue 267 | 268 | if not self.connect_driver(None)[0]: 269 | rospy.logerr("Failed to connect.") # TODO: can we check for timeout here? 270 | continue 271 | 272 | if self.publish_diagnostics: 273 | self.diagnostic_updater.setHardwareID(self.driver.get_version_string()) 274 | 275 | else: 276 | pass # loop around and try again 277 | 278 | def fast_timer(self, timer_event): 279 | time_now = rospy.Time.now() 280 | # in case of failure, assume some values are zero 281 | self.vel_l = 0 282 | self.vel_r = 0 283 | self.new_pos_l = 0 284 | self.new_pos_r = 0 285 | self.current_l = 0 286 | self.current_r = 0 287 | self.temp_v_l = 0. 288 | self.temp_v_r = 0. 289 | self.motor_state_l = "not connected" # undefined 290 | self.motor_state_r = "not connected" 291 | self.bus_voltage = 0. 292 | 293 | # Handle reading from Odrive and sending odometry 294 | if self.fast_timer_comms_active: 295 | try: 296 | # check errors 297 | error_string = self.driver.get_errors() 298 | if error_string: 299 | self.fast_timer_comms_active = False 300 | else: 301 | # reset watchdog 302 | self.driver.feed_watchdog() 303 | 304 | # read all required values from ODrive for odometry 305 | self.motor_state_l = self.driver.left_state() 306 | self.motor_state_r = self.driver.right_state() 307 | 308 | self.encoder_cpr = self.driver.encoder_cpr 309 | self.m_s_to_value = self.encoder_cpr/self.tyre_circumference # calculated 310 | 311 | self.driver.update_time(time_now.to_sec()) 312 | self.vel_l = self.driver.left_vel_estimate() # units: encoder counts/s 313 | self.vel_r = -self.driver.right_vel_estimate() # neg is forward for right 314 | self.new_pos_l = self.driver.left_pos() # units: encoder counts 315 | self.new_pos_r = -self.driver.right_pos() # sign! 316 | 317 | # for temperatures 318 | self.temp_v_l = self.driver.left_temperature() 319 | self.temp_v_r = self.driver.right_temperature() 320 | # for current 321 | self.current_l = self.driver.left_current() 322 | self.current_r = self.driver.right_current() 323 | # voltage 324 | self.bus_voltage = self.driver.bus_voltage() 325 | 326 | except (ChannelBrokenException, ChannelDamagedException): 327 | rospy.logerr("ODrive USB connection failure in fast_timer." + traceback.format_exc(1)) 328 | self.fast_timer_comms_active = False 329 | self.status = "disconnected" 330 | self.status_pub.publish(self.status) 331 | self.driver = None 332 | except: 333 | rospy.logerr("Fast timer ODrive failure:" + traceback.format_exc()) 334 | self.fast_timer_comms_active = False 335 | 336 | # odometry is published regardless of ODrive connection or failure (but assumed zero for those) 337 | # as required by SLAM 338 | if self.publish_odom: 339 | self.pub_odometry(time_now) 340 | if self.publish_temperatures: 341 | self.pub_temperatures() 342 | if self.publish_current: 343 | self.pub_current() 344 | if self.publish_joint_angles: 345 | self.pub_joint_angles(time_now) 346 | if self.publish_diagnostics: 347 | self.diagnostic_updater.update() 348 | 349 | try: 350 | # check and stop motor if no vel command has been received in > 1s 351 | #if self.fast_timer_comms_active: 352 | if self.driver: 353 | if (time_now - self.last_cmd_vel_time).to_sec() > 0.5 and self.last_speed > 0: 354 | self.driver.drive(0,0) 355 | self.last_speed = 0 356 | self.last_cmd_vel_time = time_now 357 | # release motor after 10s stopped 358 | if (time_now - self.last_cmd_vel_time).to_sec() > 10.0 and self.driver.engaged(): 359 | self.driver.release() # and release 360 | except (ChannelBrokenException, ChannelDamagedException): 361 | rospy.logerr("ODrive USB connection failure in cmd_vel timeout." + traceback.format_exc(1)) 362 | self.fast_timer_comms_active = False 363 | self.driver = None 364 | except: 365 | rospy.logerr("cmd_vel timeout unknown failure:" + traceback.format_exc()) 366 | self.fast_timer_comms_active = False 367 | 368 | 369 | # handle sending drive commands. 370 | # from here, any errors return to get out 371 | if self.fast_timer_comms_active and not self.command_queue.empty(): 372 | # check to see if we're initialised and engaged motor 373 | try: 374 | if not self.driver.has_prerolled(): #ensure_prerolled(): 375 | rospy.logwarn_throttle(5.0, "ODrive has not been prerolled, ignoring drive command.") 376 | motor_command = self.command_queue.get_nowait() 377 | return 378 | except: 379 | rospy.logerr("Fast timer exception on preroll." + traceback.format_exc()) 380 | self.fast_timer_comms_active = False 381 | try: 382 | motor_command = self.command_queue.get_nowait() 383 | except Queue.Empty: 384 | rospy.logerr("Queue was empty??" + traceback.format_exc()) 385 | return 386 | 387 | if motor_command[0] == 'drive': 388 | try: 389 | if self.publish_current and self.i2t_error_latch: 390 | # have exceeded i2t bounds 391 | return 392 | 393 | if not self.driver.engaged(): 394 | self.driver.engage() 395 | self.status = "engaged" 396 | 397 | left_linear_val, right_linear_val = motor_command[1] 398 | self.driver.drive(left_linear_val, right_linear_val) 399 | self.last_speed = max(abs(left_linear_val), abs(right_linear_val)) 400 | self.last_cmd_vel_time = time_now 401 | except (ChannelBrokenException, ChannelDamagedException): 402 | rospy.logerr("ODrive USB connection failure in drive_cmd." + traceback.format_exc(1)) 403 | self.fast_timer_comms_active = False 404 | self.driver = None 405 | except: 406 | rospy.logerr("motor drive unknown failure:" + traceback.format_exc()) 407 | self.fast_timer_comms_active = False 408 | 409 | elif motor_command[0] == 'release': 410 | pass 411 | # ? 412 | else: 413 | pass 414 | 415 | 416 | def terminate(self): 417 | self.fast_timer.shutdown() 418 | if self.driver: 419 | self.driver.release() 420 | 421 | # ROS services 422 | def connect_driver(self, request): 423 | if self.driver: 424 | return (False, "Already connected.") 425 | 426 | ODriveClass = ODriveInterfaceAPI if not self.sim_mode else ODriveInterfaceSimulator 427 | 428 | self.driver = ODriveInterfaceAPI(logger=ROSLogger()) 429 | rospy.loginfo("Connecting to ODrive...") 430 | if not self.driver.connect(right_axis=self.axis_for_right): 431 | self.driver = None 432 | #rospy.logerr("Failed to connect.") 433 | return (False, "Failed to connect.") 434 | 435 | #rospy.loginfo("ODrive connected.") 436 | 437 | # okay, connected, 438 | self.m_s_to_value = self.driver.encoder_cpr/self.tyre_circumference 439 | 440 | if self.publish_odom: 441 | self.old_pos_l = self.driver.left_axis.encoder.pos_cpr 442 | self.old_pos_r = self.driver.right_axis.encoder.pos_cpr 443 | 444 | self.fast_timer_comms_active = True 445 | 446 | self.status = "connected" 447 | self.status_pub.publish(self.status) 448 | return (True, "ODrive connected successfully") 449 | 450 | def disconnect_driver(self, request): 451 | if not self.driver: 452 | rospy.logerr("Not connected.") 453 | return (False, "Not connected.") 454 | try: 455 | if not self.driver.disconnect(): 456 | return (False, "Failed disconnection, but try reconnecting.") 457 | except: 458 | rospy.logerr('Error while disconnecting: {}'.format(traceback.format_exc())) 459 | finally: 460 | self.status = "disconnected" 461 | self.status_pub.publish(self.status_pub) 462 | self.driver = None 463 | return (True, "Disconnection success.") 464 | 465 | def calibrate_motor(self, request): 466 | if not self.driver: 467 | rospy.logerr("Not connected.") 468 | return (False, "Not connected.") 469 | 470 | if self.has_preroll: 471 | self.odometry_update_enabled = False # disable odometry updates while we preroll 472 | if not self.driver.preroll(wait=True): 473 | self.status = "preroll_fail" 474 | self.status_pub.publish(self.status) 475 | return (False, "Failed preroll.") 476 | 477 | self.status_pub.publish("ready") 478 | rospy.sleep(1) 479 | self.odometry_update_enabled = True 480 | else: 481 | if not self.driver.calibrate(): 482 | return (False, "Failed calibration.") 483 | 484 | return (True, "Calibration success.") 485 | 486 | def engage_motor(self, request): 487 | if not self.driver: 488 | rospy.logerr("Not connected.") 489 | return (False, "Not connected.") 490 | if not self.driver.has_prerolled(): 491 | return (False, "Not prerolled.") 492 | if not self.driver.engage(): 493 | return (False, "Failed to engage motor.") 494 | return (True, "Engage motor success.") 495 | 496 | def release_motor(self, request): 497 | if not self.driver: 498 | rospy.logerr("Not connected.") 499 | return (False, "Not connected.") 500 | if not self.driver.release(): 501 | return (False, "Failed to release motor.") 502 | return (True, "Release motor success.") 503 | 504 | def enable_odometry_update_service(self, request): 505 | enable = request.data 506 | 507 | if enable: 508 | self.odometry_update_enabled = True 509 | return(True, "Odometry enabled.") 510 | else: 511 | self.odometry_update_enabled = False 512 | return(True, "Odometry disabled.") 513 | 514 | def reset_odometry(self, request): 515 | self.x = 0.0 516 | self.y = 0.0 517 | self.theta = 0.0 518 | 519 | return(True, "Odometry reset.") 520 | 521 | # Helpers and callbacks 522 | 523 | def convert(self, forward, ccw): 524 | angular_to_linear = ccw * (self.wheel_track/2.0) 525 | left_linear_val = int((forward - angular_to_linear) * self.m_s_to_value) 526 | right_linear_val = int((forward + angular_to_linear) * self.m_s_to_value) 527 | 528 | return left_linear_val, right_linear_val 529 | 530 | def cmd_vel_callback(self, msg): 531 | #rospy.loginfo("Received a /cmd_vel message!") 532 | #rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z)) 533 | #rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z)) 534 | 535 | # rostopic pub -r 1 /commands/motor/current std_msgs/Float64 -- -1.0 536 | 537 | # Do velocity processing here: 538 | # Use the kinematics of your robot to map linear and angular velocities into motor commands 539 | 540 | # 3600 ERPM = 360 RPM ~= 6 km/hr 541 | 542 | #angular_to_linear = msg.angular.z * (wheel_track/2.0) 543 | #left_linear_rpm = (msg.linear.x - angular_to_linear) * m_s_to_erpm 544 | #right_linear_rpm = (msg.linear.x + angular_to_linear) * m_s_to_erpm 545 | left_linear_val, right_linear_val = self.convert(msg.linear.x, msg.angular.z) 546 | 547 | # if wheel speed = 0, stop publishing after sending 0 once. #TODO add error term, work out why VESC turns on for 0 rpm 548 | 549 | # Then set your wheel speeds (using wheel_left and wheel_right as examples) 550 | #self.left_motor_pub.publish(left_linear_rpm) 551 | #self.right_motor_pub.publish(right_linear_rpm) 552 | #wheel_left.set_speed(v_l) 553 | #wheel_right.set_speed(v_r) 554 | 555 | #rospy.logdebug("Driving left: %d, right: %d, from linear.x %.2f and angular.z %.2f" % (left_linear_val, right_linear_val, msg.linear.x, msg.angular.z)) 556 | try: 557 | drive_command = ('drive', (left_linear_val, right_linear_val)) 558 | self.command_queue.put_nowait(drive_command) 559 | except Queue.Full: 560 | pass 561 | 562 | self.last_cmd_vel_time = rospy.Time.now() 563 | 564 | def pub_diagnostics(self, stat): 565 | stat.add("Status", self.status) 566 | stat.add("Motor state L", self.motor_state_l) 567 | stat.add("Motor state R", self.motor_state_r) 568 | stat.add("FET temp L (C)", round(self.temp_v_l,1)) 569 | stat.add("FET temp R (C)", round(self.temp_v_r,1)) 570 | stat.add("Motor temp L (C)", "unimplemented") 571 | stat.add("Motor temp R (C)", "unimplemented") 572 | stat.add("Motor current L (A)", round(self.current_l,1)) 573 | stat.add("Motor current R (A)", round(self.current_r,1)) 574 | stat.add("Voltage (V)", round(self.bus_voltage,2)) 575 | stat.add("Motor i2t L", round(self.left_energy_acc,1)) 576 | stat.add("Motor i2t R", round(self.right_energy_acc,1)) 577 | 578 | # https://github.com/ros/common_msgs/blob/jade-devel/diagnostic_msgs/msg/DiagnosticStatus.msg 579 | if self.status == "disconnected": 580 | stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Not connected") 581 | else: 582 | if self.i2t_error_latch: 583 | stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "i2t overheated, drive ignored until cool") 584 | elif self.left_energy_acc > self.i2t_warning_threshold: 585 | stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Left motor over i2t warning threshold") 586 | elif self.left_energy_acc > self.i2t_error_threshold: 587 | stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Left motor over i2t error threshold") 588 | elif self.right_energy_acc > self.i2t_warning_threshold: 589 | stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Right motor over i2t warning threshold") 590 | elif self.right_energy_acc > self.i2t_error_threshold: 591 | stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Right motor over i2t error threshold") 592 | # Everything is okay: 593 | else: 594 | stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK, "Running") 595 | 596 | 597 | def pub_temperatures(self): 598 | # https://discourse.odriverobotics.com/t/odrive-mosfet-temperature-rise-measurements-using-the-onboard-thermistor/972 599 | # https://discourse.odriverobotics.com/t/thermistors-on-the-odrive/813/7 600 | # https://www.digikey.com/product-detail/en/murata-electronics-north-america/NCP15XH103F03RC/490-4801-1-ND/1644682 601 | #p3 = 363.0 602 | #p2 = -459.2 603 | #p1 = 308.3 604 | #p0 = -28.1 605 | # 606 | #vl = self.temp_v_l 607 | #vr = self.temp_v_r 608 | 609 | #temperature_l = p3*vl**3 + p2*vl**2 + p1*vl + p0 610 | #temperature_r = p3*vr**3 + p2*vr**2 + p1*vr + p0 611 | 612 | #print(temperature_l, temperature_r) 613 | 614 | self.temperature_publisher_left.publish(self.temp_v_l) 615 | self.temperature_publisher_right.publish(self.temp_v_r) 616 | 617 | # Current publishing and i2t calculation 618 | i2t_current_nominal = 2.0 619 | i2t_update_rate = 0.01 620 | 621 | def pub_current(self): 622 | self.current_publisher_left.publish(float(self.current_l)) 623 | self.current_publisher_right.publish(float(self.current_r)) 624 | 625 | now = time.time() 626 | 627 | if not hasattr(self, 'last_pub_current_time'): 628 | self.last_pub_current_time = now 629 | self.left_energy_acc = 0 630 | self.right_energy_acc = 0 631 | return 632 | 633 | # calculate and publish i2t 634 | dt = now - self.last_pub_current_time 635 | 636 | power = max(0, self.current_l**2 - self.i2t_current_nominal**2) 637 | energy = power * dt 638 | self.left_energy_acc *= 1 - self.i2t_update_rate * dt 639 | self.left_energy_acc += energy 640 | 641 | power = max(0, self.current_r**2 - self.i2t_current_nominal**2) 642 | energy = power * dt 643 | self.right_energy_acc *= 1 - self.i2t_update_rate * dt 644 | self.right_energy_acc += energy 645 | 646 | self.last_pub_current_time = now 647 | 648 | self.i2t_publisher_left.publish(float(self.left_energy_acc)) 649 | self.i2t_publisher_right.publish(float(self.right_energy_acc)) 650 | 651 | # stop odrive if overheated 652 | if self.left_energy_acc > self.i2t_error_threshold or self.right_energy_acc > self.i2t_error_threshold: 653 | if not self.i2t_error_latch: 654 | self.driver.release() 655 | self.status = "overheated" 656 | self.i2t_error_latch = True 657 | rospy.logerr("ODrive has exceeded i2t error threshold, ignoring drive commands. Waiting to cool down.") 658 | elif self.i2t_error_latch: 659 | if self.left_energy_acc < self.i2t_resume_threshold and self.right_energy_acc < self.i2t_resume_threshold: 660 | # have cooled enough now 661 | self.status = "ready" 662 | self.i2t_error_latch = False 663 | rospy.logerr("ODrive has cooled below i2t resume threshold, ignoring drive commands. Waiting to cool down.") 664 | 665 | 666 | # current_quantizer = 5 667 | # 668 | # self.left_current_accumulator += self.current_l 669 | # self.right_current_accumulator += self.current_r 670 | # 671 | # self.current_loop_count += 1 672 | # if self.current_loop_count >= current_quantizer: 673 | # self.current_publisher_left.publish(float(self.left_current_accumulator) / current_quantizer) 674 | # self.current_publisher_right.publish(float(self.right_current_accumulator) / current_quantizer) 675 | # 676 | # self.current_loop_count = 0 677 | # self.left_current_accumulator = 0.0 678 | # self.right_current_accumulator = 0.0 679 | 680 | def pub_odometry(self, time_now): 681 | now = time_now 682 | self.odom_msg.header.stamp = now 683 | self.tf_msg.header.stamp = now 684 | 685 | wheel_track = self.wheel_track # check these. Values in m 686 | tyre_circumference = self.tyre_circumference 687 | # self.m_s_to_value = encoder_cpr/tyre_circumference set earlier 688 | 689 | # if odometry updates disabled, just return the old position and zero twist. 690 | if not self.odometry_update_enabled: 691 | self.odom_msg.twist.twist.linear.x = 0. 692 | self.odom_msg.twist.twist.angular.z = 0. 693 | 694 | # but update the old encoder positions, so when we restart updates 695 | # it will start by giving zero change from the old position. 696 | self.old_pos_l = self.new_pos_l 697 | self.old_pos_r = self.new_pos_r 698 | 699 | self.odom_publisher.publish(self.odom_msg) 700 | if self.publish_tf: 701 | self.tf_publisher.sendTransform(self.tf_msg) 702 | 703 | return 704 | 705 | # Twist/velocity: calculated from motor values only 706 | s = tyre_circumference * (self.vel_l+self.vel_r) / (2.0*self.encoder_cpr) 707 | w = tyre_circumference * (self.vel_r-self.vel_l) / (wheel_track * self.encoder_cpr) # angle: vel_r*tyre_radius - vel_l*tyre_radius 708 | self.odom_msg.twist.twist.linear.x = s 709 | self.odom_msg.twist.twist.angular.z = w 710 | 711 | #rospy.loginfo("vel_l: % 2.2f vel_r: % 2.2f vel_l: % 2.2f vel_r: % 2.2f x: % 2.2f th: % 2.2f pos_l: % 5.1f pos_r: % 5.1f " % ( 712 | # vel_l, -vel_r, 713 | # vel_l/encoder_cpr, vel_r/encoder_cpr, self.odom_msg.twist.twist.linear.x, self.odom_msg.twist.twist.angular.z, 714 | # self.driver.left_axis.encoder.pos_cpr, self.driver.right_axis.encoder.pos_cpr)) 715 | 716 | # Position 717 | delta_pos_l = self.new_pos_l - self.old_pos_l 718 | delta_pos_r = self.new_pos_r - self.old_pos_r 719 | 720 | self.old_pos_l = self.new_pos_l 721 | self.old_pos_r = self.new_pos_r 722 | 723 | # Check for overflow. Assume we can't move more than half a circumference in a single timestep. 724 | half_cpr = self.encoder_cpr/2.0 725 | if delta_pos_l > half_cpr: delta_pos_l = delta_pos_l - self.encoder_cpr 726 | elif delta_pos_l < -half_cpr: delta_pos_l = delta_pos_l + self.encoder_cpr 727 | if delta_pos_r > half_cpr: delta_pos_r = delta_pos_r - self.encoder_cpr 728 | elif delta_pos_r < -half_cpr: delta_pos_r = delta_pos_r + self.encoder_cpr 729 | 730 | # counts to metres 731 | delta_pos_l_m = delta_pos_l / self.m_s_to_value 732 | delta_pos_r_m = delta_pos_r / self.m_s_to_value 733 | 734 | # Distance travelled 735 | d = (delta_pos_l_m+delta_pos_r_m)/2.0 # delta_ps 736 | th = (delta_pos_r_m-delta_pos_l_m)/wheel_track # works for small angles 737 | 738 | xd = math.cos(th)*d 739 | yd = -math.sin(th)*d 740 | 741 | # elapsed time = event.last_real, event.current_real 742 | #elapsed = (event.current_real-event.last_real).to_sec() 743 | # calc_vel: d/elapsed, th/elapsed 744 | 745 | # Pose: updated from previous pose + position delta 746 | self.x += math.cos(self.theta)*xd - math.sin(self.theta)*yd 747 | self.y += math.sin(self.theta)*xd + math.cos(self.theta)*yd 748 | self.theta = (self.theta + th) % (2*math.pi) 749 | 750 | #rospy.loginfo("dl_m: % 2.2f dr_m: % 2.2f d: % 2.2f th: % 2.2f xd: % 2.2f yd: % 2.2f x: % 5.1f y: % 5.1f th: % 5.1f" % ( 751 | # delta_pos_l_m, delta_pos_r_m, 752 | # d, th, xd, yd, 753 | # self.x, self.y, self.theta 754 | # )) 755 | 756 | # fill odom message and publish 757 | 758 | self.odom_msg.pose.pose.position.x = self.x 759 | self.odom_msg.pose.pose.position.y = self.y 760 | q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, self.theta) 761 | self.odom_msg.pose.pose.orientation.z = q[2] # math.sin(self.theta)/2 762 | self.odom_msg.pose.pose.orientation.w = q[3] # math.cos(self.theta)/2 763 | 764 | #rospy.loginfo("theta: % 2.2f z_m: % 2.2f w_m: % 2.2f q[2]: % 2.2f q[3]: % 2.2f (q[0]: %2.2f q[1]: %2.2f)" % ( 765 | # self.theta, 766 | # math.sin(self.theta)/2, math.cos(self.theta)/2, 767 | # q[2],q[3],q[0],q[1] 768 | # )) 769 | 770 | #self.odom_msg.pose.covariance 771 | # x y z 772 | # x y z 773 | 774 | self.tf_msg.transform.translation.x = self.x 775 | self.tf_msg.transform.translation.y = self.y 776 | #self.tf_msg.transform.rotation.x 777 | #self.tf_msg.transform.rotation.x 778 | self.tf_msg.transform.rotation.z = q[2] 779 | self.tf_msg.transform.rotation.w = q[3] 780 | 781 | if self.publish_raw_odom: 782 | self.raw_odom_publisher_encoder_left.publish(self.new_pos_l) 783 | self.raw_odom_publisher_encoder_right.publish(self.new_pos_r) 784 | self.raw_odom_publisher_vel_left.publish(self.vel_l) 785 | self.raw_odom_publisher_vel_right.publish(self.vel_r) 786 | 787 | # ... and publish! 788 | self.odom_publisher.publish(self.odom_msg) 789 | if self.publish_tf: 790 | self.tf_publisher.sendTransform(self.tf_msg) 791 | 792 | def pub_joint_angles(self, time_now): 793 | jsm = self.joint_state_msg 794 | jsm.header.stamp = time_now 795 | if self.driver: 796 | jsm.position[0] = 2*math.pi * self.new_pos_l / self.encoder_cpr 797 | jsm.position[1] = 2*math.pi * self.new_pos_r / self.encoder_cpr 798 | 799 | self.joint_state_publisher.publish(jsm) 800 | 801 | def start_odrive(): 802 | rospy.init_node('odrive') 803 | odrive_node = ODriveNode() 804 | odrive_node.main_loop() 805 | #rospy.spin() 806 | 807 | if __name__ == '__main__': 808 | try: 809 | start_odrive() 810 | except rospy.ROSInterruptException: 811 | pass 812 | 813 | --------------------------------------------------------------------------------