├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── README.md ├── arduino_files └── serial_odom │ ├── car.h │ ├── interrupt_stuff.h │ ├── serial_odom.ino │ ├── uart_odom_msg.h │ ├── vehicle_config.h │ ├── wheel.h │ ├── wheel_type_four.cpp │ ├── wheel_type_one.cpp │ └── wheel_type_two.cpp ├── package.xml └── script ├── ardu_odom.py ├── ardu_odom_comm.py ├── ardu_odom_node.py ├── omni_base ├── omni_base_driver.py └── omni_serial_com.py ├── tf_test.cpp └── v0.3 └── serial_odom.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "arduino_files/serial_odom/EnableInterrupt"] 2 | path = arduino_files/serial_odom/EnableInterrupt 3 | url = https://github.com/EwingKang/EnableInterrupt.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(serial_odom) 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 | geometry_msgs 12 | nav_msgs 13 | std_msgs 14 | roscpp 15 | rospy 16 | tf 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a run_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # geometry_msgs# nav_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES encoder_reader 111 | # CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy tf 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | # include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/encoder_reader.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | add_executable(tf_test_node script/tf_test.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | add_dependencies(tf_test_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | target_link_libraries(tf_test_node ${catkin_LIBRARIES}) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_encoder_reader.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Simple-ROS-Arduino-odometry 2 | I started this project because I'm done with rosserial: it is just too bulky for the arduino's 2KB flash, and almost impossible to debug. The only two things it does is refuse to connect or report trying to sync. Trust me, it's much quicker to just write your own serial communication. It's quicker even I've never written any Python and actually learning Python at the same time! 3 | For more information, please see [About the Code](#project-details) section below. 4 | 5 | **PLEASE CHECK [ENCODER](#encoder) SECTION BELOW BEFORE PROCEEDING** 6 | 7 | 8 | ## Getting Started 9 | ### Prerequisites 10 | Tested and built with ROS-kinetic, Python 2.7, with Ubuntu 16.04LTS, but should work with any version near enough. 11 | Please make sure you have the following items set-up or at least you know what you're doing: 12 | * [install ROS](http://wiki.ros.org/kinetic/Installation/Ubuntu) (tested in ROS-kinetic) 13 | * [setup ROS catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) (it is recommanded to source the setup in .bashrc) 14 | * [Install the Arduino IDE](https://www.arduino.cc/en/Guide/Linux) ( I recommand you to put the Arduino IDE under /opt/ before running install.sh ) 15 | * Add your user to dialout group as required by the Arduino IDE (Hint1:`sudo usermod -a -G dialout yourUserName`, Hint2: you have to login again, just reboot!) 16 | * (**For non-quadrature encoders aka tachometers**) We need to subscribe to topic `/cmd_vel`(_[geometry_msgs.msg::Twist](http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Twist.html)_) to determine the direction of the wheel rotation. 17 | 18 | ### Installation 19 | 20 | 1. Go to your catkin directory, clone the project into folder /serial_odom along with all the submodules(Arduino library). 21 | ``` 22 | cd ~/catkin_ws/src 23 | git clone --recurse-submodules -j8 https://github.com/EwingKang/Simple-ROS-Arduino-odometry serial_odom 24 | ``` 25 | 2. (**Optional**) Link(or move) the Ardunio program to workspace. It is possible to open the (project)/arduino_files/serial_odom/serial_odom.ino with Arduino IDE directly. But I find it faster to find if everything is under Arduino IDE directory. You'll see a link folder in the Arduino/ afterwards 26 | ``` 27 | ln -s ~/catkin_ws/src/serial_odom/arduino_files/serial_odom/ ~/Arduino/ 28 | ``` 29 | 3. "Compile" the file with catkin_make so ros recgonize the package 30 | ``` 31 | cd ~/catkin_ws/ 32 | catkin_make 33 | ``` 34 | 4. Upload the Arduino code with Arduino IDE 35 | Please [setup your vehicle parameter](#vehicle_settings) first! 36 | 37 | ### Vehicle settings 38 | To make sure you have the correct result, it is necessary to setup parameters matching your vehicle geometry. Since odometry is calculated "on-board" the Arduino, you should change the config header accordingly 39 | 40 | * Physical parameters in [vehicle_config.h](arduino_files/serial_odom/vehicle_config.h): 41 | ``` 42 | #define ENC_TYPE 4 43 | #define ENC_ENABLE_CUMU // enable cumulative counter 44 | 45 | //========== VEHICLE CONFIG ==========// 46 | #define WHEEL_RAD 0.033f // wheel radius in m 47 | #define B 0.1f // b = wheel separation distance in m 48 | #define ENC_REDUCTION 21 // encoder reduction ratio = (wheel rpm)/(encoder rpm) 49 | #define MAX_WHEEL_RPS ((float)201/(float)60) // maximum wheel revolution per second 50 | 51 | //========== ENCODER CONFIG ==========// 52 | #define ENC_TPR 334 // encoder tooth count 53 | #define WHEEL_PPR (ENC_TYPE*ENC_TPR*ENC_REDUCTION) // pulse count per wheel revolution 54 | ``` 55 | * Special (cheap/homemade) Encoders: 56 | Please check the [Tachometer](#tachometer) section below. 57 | 58 | ### Run the node 59 | Hook up all your cable, make sure you have the correct device (in this case /dev/ttyACM0), and simply rosrun: 60 | ``` 61 | rosrun serial_odom serial_odom.py _port:=/dev/ttyACM0 62 | ``` 63 | The code will reset the Arduino automatically, and you'll see: 64 | 1. Node starting 65 | 2. Independent communication thread starting 66 | 3. Rebooting Arduino 67 | 4. Arduino standby message 68 | 2. Signal to start Arduino 69 | 6. First packet received and `/odom` is now published 70 | 71 | ### Check the result 72 | In another terminal, you can check the result by: 73 | * rostopic 74 | You be able to see the `/odom` topic with `rostopic list`. Use `rostopic echo /odom` to see the raw data 75 | * rviz: 76 | Run `rviz` and the ROS visualization tool will begin. Set the frame to "_Odom_" and you'll see "_base_link_" moving around. 77 | 78 | ### ROS node parameters 79 | All the parameters are under namespace `/serial_odom` 80 | * **~odom_topic** (default: 'odom') 81 | * **~base_id** (default: 'base_link') # base frame id 82 | * **~odom_id** (default: 'odom') # odom frame id 83 | * **~enable_tf** (default: True) 84 | 85 | * **~port** (default: '/dev/ttyACM0') # port 86 | * **~baudrate** (default: 115200) ) # baudrate 87 | * **~serial_timeout** (default: 10) ) 88 | * **~odom_freq** (default: 100) ) # hz of communication* 89 | * **~tx_freq** (default: 5) ) # hz of communication 90 | 91 | * **~quadrature** (default: True) 92 | * **~cmd_vel** (default: 'cmd_vel') 93 | * **~cmd_vel_timeout** (default: 3) ) # 94 | 95 | 96 | ## Project details 97 | TODO 98 | ### Tachometer 99 | TODO 100 | ### Encoder 101 | 102 | I'm using a cheap ass 175NTD(~5$) chinese-made encoder like [this one](https://world.taobao.com/item/9610181079.htm?spm=a21wu.10013406-tw.0.0.3ca71191qiAk3A) and it is super lame. Despite the claimed AB phase output with a 100 grid encoder plate, it just simply connot hangle that. And I have to make my own 25 grid encoder plate out of catdboard. Anyway, you're welcome to take this code and modified it to adopt proper A/B phase encoder, or even better, you can donate me some proper encoders and I'll change the code for you. Currectly, the direction information comes from a ROS topic, which piggyback the motor controller. 103 | 104 | ### Python 105 | I'm using Python 2.7 that comes with Ubuntu-16.04 & ROS-kinetic, with following modules: 106 | * rospy and its peripherals 107 | * rostf 108 | * sys, time, math 109 | * serial ([pySerial](http://pyserial.readthedocs.io/en/latest/pyserial.html)) 110 | * threading 111 | 112 | pySerial is used to easily handle the serial port and capture the data. Serial communication is handled in a seperate thread wih Python module "threading", which make it very elastic, powerful, and stable. 113 | 114 | ### Arduino 115 | On the Arduino side, data is struted and can be easily modified. [0x00 0xFF] is used as the starting signal of each packet. 116 | 117 | ### Line-by-line explaination 118 | TODO: see wiki 119 | 120 | ## Other costumizable parameters 121 | * Arduino sceduling system 122 | ``` 123 | #define SERIAL_PERD 100000 // Serial tx/rx check period, 100 ms is 10 Hz 124 | #define CAR_PERD 20000 // Main calculation period, 20 ms is 50 Hz 125 | ``` 126 | 127 | * ROS node (Python script) 128 | make sure your 129 | ``` 130 | self.comm_freq = float( rospy.get_param('~odom_freq', '15') ) # hz of communication 131 | ``` 132 | is higher than the rate the Arduino sends the message. For example: 133 | ``` 134 | ***THIS IS GOOD*** 135 | #define SERIAL_PERD 100000 // Serial tx/rx check period, 100 ms is 10 Hz 136 | --------- 137 | self.comm_freq = float( rospy.get_param('~odom_freq', '15') ) # hz of communication 138 | ``` 139 | If you recieve less frequently than the transmit rate, packet will be lost and you'll potentially have a jumping odometry. Equal rate is not a good idea since simetimes there might be a slight difference between system clock, For example 140 | ``` 141 | ***THIS IS BAD*** 142 | #define SERIAL_PERD 100000 // Serial tx/rx check period, 100 ms is 10 Hz 143 | --------- 144 | self.comm_freq = float( rospy.get_param('~odom_freq', '10') ) # hz of communication 145 | ``` 146 | 147 | ## Version 148 | 1.0 149 | ## Author 150 | **Ewing Kang** - [EwingKang](https://github.com/EwingKang) 151 | ## License 152 | GPLv3 153 | ## TODOs 154 | [] github wiki 155 | -------------------------------------------------------------------------------- /arduino_files/serial_odom/car.h: -------------------------------------------------------------------------------- 1 | #ifndef Car_h_ 2 | #define Car_h_ 3 | 4 | #include 5 | #include "wheel.h" 6 | 7 | class Car { 8 | public: 9 | const float b; // B = wheel separation distance 10 | //float pos_cov[36], vel_cov[36]; 11 | Wheel wheel_l, wheel_r; 12 | 13 | /********************************************************** 14 | Constructor 15 | ***********************************************************/ 16 | #if ENC_TYPE == 1 || ENC_TYPE == 2 17 | Car(uint8_t pinL, uint8_t pinR, float wheel_rad, unsigned int wheel_tooth, float sep) 18 | : wheel_l(pinL, wheel_rad, wheel_tooth) 19 | , wheel_r(pinR, wheel_rad, wheel_tooth) 20 | , b(sep) 21 | { 22 | last_t = micros(); 23 | } 24 | #elif ENC_TYPE == 4 25 | Car(uint8_t pinLA, uint8_t pinLB, uint8_t pinRA, uint8_t pinRB, float wheel_rad, unsigned int wheel_tooth, float sep) 26 | : wheel_l(pinLA, pinLB, wheel_rad, wheel_tooth) 27 | , wheel_r(pinRA, pinRB, wheel_rad, wheel_tooth) 28 | , b(sep) 29 | { 30 | last_t = micros(); 31 | } 32 | #else 33 | #error error Encoder type incorrect or not set 34 | #endif 35 | 36 | /********************************************************** 37 | Vehicl state update 38 | Func: update and calculate vehicle state according to 39 | encoder counters. 40 | Input: arduino time in microseconds 41 | Output: true/success false/error 42 | ***********************************************************/ 43 | bool update(const unsigned long &time_now) { 44 | unsigned long dt; 45 | float ds_l = 0, ds_r = 0; 46 | float dS, dth, dx, dy; 47 | 48 | if(paused) return false; 49 | 50 | wheel_l.update(time_now); 51 | wheel_r.update(time_now); 52 | ds_l = wheel_l.get_dx(); 53 | ds_r = wheel_r.get_dx(); 54 | 55 | dS = (ds_l + ds_r) / 2; 56 | dth = (ds_r - ds_l) / b; 57 | dx = dS * cos(theta + dth/2); 58 | dy = dS * sin(theta + dth/2); 59 | 60 | x += dx; 61 | y += dy; 62 | theta += dth; 63 | 64 | dt = time_now - last_t; 65 | 66 | if (dt == 0){ 67 | v_x = 0; 68 | v_y = 0; 69 | omega = 0; 70 | }else{ 71 | v_x = dS / dt; 72 | v_y = 0; 73 | omega = dth / dt; 74 | } 75 | last_t = time_now; 76 | return true; 77 | } 78 | 79 | /********************************************************** 80 | Vehicl fast update 81 | Func: update value without expansive calculations 82 | Input: arduino time in microseconds 83 | Output: true/success false/error 84 | Note: *** THERE SHOULD BE ONLY ONE CALL TO EITHER *** 85 | *** update() OR fast_update() *** 86 | ***********************************************************/ 87 | void fast_update(short &enc_l, short &enc_r) { 88 | wheel_l.fast_update((int16_t &)enc_l); 89 | wheel_r.fast_update((int16_t &)enc_r); 90 | return; 91 | } 92 | 93 | /********************************************************** 94 | Set Vehicle direction 95 | Func: Set direction of the wheels according to 96 | directional information. Using WASD convention. 97 | Input: ascii char from serial 98 | Output: 99 | Note: ONLY USEFULL WHEN NOT USING QUADRATURE ENCODERS 100 | ***********************************************************/ 101 | #if ENC_TYPE == 1 || ENC_TYPE == 2 102 | void set_direction(const char &cmd){ 103 | if(cmd == 'W' || cmd == 'E' || cmd == 'D') wheel_l.set_dir(DIR_FORD); 104 | else if (cmd == 'S' || cmd == 'A' || cmd == 'Z') wheel_l.set_dir(DIR_BACK); 105 | else if (cmd == 'X' || cmd == 'Q' || cmd == 'C') wheel_l.set_dir(DIR_STOP); 106 | 107 | if(cmd == 'W' || cmd == 'Q' || cmd == 'A') wheel_r.set_dir(DIR_FORD); 108 | else if (cmd == 'S' || cmd == 'D' || cmd == 'C') wheel_r.set_dir(DIR_BACK); 109 | else if (cmd == 'X' || cmd == 'E' || cmd == 'Z') wheel_r.set_dir(DIR_STOP); 110 | 111 | return; 112 | } 113 | #endif 114 | 115 | /********************************************************** 116 | System pause 117 | Func: pause system TODO and all interrupts 118 | Input: true/pause false/unpause 119 | Output: 120 | ***********************************************************/ 121 | void pause(const bool is_paused){ 122 | this->paused = is_paused; 123 | #if ENC_TYPE == 1 || ENC_TYPE == 2 124 | if(is_paused) { 125 | wheel_l.set_dir(0); 126 | wheel_r.set_dir(0); 127 | } 128 | #endif 129 | 130 | } 131 | 132 | /********************************************************** 133 | System reset 134 | Func: reset all value 135 | Input: 136 | Output: 137 | ***********************************************************/ 138 | void reset(){ 139 | wheel_l.reset(); 140 | wheel_r.reset(); 141 | x = 0; 142 | y = 0; 143 | theta = 0; 144 | v_x = 0; 145 | v_y = 0; 146 | omega = 0; 147 | last_t = micros(); 148 | return; 149 | } 150 | 151 | /********************************************************** 152 | Get vehicle state 153 | Func: 154 | Input: reference to state 155 | Output: (by reference) x(m), y(m), theta(radians) 156 | ***********************************************************/ 157 | void GetPose(float &x, float &y, float &th) { 158 | x = this -> x; 159 | y = this -> y; 160 | th = this -> theta; 161 | return; 162 | } 163 | 164 | /********************************************************** 165 | Get vehicle state derivatives 166 | Func: 167 | Input: reference to state derivatives 168 | Output: (by reference) x(m/s), y(m/s), theta(radians) 169 | ***********************************************************/ 170 | void GetTwist(float &vx, float &vy, float &omega) { 171 | vx = this -> v_x; 172 | vy = this -> v_y; 173 | omega = this -> omega; 174 | return; 175 | } 176 | 177 | 178 | private: 179 | float x, y, theta; 180 | float v_x, v_y, omega; 181 | bool paused; 182 | unsigned long last_t; 183 | }; 184 | 185 | #endif 186 | -------------------------------------------------------------------------------- /arduino_files/serial_odom/interrupt_stuff.h: -------------------------------------------------------------------------------- 1 | extern Car duckiebot; 2 | /*************************************** 3 | prototypes 4 | ***************************************/ 5 | void risingLA(void); 6 | void fallingLA(void); 7 | void risingLB(void); 8 | void fallingLB(void); 9 | void risingRA(void); 10 | void fallingRA(void); 11 | void risingRB(void); 12 | void fallingRB(void); 13 | 14 | /*************************************** 15 | Interfaces 16 | ***************************************/ 17 | void enable_encoder_int_type1() { 18 | enableInterrupt(duckiebot.wheel_l.pinA, risingLA, RISING); 19 | enableInterrupt(duckiebot.wheel_r.pinA, risingRA, RISING); 20 | } 21 | void enable_encoder_int_type2() { 22 | enableInterrupt(duckiebot.wheel_l.pinA, risingLA, RISING); 23 | enableInterrupt(duckiebot.wheel_r.pinA, risingRA, RISING); 24 | } 25 | void enable_encoder_int_type4() { 26 | enableInterrupt(duckiebot.wheel_l.pinA, risingLA, RISING); 27 | enableInterrupt(duckiebot.wheel_r.pinA, risingRA, RISING); 28 | enableInterrupt(duckiebot.wheel_l.pinB, risingLB, RISING); 29 | enableInterrupt(duckiebot.wheel_r.pinB, risingRB, RISING); 30 | } 31 | 32 | void disable_encoder_int() { 33 | disableInterrupt(duckiebot.wheel_l.pinA); 34 | disableInterrupt(duckiebot.wheel_r.pinA); 35 | #if ENC_TYPE == 4 36 | disableInterrupt(duckiebot.wheel_l.pinB); 37 | disableInterrupt(duckiebot.wheel_l.pinB); 38 | #endif 39 | } 40 | 41 | //Note: interfacing functions are down below 42 | /******************************************** 43 | global callbacks for interrupt library 44 | This is super ugly, unfortunately, OO is not 45 | an option with PinChangeInt lib 46 | *********************************************/ 47 | void risingLA(void) { 48 | duckiebot.wheel_l.Cb_RiseA(); 49 | #if ENC_TYPE != 1 50 | enableInterrupt(duckiebot.wheel_l.pinA, fallingLA, FALLING); //attach the falling end 51 | #endif 52 | } 53 | void fallingLA(void) { 54 | duckiebot.wheel_l.Cb_FallA(); 55 | enableInterrupt(duckiebot.wheel_l.pinA, risingLA, RISING); 56 | } 57 | void risingLB(void) { 58 | duckiebot.wheel_l.Cb_RiseB(); 59 | enableInterrupt(duckiebot.wheel_l.pinB, fallingLB, FALLING); 60 | } 61 | void fallingLB(void) { 62 | duckiebot.wheel_l.Cb_FallB(); 63 | enableInterrupt(duckiebot.wheel_l.pinB, risingLB, RISING); 64 | } 65 | void risingRA(void) { 66 | duckiebot.wheel_r.Cb_RiseA(); 67 | #if ENC_TYPE != 1 68 | enableInterrupt(duckiebot.wheel_r.pinA, fallingRA, FALLING); 69 | #endif 70 | } 71 | void fallingRA(void) { 72 | duckiebot.wheel_r.Cb_FallA(); 73 | enableInterrupt(duckiebot.wheel_r.pinA, risingRA, RISING); 74 | } 75 | void risingRB(void) { 76 | duckiebot.wheel_r.Cb_RiseB(); 77 | enableInterrupt(duckiebot.wheel_r.pinB, fallingRB, FALLING); 78 | } 79 | void fallingRB(void) { 80 | duckiebot.wheel_r.Cb_FallB(); 81 | enableInterrupt(duckiebot.wheel_r.pinB, risingRB, RISING); 82 | } 83 | -------------------------------------------------------------------------------- /arduino_files/serial_odom/serial_odom.ino: -------------------------------------------------------------------------------- 1 | 2 | //========== LIBRARIES ==========// 3 | #include "EnableInterrupt/EnableInterrupt.h" //Software interrupt lib 4 | //https://github.com/GreyGnome/EnableInterrupt 5 | //PinChangeInt is deprecated 6 | 7 | #include "vehicle_config.h" // USER customize settings 8 | #include "car.h" // Car library 9 | #include "uart_odom_msg.h" // UART message 10 | 11 | //========== DEBUG ==========// 12 | //#define DEBUG_SER_MON // serial debug output 13 | const PROGMEM float expected_int_rate = WHEEL_PPR * MAX_WHEEL_RPS; 14 | 15 | //========== STATE/COMM CONFIG ==========// 16 | #define SERIAL_PERD 20000 // 20 ms is 50 hz 17 | #define CAR_PERD 10000 // 10 ms is 100 hz 18 | #define PAUSED_PERD 500000 // 500 ms is 2 hz 19 | 20 | //========== OBJECTS ==========// 21 | Car duckiebot(4, 5, 6, 7, WHEEL_RAD, WHEEL_PPR, B); // general quadrature encoder 22 | //Car duckiebot(4, 5, WHEEL_RAD, WHEEL_PPR, B); // only two pins if ENC_TYPE is 1 or 2 23 | Odom_Msg odom_msg; 24 | Odom_Msg_Bfr *odom_msg_ptr = (Odom_Msg_Bfr *)&odom_msg; //byte block of Odom_msg 25 | 26 | bool loop_on; 27 | uint8_t seq; 28 | 29 | // prototypes 30 | void decode_cmd(int &, bool &); 31 | void set_msg_header(Odom_Msg&); 32 | 33 | #include "interrupt_stuff.h" 34 | 35 | void setup() { 36 | // clear all 37 | memset(&odom_msg, 0, sizeof(odom_msg)); 38 | loop_on = false; 39 | seq = 0; 40 | Serial.begin(115200); 41 | 42 | // hold here until starting signal 43 | while(Serial.read() != 'B') { 44 | delay(500); 45 | #ifdef DEBUG_SER_MON 46 | Serial.println("***Debug Mode, full text output***"); 47 | #endif 48 | Serial.print("Waiting: "); 49 | Serial.print(seq); 50 | Serial.println(", \"B\" to start"); 51 | seq++; 52 | } 53 | 54 | // initialize object 55 | loop_on = true; 56 | seq = 0; 57 | set_msg_header(odom_msg); 58 | duckiebot.reset(); 59 | while(Serial.available()) { Serial.read(); } // flush input buffer 60 | 61 | #if ENC_TYPE == 1 62 | enable_encoder_int_type1(); 63 | #elif ENC_TYPE == 2 64 | enable_encoder_int_type2(); 65 | #elif ENC_TYPE == 4 66 | enable_encoder_int_type4(); 67 | #else 68 | #error incorrect encoder settings 69 | #endif 70 | } 71 | 72 | void loop() { 73 | static unsigned long last_pub, last_update; // time keeper 74 | unsigned long time_now = micros(); 75 | 76 | if(loop_on) { 77 | if ( (time_now - last_update) >= CAR_PERD) { 78 | if( !duckiebot.update(time_now) ){ // update car 79 | Serial.println("update err"); 80 | } 81 | last_update = time_now; 82 | } 83 | 84 | if( (time_now - last_pub) >= SERIAL_PERD ) { 85 | // odometry message update 86 | odom_msg.t_micro = time_now; //micro seconds 87 | odom_msg.seq++; 88 | duckiebot.GetPose(odom_msg.x, odom_msg.y, odom_msg.th); //pose: coordinate 89 | duckiebot.GetTwist(odom_msg.v_x, odom_msg.v_y, odom_msg.omega); //velocity in body frame 90 | 91 | //sent data 92 | #ifdef DEBUG_SER_MON 93 | //Serial.print("x:"); 94 | //Serial.print(odom_msg.x); 95 | //Serial.print("y:"); 96 | //Serial.print(odom_msg.y); 97 | //Serial.print("th:"); 98 | //Serial.println(odom_msg.omega); 99 | Serial.print("A dt:"); 100 | Serial.print(duckiebot.wheel_l.get_count()); 101 | Serial.print("c:"); 102 | Serial.print(duckiebot.wheel_l.cumulative_cntr()); 103 | Serial.print("R:"); 104 | Serial.println(duckiebot.wheel_l.cumulative_revo()); 105 | #else 106 | Serial.write((uint8_t*)odom_msg_ptr, sizeof(*odom_msg_ptr)); 107 | #endif 108 | 109 | //input parsing 110 | decode_cmd(Serial.read(), loop_on); 111 | last_pub = time_now; 112 | } 113 | }else { //if (loop_on) 114 | if( (time_now - last_pub) >= PAUSED_PERD ) { 115 | Serial.print("Paused:"); 116 | Serial.println(seq); 117 | seq++; 118 | last_pub = time_now; 119 | } 120 | decode_cmd(Serial.read(), loop_on); // continuous cmd parsing if not looping 121 | } 122 | 123 | } 124 | 125 | /******************************************************************* 126 | decode command 127 | Func: 128 | Input: 129 | Output: 130 | Note: 131 | *******************************************************************/ 132 | void decode_cmd(int cmd, bool &loop_on) { 133 | if(cmd == -1) return; ///no input, do nothing 134 | 135 | if(cmd == 'P') { /// system toggle pause 136 | if(loop_on) { // pause 137 | loop_on = false; 138 | duckiebot.pause(true); 139 | }else { // start 140 | loop_on = true; 141 | set_msg_header(odom_msg); 142 | duckiebot.pause(false); 143 | } 144 | }else if(cmd == 'R') { /// system reset 145 | loop_on = false; 146 | duckiebot.reset(); 147 | memset(&odom_msg, 0, sizeof(odom_msg)); 148 | seq = 0; 149 | set_msg_header(odom_msg); 150 | }else if(cmd == 'B') { /// system start 151 | loop_on = true; 152 | set_msg_header(odom_msg); 153 | duckiebot.pause(false); 154 | }else{ /// decode direction command 155 | #if ENC_TYPE == 1 || ENC_TYPE == 2 156 | duckiebot.set_direction(cmd); 157 | #endif 158 | } 159 | return; 160 | } 161 | 162 | /******************************************************************* 163 | set message header 164 | Func: 165 | Input: 166 | Output: 167 | Note: 168 | *******************************************************************/ 169 | void set_msg_header(Odom_Msg& odom_msg) { 170 | memset(&(odom_msg.headerA), 0x00, sizeof(odom_msg.headerA)); 171 | memset(&(odom_msg.headerB), 0xFF, sizeof(odom_msg.headerB)); 172 | return; 173 | } -------------------------------------------------------------------------------- /arduino_files/serial_odom/uart_odom_msg.h: -------------------------------------------------------------------------------- 1 | #ifndef OdomMsg_h_ 2 | #define OdomMsg_h_ 3 | //Arduino Uno has a buffer size of 64-1 Bytes 4 | 5 | typedef struct Odom_Msg{ 6 | byte headerA, headerB; //2B should be set to 0x00 0xFF 00000000 11111111 7 | unsigned int seq; //2B, for sequencing 8 | unsigned long t_micro; //4B, in arduino Micros() format 9 | float x, y, th; //4B*3, state of the odom 10 | float v_x, v_y, omega; //4B*3, state of the odom 11 | }; 12 | 13 | typedef struct Enc_msg{ 14 | byte headerA, headerB; //2B should be set to 0x00 0xFE 00000000 11111110 15 | unsigned char seq; //1B sequencing 16 | short dx, dy, dth; //2B*3, in 0.1mm, maxed out at [+-3.2767] meters 17 | }; 18 | 19 | typedef union Odom_Msg_Bfr { 20 | Odom_Msg odom_msg; 21 | byte msg[32]; 22 | }; 23 | 24 | typedef union Enc_Msg_Bfr { 25 | Enc_msg enc_msg; 26 | byte msg[9]; 27 | }; 28 | #endif -------------------------------------------------------------------------------- /arduino_files/serial_odom/vehicle_config.h: -------------------------------------------------------------------------------- 1 | /**************************************** 2 | * ENC_TYPE options: 3 | * 1 rising edge counter, need direction input 4 | * 2 tachometer with rising and falling edge, need direction input 5 | * 4 quadrature encder, auto direction 6 | ****************************************/ 7 | #define ENC_TYPE 4 8 | #define ENC_ENABLE_CUMU // enable cumulative counter 9 | 10 | //========== VEHICLE CONFIG ==========// 11 | #define WHEEL_RAD 0.033f // wheel radius in m 12 | #define B 0.1f // b = wheel separation distance 13 | #define ENC_REDUCTION 21 // encoder reduction ratio = (wheel rpm)/(encoder rpm) 14 | #define MAX_WHEEL_RPS ((float)201/(float)60) // maximum wheel revolution per second 15 | 16 | //========== ENCODER CONFIG ==========// 17 | #define ENC_TPR 334 // encoder tooth count 18 | #define WHEEL_PPR (ENC_TYPE*ENC_TPR*ENC_REDUCTION) // pulse count per wheel revolution 19 | -------------------------------------------------------------------------------- /arduino_files/serial_odom/wheel.h: -------------------------------------------------------------------------------- 1 | #ifndef Wheel_h_ 2 | #define Wheel_h_ 3 | 4 | #import // because of order the compiler auto builds 5 | 6 | #include 7 | #include // because of order the compiler auto builds 8 | #include 9 | 10 | #define DIR_FORD 1 11 | #define DIR_BACK -1 12 | #define DIR_STOP 0 13 | 14 | class Wheel 15 | { 16 | public: 17 | //=======================================================// 18 | const uint8_t pinA, pinB; 19 | 20 | /////// Constructor /////// 21 | #if ENC_TYPE == 1 || ENC_TYPE == 2 22 | Wheel(uint8_t pin, float rad, unsigned int too); 23 | #elif ENC_TYPE == 4 24 | Wheel(uint8_t pin1, uint8_t pin2, float rad, unsigned int too); 25 | #else 26 | #error error Encoder type incorrect or not set 27 | #endif 28 | 29 | /////// callback group /////// 30 | void Cb_RiseA(void); 31 | void Cb_RiseB(void); 32 | void Cb_FallA(void); 33 | void Cb_FallB(void); 34 | 35 | /////// state machine group /////// 36 | void update(const unsigned long &t_now); // update calculation 37 | void fast_update(int16_t &enc); // fast update. should not be used with update() simultaneously 38 | void reset(); // reset counter 39 | void set_dir(int8_t dir) {direction = dir;} 40 | 41 | /////// calculation group /////// 42 | //return the value calculated at last update 43 | int get_count() { return cntr; } 44 | float get_rev() { return (float)cntr / ppr; } 45 | float get_dth() { return 2*M_PI * get_rev(); } // return the forward distance at last update 46 | float get_dx() { return get_dth()*radius; } // return the forward distance at last update 47 | float get_angular_vel() {return get_dth() * 1e6 /duration_us; } 48 | float get_linear_vel() {return get_angular_vel() * radius; } 49 | 50 | /////// statictics group /////// 51 | #ifdef ENC_ENABLE_CUMU 52 | long cumulative_cntr() {return cntr_cumu; } 53 | float cumulative_revo() {return (float)cntr_cumu / ppr; } // revolution count 54 | float cumulative_angle() {return 2*M_PI * cumulative_revo(); } // in radians 55 | float cumulative_dist() {return cumulative_angle() * radius; } 56 | 57 | unsigned long total_cntr() {return cntr_total;} 58 | float total_revo() {return (float)cntr_total/ppr;} 59 | float total_angle() {return 2*M_PI * total_revo(); }// abs cumulative value 60 | float total_dist() {return total_angle() * radius;}// absolute cumulative value 61 | #endif 62 | 63 | //=======================================================// 64 | private: 65 | const float radius; 66 | const uint16_t ppr; // pulse per wheel revolution for one phase 67 | 68 | int8_t pin_state; 69 | int8_t direction; 70 | int16_t cntr, int_cntr; // output counter and interrupt counter 71 | #ifdef ENC_ENABLE_CUMU 72 | int32_t cntr_cumu; // cumulative counter, updated with update() call 73 | uint32_t cntr_total; // abs counter 74 | #endif 75 | 76 | unsigned long last_call_t_us; // last update time 77 | unsigned long duration_us; // duration from last to 78 | }; 79 | 80 | #endif -------------------------------------------------------------------------------- /arduino_files/serial_odom/wheel_type_four.cpp: -------------------------------------------------------------------------------- 1 | #include "vehicle_config.h" 2 | #if ENC_TYPE == 4 3 | #include "wheel.h" 4 | 5 | #define ENC_ST_H 0xFF //11111111 6 | #define ENC_ST_L 0x00 //00000000 7 | 8 | #define ENC_A 0x01 //00000001 9 | #define ENC_B 0x02 //00000010 10 | 11 | /*************************************** 12 | constructor 13 | ****************************************/ 14 | Wheel::Wheel(uint8_t pin1, uint8_t pin2, float rad, unsigned int too): 15 | pinA(pin1), pinB(pin2), radius(rad), ppr(too) 16 | { 17 | pinMode(pinA, INPUT); //set the pin_rc to input 18 | pinMode(pinB, INPUT); //set the pin_rc to input 19 | reset(); 20 | } 21 | 22 | /*************************************** 23 | pin interrupt callbacks 24 | ****************************************/ 25 | void Wheel::Cb_RiseA(void) { 26 | if( pin_state&ENC_A ) return; // immediately return if A is already high 27 | if( pin_state&ENC_B ) int_cntr--; // if pinB is HIGH 28 | else int_cntr++; 29 | pin_state |= ENC_A; // set pinA High 30 | #ifdef ENC_ENABLE_CUMU 31 | cntr_total++; 32 | #endif 33 | return; 34 | } 35 | void Wheel::Cb_FallA(void) { 36 | if( !(pin_state&ENC_A) ) return; // immediately return if A is already low 37 | if( pin_state&ENC_B ) int_cntr++; // if pinB is HIGH 38 | else int_cntr--; 39 | pin_state &= (~ENC_A); // set pinA low 40 | #ifdef ENC_ENABLE_CUMU 41 | cntr_total++; 42 | #endif 43 | return; 44 | } 45 | void Wheel::Cb_RiseB(void) { 46 | if( pin_state&ENC_B ) return; // immediately return if B is already high 47 | if( pin_state&ENC_A ) int_cntr++; // if pinA is HIGH 48 | else int_cntr--; 49 | pin_state |= ENC_B; // set pinB High 50 | #ifdef ENC_ENABLE_CUMU 51 | cntr_total++; 52 | #endif 53 | return; 54 | } 55 | void Wheel::Cb_FallB(void) { 56 | if( ! (pin_state&ENC_B) ) return; // immediately return if B is already low 57 | if( pin_state&ENC_A ) int_cntr--; // if pinA is HIGH 58 | else int_cntr++; 59 | pin_state &= (~ENC_B); // set pinA low 60 | #ifdef ENC_ENABLE_CUMU 61 | cntr_total++; 62 | #endif 63 | return; 64 | } 65 | 66 | /*************************************** 67 | update calculation 68 | ****************************************/ 69 | void Wheel::update(const unsigned long &t_now){ 70 | cntr = int_cntr; // copy interrupt counts 71 | #ifdef ENC_ENABLE_CUMU 72 | cntr_cumu += int_cntr; 73 | #endif 74 | int_cntr = 0; // reset interrupt counts 75 | duration_us = t_now - last_call_t_us; 76 | last_call_t_us = t_now; 77 | } 78 | /******************************************************************* 79 | fast update 80 | Func: No duration or velocity calculations, useful for super 81 | fast encoders. 82 | Input: Reference to encoder counter 83 | Output: interrupt counts 84 | Note: *** THERE SHOULD BE ONLY ONE CALL TO EITHER update() OR 85 | fast_update() *** 86 | *******************************************************************/ 87 | void Wheel::fast_update(int16_t &enc){ 88 | enc = int_cntr; // copy interrupt counts 89 | int_cntr = 0; // reset interrupt counts 90 | return; 91 | } 92 | 93 | /*************************************** 94 | reset counter 95 | ****************************************/ 96 | void Wheel::reset(){ 97 | cntr = 0; 98 | int_cntr = 0; // output counter and interrupt counter 99 | direction = 0; 100 | pin_state = 0; 101 | #ifdef ENC_ENABLE_CUMU 102 | cntr_cumu = 0; // cumulative counter, updated with update() call 103 | cntr_total = 0; // abs counter 104 | #endif 105 | } 106 | 107 | #endif -------------------------------------------------------------------------------- /arduino_files/serial_odom/wheel_type_one.cpp: -------------------------------------------------------------------------------- 1 | #include "vehicle_config.h" 2 | #if ENC_TYPE == 1 3 | #include "wheel.h" 4 | 5 | /*************************************** 6 | constructor 7 | ****************************************/ 8 | Wheel::Wheel(uint8_t pin, float rad, unsigned int too): 9 | pinA(pin), pinB(0), radius(rad), ppr(too) 10 | { 11 | pinMode(pinA, INPUT); //set the pin_rc to input 12 | reset(); 13 | } 14 | 15 | /*************************************** 16 | pin interrupt callbacks 17 | ****************************************/ 18 | void Wheel::Cb_RiseA(void) { 19 | if (direction == DIR_FORD) int_cntr++; 20 | else if (direction == DIR_BACK) int_cntr--; 21 | #ifdef ENC_ENABLE_CUMU 22 | cntr_total++; 23 | #endif 24 | return; 25 | } 26 | //void Wheel::Cb_FallA(const unsigned long &time) {return;} 27 | //void Wheel::Cb_RiseB(const unsigned long &time) {return;} 28 | //void Wheel::Cb_FallB(const unsigned long &time) {return;} 29 | 30 | /*************************************** 31 | update calculation 32 | ****************************************/ 33 | void Wheel::update(const unsigned long &t_now){ 34 | cntr = int_cntr; // copy interrupt counts 35 | #ifdef ENC_ENABLE_CUMU 36 | cntr_cumu += int_cntr; 37 | #endif 38 | int_cntr = 0; // reset interrupt counts 39 | duration_us = t_now - last_call_t_us; 40 | last_call_t_us = t_now; 41 | } 42 | /******************************************************************* 43 | fast update 44 | Func: No duration or velocity calculations, useful for super 45 | fast encoders. 46 | Input: Reference to encoder counter 47 | Output: interrupt counts 48 | Note: *** THERE SHOULD BE ONLY ONE CALL TO EITHER update() OR 49 | fast_update() *** 50 | *******************************************************************/ 51 | void Wheel::fast_update(int16_t &enc){ 52 | enc = int_cntr; // copy interrupt counts 53 | int_cntr = 0; // reset interrupt counts 54 | return; 55 | } 56 | 57 | /*************************************** 58 | reset counter 59 | ****************************************/ 60 | void Wheel::reset(){ 61 | cntr = 0; 62 | int_cntr = 0; // output counter and interrupt counter 63 | direction = 0; 64 | pin_state = 0; 65 | #ifdef ENC_ENABLE_CUMU 66 | cntr_cumu = 0; // cumulative counter, updated with update() call 67 | cntr_total = 0; // abs counter 68 | #endif 69 | } 70 | 71 | #endif -------------------------------------------------------------------------------- /arduino_files/serial_odom/wheel_type_two.cpp: -------------------------------------------------------------------------------- 1 | #include "vehicle_config.h" 2 | #if ENC_TYPE == 2 3 | #include "wheel.h" 4 | 5 | #define ENC_ST_H 1 6 | #define ENC_ST_L 0 7 | 8 | /*************************************** 9 | constructor 10 | ****************************************/ 11 | Wheel::Wheel(uint8_t pin, float rad, unsigned int too): 12 | pinA(pin), pinB(0), radius(rad), ppr(too) 13 | { 14 | pinMode(pinA, INPUT); //set the pin_rc to input 15 | reset(); 16 | } 17 | 18 | /*************************************** 19 | pin interrupt callbacks 20 | ****************************************/ 21 | void Wheel::Cb_RiseA(void) { 22 | if( pin_state == ENC_ST_H) return; // immediately return if overlaping state 23 | if (direction == DIR_FORD) int_cntr++; 24 | else if (direction == DIR_BACK) int_cntr--; 25 | pin_state = ENC_ST_H; 26 | #ifdef ENC_ENABLE_CUMU 27 | cntr_total++; 28 | #endif 29 | return; 30 | } 31 | void Wheel::Cb_FallA(void) { 32 | if( pin_state == ENC_ST_L) return; // immediately return if overlaping state 33 | if (direction == DIR_FORD) int_cntr++; 34 | else if (direction == DIR_BACK) int_cntr--; 35 | pin_state = ENC_ST_L; 36 | #ifdef ENC_ENABLE_CUMU 37 | cntr_total++; 38 | #endif 39 | return; 40 | } 41 | //void Wheel::Cb_RiseB(const unsigned long &time) {return;} 42 | //void Wheel::Cb_FallB(const unsigned long &time) {return;} 43 | 44 | 45 | /*************************************** 46 | update calculation 47 | ****************************************/ 48 | void Wheel::update(const unsigned long &t_now){ 49 | cntr = int_cntr; // copy interrupt counts 50 | #ifdef ENC_ENABLE_CUMU 51 | cntr_cumu += int_cntr; 52 | #endif 53 | int_cntr = 0; // reset interrupt counts 54 | duration_us = t_now - last_call_t_us; 55 | last_call_t_us = t_now; 56 | } 57 | /******************************************************************* 58 | fast update 59 | Func: No duration or velocity calculations, useful for super 60 | fast encoders. 61 | Input: Reference to encoder counter 62 | Output: interrupt counts 63 | Note: *** THERE SHOULD BE ONLY ONE CALL TO EITHER update() OR 64 | fast_update() *** 65 | *******************************************************************/ 66 | void Wheel::fast_update(int16_t &enc){ 67 | enc = int_cntr; // copy interrupt counts 68 | int_cntr = 0; // reset interrupt counts 69 | return; 70 | } 71 | 72 | /*************************************** 73 | reset counter 74 | ****************************************/ 75 | void Wheel::reset(){ 76 | cntr = 0; 77 | int_cntr = 0; // output counter and interrupt counter 78 | direction = 0; 79 | pin_state = 3; 80 | #ifdef ENC_ENABLE_CUMU 81 | cntr_cumu = 0; // cumulative counter, updated with update() call 82 | cntr_total = 0; // abs counter 83 | #endif 84 | } 85 | 86 | #endif -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | serial_odom 4 | 0.5.0 5 | Decoder for my Arduino Uno encodeer 6 | 7 | 8 | 9 | 10 | Ewing Kang 11 | 12 | 13 | 14 | 15 | 16 | MIT 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 | catkin 52 | geometry_msgs 53 | nav_msgs 54 | roscpp 55 | rospy 56 | tf 57 | geometry_msgs 58 | nav_msgs 59 | roscpp 60 | rospy 61 | tf 62 | geometry_msgs 63 | nav_msgs 64 | roscpp 65 | rospy 66 | tf 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /script/ardu_odom.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import tf 3 | import time 4 | import sys 5 | import math 6 | from operator import add 7 | import string 8 | import threading 9 | import geometry_msgs.msg 10 | from geometry_msgs.msg import Twist 11 | from geometry_msgs.msg import Vector3 12 | from nav_msgs.msg import Odometry 13 | 14 | class ArduOdom: 15 | '''******************************************************************* 16 | Constructor 17 | *******************************************************************''' 18 | def __init__(self): 19 | #======== Get params =========# 20 | self.param = {} # create a dictionary 21 | #self.debug_mode = bool(rospy.get_param('~debug_mode', False)) # true for detail info 22 | 23 | self.param["odom_topic"] = rospy.get_param("~odom_topic", 'odom') 24 | self.param["baseId"] = rospy.get_param('~base_id', 'base_link') # base frame id 25 | self.param["odomId"] = rospy.get_param('~odom_id', 'odom') # odom frame id 26 | self.param["enable_tf"] = rospy.get_param("~enable_tf", True) 27 | 28 | self.param["device_port"] = rospy.get_param('~port', '/dev/ttyACM0') # port 29 | self.param["baudrate"] = int( rospy.get_param('~baudrate', '115200') ) # baudrate 30 | self.param["timeout"] = float( rospy.get_param('~serial_timeout', '10') ) # 31 | self.param["odom_freq"] = float( rospy.get_param('~odom_freq', '100') ) # hz of communication 32 | self.param["tx_freq"] = float( rospy.get_param('~tx_freq', '5') ) # hz of communication 33 | 34 | self.param["quadrature"] = rospy.get_param("~quadrature", True) 35 | self.param["vel_cmd"] = rospy.get_param("~cmd_vel", 'cmd_vel') # sub topic, only effective if not quadrature type 36 | self.param["cmd_timeout"] = float( rospy.get_param('~cmd_vel_timeout', '3') ) # set speed 0 after this seconds, only effective if not quadrature type 37 | 38 | 39 | rospy.set_param("serial_odom", self.param) # set ros param 40 | 41 | #========== ROS message ==========# 42 | rospy.loginfo("Initiating Node") 43 | rospy.loginfo("Publishing odometry at: \"" + self.param["odom_topic"] + "\"") 44 | 45 | self.comm_handle_ok = False # set before timer to prevent stuff from starting prematurely 46 | 47 | #========== ROS handler ==========# 48 | if not self.param["quadrature"]: 49 | self.enc_sub = rospy.Subscriber(self.param["vel_cmd"], Twist, self.cmdvel_cb, queue_size=10) 50 | rospy.loginfo("Non-quadrature type encoder,subscribing to \"" + self.param["vel_cmd"] + "\" for wheel direction determination") 51 | self.odom_pub = rospy.Publisher(self.param["odom_topic"], Odometry, queue_size=10) # pubisher 52 | 53 | self.timer_odom = rospy.Timer(rospy.Duration(1.0/self.param["odom_freq"]), self.odomPub) # timer 54 | self.timer_txcmd = rospy.Timer( rospy.Duration(1.0/self.param["tx_freq"]), 55 | self.send_vel_cmd) # transmit command 56 | 57 | if self.param["enable_tf"]: 58 | rospy.loginfo("Publishing transform from %s to %s", self.param["odomId"], self.param["baseId"]) 59 | self.tf_br = tf.TransformBroadcaster() 60 | 61 | #========== variables for ROS topic ==========# 62 | self.odom = Odometry() 63 | self.odom.header.seq = 0 64 | self.odom.header.frame_id = self.param["odomId"] 65 | self.odom.child_frame_id = self.param["baseId"] 66 | for i in range(36): 67 | self.odom.pose.covariance[i] = 0 68 | self.odom.twist.covariance[i] = 0 69 | self.odom.twist.covariance[0] = 1.0 70 | self.odom.twist.covariance[35] = 1.0 71 | 72 | self.x_e = float(0) 73 | self.y_e = float(0) 74 | self.th = float(0) 75 | self.odom_last_seq = 0; 76 | self.enc_count_per_revo = (390*4) 77 | self.wheel_radius = 0.029 78 | self.b = 0.1 # wheel separation 79 | self.last_odom_time = time.time() 80 | self.last_cmd_vel_time = self.last_odom_time 81 | self.no_cmd_received = True 82 | self.first_odom = True 83 | 84 | self.veh_cmd = {"Vx":0, "Vy":0, "Omega":0} 85 | 86 | '''******************************************************************* 87 | Get serial handle object, must be called before every functions 88 | except constructor 89 | *******************************************************************''' 90 | def set_data_port(self, _serial_handle): 91 | self.comm_handle = _serial_handle 92 | self.comm_handle_ok = True 93 | 94 | '''******************************************************************* 95 | ROS cmd_vel Subscriber callback 96 | *******************************************************************''' 97 | def cmdvel_cb(self, vel_cmd): 98 | self.veh_cmd["Vx"] = vel_cmd.linear.x 99 | self.veh_cmd["Vy"] = vel_cmd.linear.y 100 | self.veh_cmd["Omega"] = vel_cmd.angular.z 101 | self.no_cmd_received = False 102 | self.last_cmd_vel_time = time.time() 103 | 104 | '''******************************************************************* 105 | send velocity command to serial handler 106 | *******************************************************************''' 107 | def send_vel_cmd(self, event): 108 | ### if haven't seen vel_cmd for a long time 109 | if( (time.time()-self.last_cmd_vel_time) > self.param["cmd_timeout"] and 110 | not self.no_cmd_received ): 111 | self.veh_cmd["Vx"] = 0 112 | self.veh_cmd["Vy"] = 0 113 | self.veh_cmd["Omega"] = 0 114 | self.no_cmd_received = True 115 | rospy.logwarn("no topic \"%s\" received, base stop.", self.param["vel_cmd"]) 116 | ### decode cmd_vel 117 | wheel = {"l":0, "r":0} 118 | wheel["l"] = self.veh_cmd["Vx"] - self.veh_cmd["Omega"] * self.b/2 # vL = vx - omega*(b/2) 119 | wheel["r"] = self.veh_cmd["Vx"] + self.veh_cmd["Omega"] * self.b/2 # vR = vx + omega*(b/2) 120 | self.comm_handle.send_dir_cmd(wheel) 121 | 122 | '''******************************************************************* 123 | ROS Odometry topic publisher, called by timer 124 | *******************************************************************''' 125 | def odomPub(self, event): 126 | if self.comm_handle_ok and self.comm_handle.serialOK(): 127 | if self.comm_handle.odom_new_data(): 128 | #===== get new data from comm handle =====# 129 | xyt_rtn = self.comm_handle.get_odom_data() 130 | if xyt_rtn is None: 131 | return 132 | 133 | #===== continuity check =====# 134 | if (xyt_rtn["seq"] != ((self.odom_last_seq + 1)%65536) ): 135 | if (not self.first_odom): 136 | rospy.logwarn("callback sequence mismatch, prev: %d, now:%d", 137 | self.odom_last_seq, xyt_rtn["seq"] ) 138 | self.odom_last_seq = xyt_rtn["seq"] 139 | return # disregard first data 140 | else: 141 | self.first_odom = False 142 | self.odom_last_seq = xyt_rtn["seq"] 143 | return # disregard first data 144 | self.odom_last_seq = xyt_rtn["seq"] 145 | 146 | #===== data assign =====# 147 | ### DEBUG 148 | #self.temp += xyt_rtn["enc_dt"][1] 149 | #print(self.temp) 150 | #print(" A: {} B: {} C: {}".format( xyt_rtn["enc_dt"][0], 151 | # xyt_rtn["enc_dt"][1], 152 | # xyt_rtn["enc_dt"][2] ) ) 153 | self.x_e = xyt_rtn["pos"][0] 154 | self.y_e = xyt_rtn["pos"][1] 155 | self.th = xyt_rtn["pos"][2] 156 | vx = xyt_rtn["vel"][0] 157 | vy = xyt_rtn["vel"][1] 158 | omega = xyt_rtn["vel"][2] 159 | 160 | time_now = time.time() 161 | dt = time_now - self.last_odom_time 162 | 163 | #======= ROS topic publish =======# 164 | self.odom.header.seq += 1 165 | self.odom.header.stamp = rospy.Time.now() 166 | self.odom.pose.pose.position = geometry_msgs.msg.Point(self.x_e, self.y_e, 0.0) 167 | quat = tf.transformations.quaternion_from_euler(0, 0, self.th); 168 | self.odom.pose.pose.orientation.x = quat[0] 169 | self.odom.pose.pose.orientation.y = quat[1] 170 | self.odom.pose.pose.orientation.z = quat[2] 171 | self.odom.pose.pose.orientation.w = quat[3] 172 | self.odom.twist.twist.linear = Vector3(vx, vy, 0.0) 173 | self.odom.twist.twist.angular = Vector3(0.0, 0.0, omega) 174 | 175 | self.odom_pub.publish(self.odom) 176 | 177 | if self.param["enable_tf"]: 178 | self.tf_br.sendTransform((self.x_e, self.y_e, 0), 179 | quat, 180 | self.odom.header.stamp, 181 | self.param["baseId"], 182 | self.param["odomId"] ) 183 | 184 | self.last_odom_time = time_now; 185 | #else: 186 | #rospy.logerr('data not available') -------------------------------------------------------------------------------- /script/ardu_odom_comm.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import time 3 | import math 4 | import serial 5 | import threading 6 | import struct 7 | import binascii 8 | 9 | class ArduOdomCommunication: 10 | def __init__(self, port, baudrate, odom_freq, timeout): 11 | #===== serial config =====# 12 | self.port = port 13 | self.baudrate = baudrate 14 | self.odom_freq = odom_freq 15 | self.timeout = timeout 16 | #===== status flags =====# 17 | self._serialOK = False 18 | self._is_synced = False 19 | self._odom_new_data = False 20 | #self._cmd_new_data = False 21 | self._first_odom = True 22 | #self._first_cmd = True 23 | 24 | self.t_stop = threading.Event() # threading stuff 25 | self.error_flag = False 26 | try: 27 | rospy.loginfo("Opening serial port: "+ self.port) 28 | self.serial = serial.Serial(self.port, self.baudrate, timeout=self.timeout) 29 | except Exception: 30 | rospy.logerr("Failed to open serial port") 31 | raise 32 | return 33 | 34 | self.odom = {"pos":[0, 0, 0], "vel":[0, 0, 0]} 35 | self.cmd = [0, 0, 0] 36 | self.odom_seq = 0 37 | self.cmd_seq = 0 38 | self.last_odom_seq = 0 39 | self.last_cmd_seq = 0 40 | 41 | '''******************************************************************* 42 | Independent thread that constantly checking Serial port 43 | *******************************************************************''' 44 | def serial_thread(self): 45 | #===== Serial init =====# 46 | rospy.loginfo("===== Serial thread =====") 47 | rospy.loginfo("Rebooting Arduino...") 48 | time.sleep(1) # for Arduino to reboot 49 | rospy.loginfo("First 3 data readings:") 50 | self.serial.reset_input_buffer() 51 | 52 | 53 | try: 54 | init_msg = self.serial.readline() 55 | for x in range(0, 3): 56 | init_msg = self.serial.readline() # Note: try restart motor board if error 57 | print( init_msg.encode('ascii')[0:(len(init_msg)-1)] ) 58 | except Exception: 59 | rospy.logerr("Port %s timeout after %d seconds", self.port, self.timeout) 60 | self.serial.close() 61 | raise 62 | return 63 | 64 | #===== Send starting signal =====# 65 | self._serialOK = True 66 | rospy.loginfo("Sending starting signal \"B\"...") 67 | self.serial.write( "BB".encode('ascii') ) 68 | init_msg = self.serial.readline() # clear waiting 4 69 | 70 | 71 | #===== Start packet rx + parsing loop =====# 72 | while (not self.t_stop.is_set()): 73 | try: 74 | reading = self.serial.read(2) 75 | except Exception: 76 | self.error_flag = True 77 | break 78 | 79 | ### encoder data packet 80 | if reading[0] == '\x00' and reading[1] == '\xFF': 81 | try: 82 | ser_in = self.serial.read(30) 83 | except Exception: 84 | self.error_flag = True 85 | break 86 | 87 | self.odom_decode(ser_in, 30) 88 | self._is_synced = True 89 | #== debug 90 | #toHex = lambda x: "".join("{:02X}".format(ord(c)) for c in reading) 91 | #print(toHex(b'\x03\xac23\n')) 92 | 93 | #we have only one packet type now 94 | #========= command data packet =========# 95 | #elif reading[0] == '\xFF' and reading[1] == '\xFC': 96 | # ser_in = self.serial.read(13) 97 | # self.cmd_decode(ser_in, 13) 98 | # self._is_synced = True 99 | 100 | ### lost sync 101 | else: 102 | if self._is_synced: 103 | if self._first_odom: 104 | rospy.loginfo("Initial syncing...") 105 | self._is_synced = False 106 | continue 107 | rospy.logerr('Out of sync:') 108 | toHex = lambda x: "".join("{:02X}".format(ord(c)) for c in reading) 109 | print(toHex(b'\x03\xac23\n')) 110 | 111 | bfr = self.serial.read(1) 112 | toHex = lambda x: "".join("{:02X}".format(ord(c)) for c in bfr) 113 | print(toHex(b' ')) 114 | self._is_synced = False 115 | 116 | 117 | 118 | if self.error_flag: 119 | rospy.logerr('serial read error') 120 | self.serial.write( 'RRR'.encode('ascii') ) 121 | self.serial.close() 122 | self._serialOK = False 123 | self._is_synced = False 124 | self._odom_new_data = False 125 | self._cmd_new_data = False 126 | print("thread ends") 127 | raise 128 | return 129 | 130 | #===== if thread stop is set =====# 131 | print("Sending stop signal to motor control") 132 | self.serial.write( 'R'.encode('ascii') ) 133 | self.serial.close() 134 | self._serialOK = False 135 | self._is_synced = False 136 | self._odom_new_data = False 137 | self._cmd_new_data = False 138 | print("thread ends") 139 | 140 | '''******************************************************************* 141 | Decode odometry data 142 | *******************************************************************''' 143 | def odom_decode(self, data, size): 144 | #https://docs.python.org/3/library/struct.html 145 | self.odom_seq = struct.unpack('f', data[0:4])[0] # int 4B 171 | self.cmd[1] = struct.unpack('>f', data[4:8])[0] 172 | self.cmd[2] = struct.unpack('>f', data[8:12])[0] 173 | self.cmd_seq = struct.unpack('B', data[12:13])[0] # unsigned byte 174 | 175 | #debug 176 | #print("cmdA", self.cmd[0], "cmdB", self.cmd[1], "cmdC", self.cmd[2]) 177 | if (self.cmd_seq != ((self.last_cmd_seq + 1)%256) ): 178 | if not self._first_cmd: 179 | rospy.logwarn("cmd seq mismatch, prev: %d, now:%d", self.last_cmd_seq, self.cmd_seq) 180 | else: 181 | self._first_cmd = False 182 | self.last_cmd_seq = self.cmd_seq 183 | self._cmd_new_data = True 184 | self._pos_new_data = True''' 185 | 186 | 187 | '''******************************************************************* 188 | Module communication from outside 189 | *******************************************************************''' 190 | def serialOK(self): 191 | return self._serialOK 192 | 193 | def odom_new_data(self): 194 | return self._odom_new_data 195 | 196 | def get_odom_data(self): 197 | if self._odom_new_data: 198 | # data assign 199 | self._odom_new_data = False 200 | return {"seq":self.odom_seq, "micro_s": self.odom_ut, 201 | "pos":self.odom["pos"], "vel":self.odom["vel"] } 202 | else: 203 | return None 204 | 205 | ''' NO CMD PACKET AT THE MOMENT 206 | def get_cmd_data(self): 207 | if self._cmd_new_data: 208 | self._cmd_new_data = False 209 | return {"seq":self.cmd_seq, "cmd":self.cmd} 210 | else: 211 | return None''' 212 | 213 | '''******************************************************************* 214 | send vel_cmd to vehicle 215 | *******************************************************************''' 216 | def send_dir_cmd(self, wheel): 217 | if self._serialOK: 218 | if wheel["l"] > 0: 219 | if wheel["r"] >0: 220 | dir_cmd = 'W' 221 | elif wheel["r"] < 0: 222 | dir_cmd = 'D' 223 | else: 224 | dir_cmd = 'E' 225 | elif wheel["l"] < 0: 226 | if wheel["r"] >0: 227 | dir_cmd = 'A' 228 | elif wheel["r"] < 0: 229 | dir_cmd = 'S' 230 | else: 231 | dir_cmd = 'Z' 232 | else: 233 | if wheel["r"] >0: 234 | dir_cmd = 'Q' 235 | elif wheel["r"] < 0: 236 | dir_cmd = 'C' 237 | else: 238 | dir_cmd = ' ' 239 | # debug 240 | #print(binascii.hexlify(serial_cmd)) 241 | self.serial.write( dir_cmd ) 242 | 243 | def stopThread(self): 244 | self.t_stop.set() 245 | if self._serialOK: 246 | while self._serialOK: 247 | time.sleep(0.1) # wait for thread to stop 248 | self.serial.close() -------------------------------------------------------------------------------- /script/ardu_odom_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import rospy 3 | import sys 4 | import threading 5 | import math 6 | 7 | from ardu_odom import ArduOdom 8 | from ardu_odom_comm import ArduOdomCommunication 9 | 10 | if __name__ == "__main__": 11 | rospy.init_node('serial_odom', anonymous=True, disable_signals=True) # ROS node init 12 | node = ArduOdom() # using node object 13 | rospy.loginfo("Arduino serial odometry starting...") 14 | 15 | # try to start a serial communication object (open com port) 16 | try: 17 | omni_com = ArduOdomCommunication( port = node.param["device_port"], 18 | baudrate = node.param["baudrate"] , 19 | odom_freq = node.param["odom_freq"] , 20 | timeout = node.param["timeout"] ) 21 | except: 22 | print("Fail to create serial connection, shutting down") 23 | rospy.signal_shutdown("open com port error") 24 | sys.exit(1) 25 | 26 | # try to start a thread for serial comm handling 27 | try: 28 | thread = threading.Thread(target=omni_com.serial_thread) 29 | #thread.daemon=True 30 | thread.start() 31 | except: 32 | rospy.signal_shutdown("serial thread error") 33 | omni_com.stopThread() 34 | sys.exit(1) 35 | 36 | node.set_data_port(omni_com) 37 | rospy.spin() 38 | 39 | print("SIGINT, stopping...") 40 | omni_com.stopThread() 41 | thread.join() 42 | rospy.signal_shutdown("user ends") 43 | print("Node ends") 44 | -------------------------------------------------------------------------------- /script/omni_base/omni_base_driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import rospy 3 | import tf 4 | import time 5 | import sys 6 | import math 7 | from operator import add 8 | import serial 9 | import string 10 | import threading 11 | import geometry_msgs.msg 12 | from geometry_msgs.msg import Twist 13 | from geometry_msgs.msg import Vector3 14 | #from geometry_msgs.msg import Point 15 | from nav_msgs.msg import Odometry 16 | from sensor_msgs.msg import Imu 17 | 18 | from omni_serial_com import OmniSerialCom 19 | 20 | class Omni_base_node: 21 | def __init__(self): 22 | #======== Get params =========# 23 | self.param = {} # create a dictionary 24 | #self.debug_mode = bool(rospy.get_param('~debug_mode', False)) # true for detail info 25 | self.param["vel_cmd"] = rospy.get_param("~cmd_vel", 'cmd_vel') 26 | self.param["odom_topic"] = rospy.get_param("~odom_topic", 'odom') 27 | self.param["imu_topic"] = rospy.get_param("~imu_topic", 'imu') 28 | self.param["baseId"] = rospy.get_param('~base_id', 'base_link') # base frame id 29 | self.param["odomId"] = rospy.get_param('~odom_id', 'odom') # odom frame id 30 | self.param["imuId"] = rospy.get_param('~imu_id', 'imu') # odom frame id 31 | self.param["enable_tf"] = rospy.get_param("~enable_tf", True) 32 | self.param["device_port"] = rospy.get_param('~port', '/dev/ttyS2') # port 33 | self.param["baudrate"] = int( rospy.get_param('~baudrate', '115200') ) # baudrate 34 | self.param["timeout"] = float( rospy.get_param('~serial_timeout', '10') ) # 35 | self.param["odom_freq"] = float( rospy.get_param('~odom_freq', '30') ) # hz of communication 36 | self.param["imu_freq"] = float( rospy.get_param('~imu_freq', '120') ) # hz of communication 37 | self.param["tx_freq"] = float( rospy.get_param('~tx_freq', '5') ) # hz of communication 38 | self.param["cmd_timeout"] = float( rospy.get_param('~cmd_vel_timeout', '3') ) # 39 | self.param["vel_gain"] = float( rospy.get_param('~vel_gain', '70') ) # hz of communication 40 | self.param["omg_gain"] = float( rospy.get_param('~omg_gain', '500') ) # hz of communication 41 | 42 | rospy.set_param("omni_base_driver", self.param) 43 | 44 | #========== ROS message ==========# 45 | rospy.loginfo("Initiating Node") 46 | rospy.loginfo("Publishing odometry at: \"" + self.param["odom_topic"] + "\"") 47 | rospy.loginfo("Publishing imu at: \"" + self.param["imu_topic"] + "\"") 48 | rospy.loginfo("Subscribing to \"" + self.param["vel_cmd"] + "\"") 49 | 50 | self.data_handle_ok = False # prevent node from starting pre maturely 51 | 52 | #========== ROS handler ==========# 53 | self.enc_sub = rospy.Subscriber(self.param["vel_cmd"], Twist, self.velcmd_cb, queue_size=10) 54 | self.odom_pub = rospy.Publisher(self.param["odom_topic"], Odometry, queue_size=10) # pubisher 55 | self.imu_pub = rospy.Publisher(self.param["imu_topic"], Imu, queue_size=10) 56 | self.timer_odom = rospy.Timer(rospy.Duration(1.0/self.param["odom_freq"]), self.odomPub) # timer 57 | self.timer_imu = rospy.Timer(rospy.Duration(1.0/self.param["imu_freq"]), self.imuPub) 58 | self.timer_txcmd = rospy.Timer( rospy.Duration(1.0/self.param["tx_freq"]), 59 | self.tx_vel_cmd) 60 | 61 | if self.param["enable_tf"]: 62 | rospy.loginfo("Publishing transform from %s to %s", self.param["odomId"], self.param["baseId"]) 63 | self.tf_br = tf.TransformBroadcaster() 64 | 65 | #========== variable for ROS publisher ==========# 66 | self.odom = Odometry() 67 | self.odom.header.seq = 0 68 | self.odom.header.frame_id = self.param["odomId"] 69 | self.odom.child_frame_id = self.param["baseId"] 70 | for i in range(36): 71 | self.odom.pose.covariance[i] = 0 72 | self.odom.twist.covariance[i] = 0 73 | self.odom.twist.covariance[0] = 1.0 74 | self.odom.twist.covariance[35] = 1.0 75 | 76 | self.imu = Imu() 77 | self.imu.header.seq = 0 78 | self.imu.header.frame_id = self.param["imuId"] 79 | self.imu.orientation.x = 0 80 | self.imu.orientation.y = 0 81 | self.imu.orientation.z = 0 82 | self.imu.orientation.w = 0 83 | for i in range(9): 84 | self.imu.orientation_covariance[i] = 0 85 | self.imu.angular_velocity_covariance[i] = 0 86 | self.imu.linear_acceleration_covariance[i] = 0 87 | self.imu.orientation_covariance[0] = -1 88 | self.imu.angular_velocity_covariance[0] = math.radians(0.05) # P.12 of the MPU6050 datasheet 89 | self.imu.angular_velocity_covariance[4] = math.radians(0.05) 90 | self.imu.angular_velocity_covariance[8] = math.radians(0.05) 91 | self.imu.linear_acceleration_covariance[0] = 400*10**-6*9.81 # in m/s**2 92 | self.imu.linear_acceleration_covariance[4] = 400*10**-6*9.81 93 | self.imu.linear_acceleration_covariance[8] = 400*10**-6*9.81 94 | 95 | self.x_e = float(0) 96 | self.y_e = float(0) 97 | self.th = float(0) 98 | self.odom_last_seq = 0; 99 | self.enc_count_per_revo = (390*4) 100 | self.wheel_radius = 0.029 101 | self.base_radius = 0.140 # 14.3 cm radius 102 | self.accel_sensitivity = 2*9.81 # 2g 103 | self.gyro_sensitivity = math.radians(250) # 250deg/sec 104 | self.last_odom_time = time.time() 105 | self.last_cmd_vel_time = self.last_odom_time 106 | self.no_cmd_received = True 107 | self.first_odom = True 108 | 109 | self.veh_cmd = {"Vx":0, "Vy":0, "Omega":0} 110 | 111 | def set_data_port(self, _serial_handle): 112 | self.data_handle = _serial_handle 113 | self.data_handle_ok = True 114 | 115 | '''******************************************************************* 116 | ROS cmd_vel Subscriber callback 117 | *******************************************************************''' 118 | def velcmd_cb(self, vel_cmd): 119 | self.veh_cmd["Vx"] = vel_cmd.linear.x * self.param["vel_gain"] 120 | self.veh_cmd["Vy"] = vel_cmd.linear.y * self.param["vel_gain"] 121 | self.veh_cmd["Omega"] = vel_cmd.angular.z * self.param["omg_gain"] 122 | self.no_cmd_received = False 123 | self.last_cmd_vel_time = time.time() 124 | 125 | '''******************************************************************* 126 | send velocity command to serial handler 127 | *******************************************************************''' 128 | def tx_vel_cmd(self, event): 129 | if( (time.time()-self.last_cmd_vel_time) > self.param["cmd_timeout"] and 130 | not self.no_cmd_received ): 131 | self.veh_cmd["Vx"] = 0 132 | self.veh_cmd["Vy"] = 0 133 | self.veh_cmd["Omega"] = 0 134 | self.no_cmd_received = True 135 | rospy.logwarn("no topic \"%s\" received, base stop.", self.param["vel_cmd"]) 136 | 137 | cmd = [ int(self.veh_cmd["Vx"]), int(self.veh_cmd["Vy"]), int(self.veh_cmd["Omega"]) ] 138 | self.data_handle.send_vel_cmd(cmd) 139 | 140 | 141 | '''******************************************************************* 142 | ROS Odometry topic publisher, called by timer 143 | *******************************************************************''' 144 | def odomPub(self, event): 145 | if self.data_handle_ok and self.data_handle.serialOK(): 146 | if self.data_handle.odom_new_data(): 147 | xyt_rtn = self.data_handle.get_odom_data() 148 | if xyt_rtn is None: 149 | return 150 | # sanity check 151 | if (xyt_rtn["seq"] != ((self.odom_last_seq + 1)%256) ): 152 | if (not self.first_odom): 153 | rospy.logwarn("callback sequence mismatch, prev: %d, now:%d", 154 | self.odom_last_seq, xyt_rtn["seq"] ) 155 | self.odom_last_seq = xyt_rtn["seq"] 156 | return # disregard first data 157 | else: 158 | self.first_odom = False 159 | self.odom_last_seq = xyt_rtn["seq"] 160 | return # disregard first data 161 | self.odom_last_seq = xyt_rtn["seq"] 162 | 163 | #debug 164 | #self.temp += xyt_rtn["enc_dt"][1] 165 | #print(self.temp) 166 | #print(" A: {} B: {} C: {}".format( xyt_rtn["enc_dt"][0], 167 | # xyt_rtn["enc_dt"][1], 168 | # xyt_rtn["enc_dt"][2] ) ) 169 | ''' 170 | dx = float(0) 171 | dy = float(0) 172 | dth = float(0) 173 | dx_e = float(0) 174 | dy_e = float(0) 175 | 176 | dx = float(-xyt_rtn["enc_dt"][0]+xyt_rtn["enc_dt"][1]) / (2* math.cos(math.radians(30))) 177 | dy = float(-xyt_rtn["enc_dt"][0]-xyt_rtn["enc_dt"][1] + 2*xyt_rtn["enc_dt"][2]) / 3 178 | dth = float(-xyt_rtn["enc_dt"][0]-xyt_rtn["enc_dt"][1] - xyt_rtn["enc_dt"][2]) / 3 179 | time_now = time.time() 180 | dt = time_now - self.last_odom_time 181 | 182 | dx = dx/self.enc_count_per_revo * 2 * math.pi * self.wheel_radius 183 | dy = dy/self.enc_count_per_revo * 2 * math.pi * self.wheel_radius 184 | dth = dth/self.enc_count_per_revo * 2 * math.pi * self.wheel_radius / self.base_radius 185 | #print("dx",dx,"dy",dy,"dth",dth) 186 | 187 | # convert to earth frame 188 | dx_e = dx * math.cos(self.th + dth/2) - dy * math.sin(self.th + dth/2) 189 | dy_e = dx * math.sin(self.th + dth/2) + dy * math.cos(self.th + dth/2) 190 | 191 | if self.data_handle.pos_new_data(): 192 | pos = self.data_handle.get_pos_data() 193 | self.x_e = pos["x"] 194 | self.y_e = pos["y"] 195 | self.th = pos["th"] 196 | else: 197 | self.x_e += dx_e 198 | self.y_e += dy_e 199 | self.th += d_th''' 200 | dx_e = float(xyt_rtn["pos_dt"][0])/10000 201 | dy_e = float(xyt_rtn["pos_dt"][1])/10000 202 | d_th = float(xyt_rtn["pos_dt"][2])/10000 203 | vx = dx_e * math.cos(self.th + d_th/2) + dy_e * math.sin(self.th + d_th/2) 204 | vy = -dx_e * math.sin(self.th + d_th/2) + dy_e * math.cos(self.th + d_th/2) 205 | time_now = time.time() 206 | self.x_e += dx_e 207 | self.y_e += dy_e 208 | self.th += d_th 209 | dt = time_now - self.last_odom_time 210 | 211 | #======= ROS topic publisher =======# 212 | self.odom.header.seq += 1 213 | self.odom.header.stamp = rospy.Time.now() 214 | self.odom.pose.pose.position = geometry_msgs.msg.Point(self.x_e, self.y_e, 0.0) 215 | quat = tf.transformations.quaternion_from_euler(0, 0, self.th); 216 | self.odom.pose.pose.orientation.x = quat[0] 217 | self.odom.pose.pose.orientation.y = quat[1] 218 | self.odom.pose.pose.orientation.z = quat[2] 219 | self.odom.pose.pose.orientation.w = quat[3] 220 | self.odom.twist.twist.linear = Vector3(vx / dt, vy / dt, 0.0) 221 | self.odom.twist.twist.angular = Vector3(0.0, 0.0, d_th/dt) 222 | 223 | self.odom_pub.publish(self.odom) 224 | 225 | if self.param["enable_tf"]: 226 | self.tf_br.sendTransform((self.x_e, self.y_e, 0), 227 | quat, 228 | self.odom.header.stamp, 229 | self.param["baseId"], 230 | self.param["odomId"] ) 231 | 232 | self.last_odom_time = time_now; 233 | #else: 234 | #rospy.logerr('data not available') 235 | 236 | '''******************************************************************* 237 | ROS Imu topic publisher, called by timer 238 | *******************************************************************''' 239 | def imuPub(self, event): 240 | if self.data_handle_ok and self.data_handle.serialOK(): 241 | if self.data_handle.imu_new_data(): 242 | imu_rtn = self.data_handle.get_imu_data() 243 | if imu_rtn is None: 244 | return 245 | self.imu.linear_acceleration.x = imu_rtn["accel"][0] * self.accel_sensitivity / 32768 246 | self.imu.linear_acceleration.y = imu_rtn["accel"][1] * self.accel_sensitivity / 32768 247 | self.imu.linear_acceleration.z = imu_rtn["accel"][2] * self.accel_sensitivity / 32768 248 | self.imu.angular_velocity.x = imu_rtn["gyro"][0] * self.gyro_sensitivity / 32768 249 | self.imu.angular_velocity.y = imu_rtn["gyro"][1] * self.gyro_sensitivity / 32768 250 | self.imu.angular_velocity.z = imu_rtn["gyro"][2] * self.gyro_sensitivity / 32768 251 | 252 | self.imu.header.seq += 1 253 | self.imu.header.stamp = rospy.Time.now() 254 | 255 | self.imu_pub.publish(self.imu) 256 | #else: 257 | #rospy.logerr('data not available') 258 | 259 | 260 | if __name__ == "__main__": 261 | 262 | # ROS Init 263 | rospy.init_node('serial_odom', anonymous=True, disable_signals=True) 264 | node = Omni_base_node() 265 | #rospy.loginfo("Serial odometry starting") 266 | try: 267 | omni_com = OmniSerialCom( port = node.param["device_port"] , 268 | baudrate = node.param["baudrate"] , 269 | imu_freq = node.param["imu_freq"] , 270 | odom_freq = node.param["odom_freq"] , 271 | timeout = node.param["timeout"] ) 272 | except: 273 | print("Fail to create serial connection, shutting down") 274 | rospy.signal_shutdown("serial err") 275 | sys.exit(0) 276 | 277 | try: 278 | thread = threading.Thread(target=omni_com.serial_thread) 279 | #thread.daemon=True 280 | thread.start() 281 | except: 282 | rospy.signal_shutdown("serial err") 283 | omni_com.stopThread() 284 | sys.exit(0) 285 | 286 | node.set_data_port(omni_com) 287 | rospy.spin() 288 | 289 | print("SIGINT, stopping...") 290 | omni_com.stopThread() 291 | thread.join() 292 | rospy.signal_shutdown("user ends") 293 | print("Node ends") 294 | -------------------------------------------------------------------------------- /script/omni_base/omni_serial_com.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | 3 | import time 4 | import sys 5 | import math 6 | import serial 7 | import threading 8 | import struct 9 | import binascii 10 | 11 | class OmniSerialCom: 12 | def __init__(self, port, baudrate, imu_freq, odom_freq, timeout): 13 | 14 | self.port = port 15 | self.baudrate = baudrate 16 | self.imu_freq = imu_freq 17 | self.odom_freq = odom_freq 18 | self.timeout = timeout 19 | 20 | self._serialOK = False 21 | 22 | self.is_synced = False 23 | self._imu_new_data = False 24 | self._odom_new_data = False 25 | self._cmd_new_data = False 26 | self._first_odom = True 27 | self._first_cmd = True 28 | 29 | self.t_stop = threading.Event() 30 | try: 31 | rospy.loginfo("Opening serial port: "+ self.port) 32 | self.serial = serial.Serial(self.port, self.baudrate, timeout=self.timeout) 33 | except: 34 | rospy.logerr("serial err") 35 | raise Exception('error') 36 | return 37 | 38 | self.imu = {"accel":[0, 0, 0], "gyro":[0, 0, 0]} 39 | self.odom = [0, 0, 0] 40 | self.cmd = [0, 0, 0] 41 | self.odom_seq = 0 42 | self.cmd_seq = 0 43 | self.last_odom_seq = 0 44 | self.last_cmd_seq = 0 45 | 46 | 47 | '''******************************************************************* 48 | Independent thread that constantly checking Serial port 49 | *******************************************************************''' 50 | def serial_thread(self): 51 | # Serial initialization 52 | rospy.loginfo("===== Serial thread =====") 53 | try: 54 | rospy.loginfo("First 3 data readings:") 55 | self.serial.reset_input_buffer() 56 | init_msg = self.serial.readline() 57 | for x in range(0, 3): 58 | init_msg = self.serial.readline() ### Note: try restart motor board if error ### 59 | print( init_msg.encode('ascii')[0:(len(init_msg)-1)] ) 60 | except serial.serialutil.SerialException: 61 | rospy.logerr("Port timeout after %d seconds at: %s", self.timeout, self.port) 62 | self.serial.close 63 | raise Exception('timeout') 64 | return 65 | 66 | # sent start signal 67 | self._serialOK = True 68 | rospy.loginfo("Sending starting signal \"SSSS\"") 69 | self.serial.write( "SSSS".encode('ascii') ) 70 | # time.sleep(0.01) # for Arduino to reboot 71 | 72 | # continuous packet recieve 73 | while (not self.t_stop.is_set()): 74 | reading = self.serial.read(2) 75 | 76 | #========= imu data packet =========# 77 | if reading[0] == '\xFF' and reading[1] == '\xFA': 78 | ser_in = self.serial.read(12) 79 | self.imu_decode(ser_in, 12) 80 | self.is_synced = True 81 | #debug 82 | #toHex = lambda x: "".join("{:02X}".format(ord(c)) for c in reading) 83 | #print(toHex(b'\x03\xac23\n')) 84 | 85 | #========= encoder data packet =========# 86 | elif reading[0] == '\xFF' and reading[1] == '\xFB': 87 | ser_in = self.serial.read(7) 88 | self.odom_decode(ser_in, 7) 89 | self.is_synced = True 90 | 91 | #========= command data packet =========# 92 | elif reading[0] == '\xFF' and reading[1] == '\xFC': 93 | ser_in = self.serial.read(13) 94 | self.cmd_decode(ser_in, 13) 95 | self.is_synced = True 96 | 97 | #========= lost sync =========# 98 | else: 99 | rospy.logerr('Out of sync:') 100 | toHex = lambda x: "".join("{:02X}".format(ord(c)) for c in reading) 101 | print(toHex(b'\x03\xac23\n')) 102 | 103 | bfr = self.serial.read(1) 104 | toHex = lambda x: "".join("{:02X}".format(ord(c)) for c in bfr) 105 | print(toHex(b' ')) 106 | self.is_synced = False 107 | 108 | # if threads ends here 109 | print("Sending stop signal to motor control") 110 | self.serial.write( 'R'.encode('ascii') ) 111 | self._serialOK = False 112 | self.is_synced = False 113 | self._odom_new_data = False 114 | self._imu_new_data = False 115 | self._cmd_new_data = False 116 | print("thread ends") 117 | 118 | 119 | '''******************************************************************* 120 | Decode imu data 121 | *******************************************************************''' 122 | def imu_decode(self, data, size): 123 | #https://docs.python.org/3/library/struct.html 124 | self.imu["accel"][0] = struct.unpack('>h', data[0:2])[0] # signed short, 2B 125 | self.imu["accel"][1] = struct.unpack('>h', data[2:4])[0] 126 | self.imu["accel"][2] = struct.unpack('>h', data[4:6])[0] 127 | self.imu["gyro"][0] = struct.unpack('>h', data[6:8])[0] 128 | self.imu["gyro"][1] = struct.unpack('>h', data[8:10])[0] 129 | self.imu["gyro"][2] = struct.unpack('>h', data[10:12])[0] 130 | #debug 131 | #print("imu", self.seq, " t_micro:", self.t_micro) 132 | self._imu_new_data = True 133 | 134 | '''******************************************************************* 135 | Decode odometry data 136 | *******************************************************************''' 137 | def odom_decode(self, data, size): 138 | #https://docs.python.org/3/library/struct.html 139 | self.odom[0] = struct.unpack('>h', data[0:2])[0] # signed short 2B 140 | self.odom[1] = struct.unpack('>h', data[2:4])[0] 141 | self.odom[2] = struct.unpack('>h', data[4:6])[0] 142 | self.odom_seq = struct.unpack('B', data[6:7])[0] # unsigned byte 143 | 144 | #debug 145 | #print("odom", self.odom_seq, self.odom[0:3]) 146 | if (self.odom_seq != ((self.last_odom_seq + 1)%256) ): 147 | if not self._first_odom: 148 | rospy.logwarn("odom seq mismatch, prev: %d, now:%d", self.last_odom_seq, self.odom_seq) 149 | else: 150 | self._first_odom = False 151 | self.last_odom_seq = self.odom_seq 152 | self._odom_new_data = True 153 | 154 | '''******************************************************************* 155 | Decode 5hz data 156 | *******************************************************************''' 157 | def cmd_decode(self, data, size): 158 | #https://docs.python.org/3/library/struct.html 159 | self.cmd[0] = struct.unpack('>f', data[0:4])[0] # int 4B 160 | self.cmd[1] = struct.unpack('>f', data[4:8])[0] 161 | self.cmd[2] = struct.unpack('>f', data[8:12])[0] 162 | self.cmd_seq = struct.unpack('B', data[12:13])[0] # unsigned byte 163 | 164 | #debug 165 | #print("cmdA", self.cmd[0], "cmdB", self.cmd[1], "cmdC", self.cmd[2]) 166 | if (self.cmd_seq != ((self.last_cmd_seq + 1)%256) ): 167 | if not self._first_cmd: 168 | rospy.logwarn("cmd seq mismatch, prev: %d, now:%d", self.last_cmd_seq, self.cmd_seq) 169 | else: 170 | self._first_cmd = False 171 | self.last_cmd_seq = self.cmd_seq 172 | self._cmd_new_data = True 173 | self._pos_new_data = True 174 | 175 | '''******************************************************************* 176 | Module communication from outside 177 | *******************************************************************''' 178 | def serialOK(self): 179 | if self._serialOK: # and self.is_synced 180 | return True 181 | else: 182 | return False 183 | 184 | def imu_new_data(self): 185 | return self._imu_new_data 186 | 187 | def odom_new_data(self): 188 | return self._odom_new_data 189 | 190 | def cmd_new_data(self): 191 | return self._cmd_new_data 192 | 193 | def pos_new_data(self): 194 | return self._pos_new_data 195 | 196 | def get_imu_data(self): 197 | if self._imu_new_data: 198 | # data assign 199 | self._imu_new_data = False 200 | return self.imu 201 | else: 202 | return None 203 | 204 | def get_odom_data(self): 205 | if self._odom_new_data: 206 | # data assign 207 | self._odom_new_data = False 208 | return {"seq":self.odom_seq, "pos_dt":self.odom} 209 | else: 210 | return None 211 | 212 | def get_cmd_data(self): 213 | if self._cmd_new_data: 214 | self._cmd_new_data = False 215 | return {"seq":self.cmd_seq, "cmd":self.cmd} 216 | else: 217 | return None 218 | 219 | '''******************************************************************* 220 | send vel_cmd to vehicle 221 | *******************************************************************''' 222 | def send_vel_cmd(self, veh_cmd): 223 | if self._serialOK: 224 | serial_cmd = bytearray(b'\xFF\xFE') 225 | serial_cmd.append(0x01) # base vector mode 226 | 227 | clamp = lambda n, minn, maxn: max(min(maxn, n), minn) 228 | serial_cmd += struct.pack('>h',clamp( veh_cmd[0], -37268, 32767 ) ) #2-bytes 229 | serial_cmd += struct.pack('>h',clamp( veh_cmd[1], -37268, 32767 ) ) 230 | serial_cmd += struct.pack('>h',clamp( veh_cmd[2], -37268, 32767 ) ) 231 | 232 | #debug 233 | #print(binascii.hexlify(serial_cmd)) 234 | 235 | self.serial.write( serial_cmd ) 236 | 237 | def stopThread(self): 238 | self.t_stop.set() 239 | if self._serialOK: 240 | while self._serialOK: 241 | time.sleep(0.1) # wait for thread to stop 242 | self.serial.close() -------------------------------------------------------------------------------- /script/tf_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | void tfPub1(); 8 | //void tfPub2(); 9 | 10 | ros::Publisher v1_pub, v2_pub, v3_pub; 11 | 12 | int main(int argc, char **argv) { 13 | 14 | // ROS Init 15 | ros::init(argc, argv, "tf_test"); 16 | ros::NodeHandle nh; 17 | v1_pub = nh.advertise("v1", 10); 18 | v2_pub = nh.advertise("v2", 10); 19 | v3_pub = nh.advertise("v3", 10); 20 | ros::Rate loop_rate(10); 21 | 22 | ROS_INFO("TF test starting"); 23 | while(ros::ok()) { 24 | tfPub1(); 25 | //tfPub2(); 26 | ros::spinOnce(); 27 | loop_rate.sleep(); 28 | } 29 | 30 | ROS_INFO("Shutting down"); 31 | 32 | } 33 | void tfPub1(){ 34 | tf::Quaternion q1, q2, q3; 35 | q1.setEuler(10*M_PI/180, 25*M_PI/180, 30*M_PI/180); 36 | q2.setEuler(-10*M_PI/180, -25*M_PI/180, -30*M_PI/180); 37 | q3.setEuler(-50*M_PI/180, 10*M_PI/180, 5*M_PI/180); 38 | 39 | static tf::TransformBroadcaster tfbr1, tfbr2, tfbr3, tfbr4, tfbr5, tfbr6, tfbr7; 40 | tf::Transform tf1(q1, tf::Vector3(1, 2, 0)); 41 | tf::Transform tf2(q2, tf::Vector3(0, 0, 3)); 42 | tf::Transform tf3 = tf1*tf2; 43 | tf::Transform tf4 = tf2*tf1; 44 | tf::Transform tf5_inW; 45 | tf5_inW.setOrigin( tf1.getBasis()*tf2.getOrigin() ); 46 | tf5_inW.setRotation( tf1.getRotation() * tf2.getRotation() ); 47 | tf::Transform tf6_inW = tf1 * tf2 * tf1.inverse(); 48 | tf::Transform tf7_inW = tf1.inverse() * tf2 * tf1; 49 | 50 | ros::Time time = ros::Time::now(); 51 | tfbr1.sendTransform(tf::StampedTransform(tf1, time, "world", "tf1")); 52 | tfbr2.sendTransform(tf::StampedTransform(tf2, time, "tf1", "tf2")); 53 | tfbr3.sendTransform(tf::StampedTransform(tf3, time, "world", "tf1xtf2")); 54 | tfbr4.sendTransform(tf::StampedTransform(tf4, time, "world", "tf2xtf1")); 55 | tfbr5.sendTransform(tf::StampedTransform(tf5_inW, time, "world", "tf5inW")); 56 | tfbr6.sendTransform(tf::StampedTransform(tf6_inW, time, "world", "tf1*tf2*tf1^-1")); 57 | tfbr7.sendTransform(tf::StampedTransform(tf7_inW, time, "world", "tf1^-1*tf2*tf1")); 58 | 59 | } 60 | /* 61 | void tfPub2(){ 62 | ros::Time time = ros::Time::now(); 63 | static int i = 0; 64 | 65 | tf::Quaternion q1, q2, q3; 66 | 67 | q2.setEuler(-10*M_PI/180, -25*M_PI/180, -30*M_PI/180); 68 | q3.setEuler(-50*M_PI/180, 10*M_PI/180, 5*M_PI/180); 69 | 70 | static tf::TransformBroadcaster tfbr1, tfbr2; 71 | tf::Transform tf1(q1, tf::Vector3(-1, -2, 0)); 72 | tf::Transform tf2(q2, tf::Vector3(0, 0, 2)); 73 | 74 | geometry_msgs::poseStamped pose1, pose2, pose3, pose4; 75 | tf::Vector3 tf_pose1(1, 2, 3), tf_pose2(3, 2, 1); 76 | pose1.header.seq = i; 77 | pose1.header.stamp = time; 78 | pose1.header.frame_id = "tf1"; 79 | tf::poseTFToMsg( tf_pose1, pose1.pose ); 80 | 81 | pose2.header.seq = i; 82 | pose2.header.stamp = time; 83 | pose2.header.frame_id = "tf2"; 84 | tf::poseTFToMsg( tf_pose2, pose2.pose ); 85 | 86 | pose3.header.seq = i; 87 | pose3.header.stamp = time; 88 | pose3.header.frame_id = "tf1"; 89 | tf::poseTFToMsg( (tf1.inverse()*tf2) * tf_pose2, pose2.pose ); 90 | 91 | pose4.header.seq = i; 92 | pose4.header.stamp = time; 93 | pose4.header.frame_id = "tf1"; 94 | //tf::vector3TFToMsg( (tf1.inverse()*tf2) * tf_vec2 * (tf1.inverse()*tf2).inverse(), 95 | //vec4.vector); 96 | 97 | v1_pub.publish(vec1); 98 | v2_pub.publish(vec2); 99 | v3_pub.publish(vec3); 100 | //v4_pub.publish(vec4); 101 | 102 | tfbr1.sendTransform(tf::StampedTransform(tf1, time, "world", "tf_vec1")); 103 | tfbr2.sendTransform(tf::StampedTransform(tf2, time, "world", "tf_vec2")); 104 | 105 | i++; 106 | 107 | }*/ -------------------------------------------------------------------------------- /script/v0.3/serial_odom.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rospy 4 | import tf 5 | import time 6 | import sys 7 | import math 8 | import serial 9 | import struct 10 | import string 11 | import threading 12 | import geometry_msgs.msg 13 | from geometry_msgs.msg import Vector3 14 | #from geometry_msgs.msg import Point 15 | from nav_msgs.msg import Odometry 16 | 17 | class SerialOdom: 18 | def __init__(self): 19 | # Get params 20 | self.odom_topic = rospy.get_param("~odom_topic", 'odom') 21 | self.baseId = rospy.get_param('~base_id', 'base_link') # base frame id 22 | self.odomId = rospy.get_param('~odom_id', 'odom') # odom frame id 23 | self.enable_tf = rospy.get_param("~enable_tf", True) 24 | self.device_port = rospy.get_param('~port', '/dev/ttyACM0') # port 25 | self.baudrate = int( rospy.get_param('~baudrate', '57600') ) # baudrate 26 | self.comm_freq = float( rospy.get_param('~odom_freq', '15') ) # hz of communication 27 | #self.debug_mode = bool(rospy.get_param('~debug_mode', False)) # true for detail info 28 | self.timeout = float( rospy.get_param('~timeout', '10') ) # hz of odom pub 29 | 30 | rospy.set_param("~odom_topic", self.odom_topic) 31 | rospy.set_param("~base_id", self.baseId) 32 | rospy.set_param("~odom_id", self.odomId) 33 | rospy.get_param("~enable_tf", self.enable_tf) 34 | rospy.get_param("~port", self.device_port) 35 | rospy.get_param("~baudrate", self.baudrate) 36 | rospy.get_param("~odom_freq", self.comm_freq) 37 | 38 | rospy.loginfo("Publishing odometry: " + self.odom_topic) 39 | # ROS handler 40 | self.encsub = rospy.Subscriber('encoder', Vector3, self.wheelcmdSub, queue_size=10) 41 | self.pub = rospy.Publisher(self.odom_topic, Odometry, queue_size=10) 42 | self.timer_odom = rospy.Timer(rospy.Duration(1.0/self.comm_freq), self.odomPub) 43 | self.serialWriter = rospy.Timer(rospy.Duration(0.1), self.msgSendCB) # 10Hz 44 | if self.enable_tf: 45 | rospy.loginfo('Publishing transform from %s to %s', self.odomId, self.baseId) 46 | self.tf_br = tf.TransformBroadcaster() 47 | 48 | # variable 49 | self.odom = Odometry() 50 | self.odom.header.seq = 0 51 | self.odom.header.frame_id = self.odomId 52 | self.odom.child_frame_id = self.baseId 53 | for i in range(36): 54 | self.odom.pose.covariance[i] = 0 55 | self.odom.twist.covariance[i] = 0 56 | self.odom.twist.covariance[0] = 1.0 57 | self.odom.twist.covariance[35] = 1.0 58 | 59 | self.seq = 0 60 | self.lastSeq = 0 61 | self.t_micro = 0 62 | self.x = 0 63 | self.y = 0 64 | self.th = 0 65 | self.v_x = 0 66 | self.v_y = 0 67 | self.omega = 0 68 | 69 | self.serial_cmd = ' ' # all stop 70 | self.dataOK = False 71 | self.serialOK = False 72 | self.serial = serial.Serial() 73 | self.t_stop = threading.Event() 74 | try: 75 | rospy.loginfo("Starting on serial port: "+ self.device_port) 76 | self.serial = serial.Serial(self.device_port, self.baudrate, timeout=self.timeout) 77 | except: 78 | rospy.logerr("serial err") 79 | self.serial.close 80 | sys.exit(0) 81 | 82 | # reading loop 83 | def serial_handle(self): 84 | # Serial initialization 85 | try: 86 | self.serial.reset_input_buffer() 87 | rospy.loginfo("Reaching for serial") 88 | rospy.loginfo("Here are the first 5 data readings ...") 89 | time.sleep(1) 90 | self.serial.reset_input_buffer() 91 | init_msg = self.serial.readline() 92 | for x in range(0, 5): 93 | #init_msg = self.serial.read(10) 94 | init_msg = self.serial.readline() 95 | rospy.loginfo( init_msg.encode('ascii')[0:(len(init_msg)-1)] ) 96 | except serial.serialutil.SerialException: 97 | rospy.logerr("Port timeout after %d seconds at: %s", self.timeout, self.device_port) 98 | self.serial.close 99 | sys.exit(0) 100 | 101 | # sent start signal 102 | self.serialOK = True 103 | self.serial.write( 'B'.encode('ascii') ) 104 | time.sleep(0.08) # for Arduino to reboot 105 | # continuous packet recieve 106 | while (not self.t_stop.is_set()): 107 | reading = self.serial.read(32) 108 | if reading[0] == '\0' and reading[1] == '\xFF': 109 | self.data = reading 110 | self.dataOK = True 111 | #debug 112 | #toHex = lambda x: "".join("{:02X}".format(ord(c)) for c in reading) 113 | #print(toHex(b'\x03\xac23\n')) 114 | else: 115 | # lost sync 116 | rospy.logerr('Out of sync:') 117 | toHex = lambda x: "".join("{:02X}".format(ord(c)) for c in reading) 118 | print(toHex(b'\x03\xac23\n')) 119 | 120 | self.serial.read(1) 121 | self.dataOK = False 122 | 123 | self.serial.write( 'B'.encode('ascii') ) 124 | time.sleep(0.08) 125 | print("thread ends") 126 | 127 | def stopThread(self): 128 | self.t_stop.set() 129 | 130 | def decode(self): 131 | if self.dataOK : 132 | #https://docs.python.org/3/library/struct.html 133 | self.seq = struct.unpack('H', self.data[2:4])[0] # unsigned int 2B 134 | self.t_micro = struct.unpack('I', self.data[4:8])[0] # unsigned long 4B, arduino Micros() 135 | self.x = struct.unpack('f', self.data[8:12])[0] # float 4B 136 | self.y = struct.unpack('f', self.data[12:16])[0] 137 | self.th = struct.unpack('f', self.data[16:20])[0] 138 | self.v_x = struct.unpack('f', self.data[20:24])[0] 139 | self.v_y = struct.unpack('f', self.data[24:28])[0] 140 | self.omega = struct.unpack('f', self.data[28:32])[0] 141 | 142 | #debug 143 | #print("seq:", self.seq, " t_micro:", self.t_micro) 144 | 145 | def wheelcmdSub(self, enc): 146 | vel_left = enc.x 147 | vel_right = enc.y 148 | if vel_left > 0: 149 | if vel_right >0: 150 | self.serial_cmd = 'W' 151 | elif vel_right < 0: 152 | self.serial_cmd = 'D' 153 | else: 154 | self.serial_cmd = 'E' 155 | elif vel_left < 0: 156 | if vel_right >0: 157 | self.serial_cmd = 'A' 158 | elif vel_right < 0: 159 | self.serial_cmd = 'S' 160 | else: 161 | self.serial_cmd = 'Z' 162 | else: 163 | if vel_right >0: 164 | self.serial_cmd = 'Q' 165 | elif vel_right < 0: 166 | self.serial_cmd = 'C' 167 | else: 168 | self.serial_cmd = ' ' 169 | 170 | def odomPub(self, event): 171 | if self.serialOK: 172 | if self.dataOK: 173 | self.decode() 174 | if (self.seq != (self.lastSeq + 1) ): 175 | rospy.logwarn("Income sequence mismatch, prev: %d, now:%d", self.lastSeq, self.seq) 176 | 177 | self.odom.header.seq += 1 178 | self.odom.header.stamp = rospy.Time.now() 179 | # odom.pose is a PoseWithCovariance.msg 180 | self.odom.pose.pose.position = geometry_msgs.msg.Point(self.x, self.y, 0.0) 181 | quat = tf.transformations.quaternion_from_euler(0, 0, self.th); 182 | self.odom.pose.pose.orientation.x = quat[0] 183 | self.odom.pose.pose.orientation.y = quat[1] 184 | self.odom.pose.pose.orientation.z = quat[2] 185 | self.odom.pose.pose.orientation.w = quat[3] 186 | 187 | 188 | # odom.twist is a TwistWithCovariance.msg 189 | self.odom.twist.twist.linear = Vector3(self.v_x, self.v_y, 0.0) 190 | self.odom.twist.twist.angular = Vector3(0.0, 0.0, self.omega) 191 | 192 | self.pub.publish(self.odom) 193 | 194 | if self.enable_tf: 195 | self.tf_br.sendTransform((self.x, self.y, 0), 196 | quat, 197 | self.odom.header.stamp, 198 | self.baseId, 199 | self.odomId ) 200 | self.dataOK = False 201 | self.lastSeq = self.seq 202 | #else: 203 | #rospy.logerr('data not available') 204 | 205 | def msgSendCB(self, event): 206 | if self.serialOK: 207 | self.serial.write( self.serial_cmd.encode('ascii') ) 208 | 209 | if __name__ == "__main__": 210 | 211 | # ROS Init 212 | rospy.init_node('serial_odom', anonymous=True, disable_signals=True) 213 | 214 | # Construct BaseControl Obj 215 | rospy.loginfo("Serial odometry starting") 216 | so = SerialOdom() 217 | thread = threading.Thread(target=so.serial_handle) 218 | #thread.daemon=True 219 | thread.start() 220 | rospy.spin() 221 | print("Shutting down") 222 | so.stopThread() 223 | thread.join() 224 | so.serial.close() 225 | rospy.signal_shutdown("user ends") 226 | --------------------------------------------------------------------------------