├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config └── params.yaml ├── include ├── UWBSerial.h ├── UWBTracker.h └── ait_link │ ├── LICENSE.txt │ ├── Serial.h │ ├── ait_link.h │ ├── ait_link_impl.h │ ├── ait_link_mbed.h │ └── uwb_link │ ├── uwb_link.h │ ├── uwb_link_impl.h │ └── uwb_link_mbed.h ├── launch ├── uwb.launch ├── uwb_multi_range.launch ├── uwb_serial.launch └── uwb_tracker.launch ├── msg ├── UWBMultiRange.msg ├── UWBMultiRangeTimestamps.msg ├── UWBMultiRangeWithOffsets.msg ├── UWBRangeWithOffset.msg └── UWBTracker.msg ├── package.xml ├── src ├── UWBSerial.cpp ├── ait_link │ ├── Serial.cpp │ └── ait_link.cpp ├── gui_utils.py ├── nodes │ └── uwb_serial_node.cpp ├── uwb_multi_range_node.py └── uwb_tracker_node.py └── utils ├── calibrate.m └── calibrate_example.m /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | .spyderproject 3 | *.pyc 4 | 5 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uwb) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | std_msgs 9 | rospy 10 | cmake_modules 11 | geometry_msgs 12 | roscpp 13 | sensor_msgs 14 | std_msgs 15 | tf 16 | dynamic_reconfigure 17 | message_generation 18 | ) 19 | find_package(Boost REQUIRED COMPONENTS system thread) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 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 dependencies might have been 40 | ## pulled in transitively but can be declared for certainty nonetheless: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | add_message_files( 55 | FILES 56 | UWBMultiRange.msg 57 | UWBMultiRangeWithOffsets.msg 58 | UWBMultiRangeTimestamps.msg 59 | UWBRangeWithOffset.msg 60 | UWBTracker.msg 61 | ) 62 | 63 | #add_service_files( 64 | # FILES 65 | #) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | generate_messages( 69 | DEPENDENCIES 70 | std_msgs 71 | geometry_msgs 72 | ) 73 | 74 | ################################### 75 | ## catkin specific configuration ## 76 | ################################### 77 | ## The catkin_package macro generates cmake config files for your package 78 | ## Declare things to be passed to dependent projects 79 | ## INCLUDE_DIRS: uncomment this if you package contains header files 80 | ## LIBRARIES: libraries you create in this project that dependent projects also need 81 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 82 | ## DEPENDS: system dependencies of this project that dependent projects also need 83 | catkin_package( 84 | # INCLUDE_DIRS include 85 | # LIBRARIES position_pid 86 | CATKIN_DEPENDS message_runtime 87 | DEPENDS system_lib 88 | ) 89 | 90 | ########### 91 | ## Build ## 92 | ########### 93 | 94 | 95 | # Get all *.cpp files that Matlab Coder created 96 | FILE(GLOB matlab_sources_uwb_tracker src/codegen/MultiRangeEstimation/*.cpp) 97 | 98 | 99 | ## Specify additional locations of header files 100 | ## Your package locations should be listed before other locations 101 | # include_directories(include) 102 | include_directories( 103 | include 104 | src/codegen 105 | ${catkin_INCLUDE_DIRS} 106 | ) 107 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 108 | rosbuild_add_boost_directories() 109 | 110 | 111 | ## Declare a C++ executable 112 | add_executable(uwb_serial_node src/nodes/uwb_serial_node.cpp src/UWBSerial.cpp include/UWBSerial.h src/ait_link/Serial.cpp include/ait_link/Serial.h src/ait_link/ait_link.cpp) 113 | 114 | ## Add cmake target dependencies of the executable/library 115 | ## as an example, message headers may need to be generated before nodes 116 | add_dependencies(uwb_serial_node ${PROJECT_NAME}_generate_messages_cpp) 117 | 118 | 119 | ## Specify libraries to link a library or executable target against 120 | target_link_libraries(uwb_serial_node 121 | ${catkin_LIBRARIES} 122 | ) 123 | 124 | 125 | ############# 126 | ## Install ## 127 | ############# 128 | 129 | # all install targets should use catkin DESTINATION variables 130 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 131 | 132 | ## Mark executable scripts (Python etc.) for installation 133 | ## in contrast to setup.py, you can choose the destination 134 | # install(PROGRAMS 135 | # scripts/my_python_script 136 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 137 | # ) 138 | 139 | ## Mark executables and/or libraries for installation 140 | # install(TARGETS position_pid position_pid_node 141 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 142 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 143 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 144 | # ) 145 | 146 | ## Mark cpp header files for installation 147 | # install(DIRECTORY include/${PROJECT_NAME}/ 148 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 149 | # FILES_MATCHING PATTERN "*.h" 150 | # PATTERN ".svn" EXCLUDE 151 | # ) 152 | 153 | ## Mark other files for installation (e.g. launch and bag files, etc.) 154 | # install(FILES 155 | # # myfile1 156 | # # myfile2 157 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 158 | # ) 159 | 160 | ############# 161 | ## Testing ## 162 | ############# 163 | 164 | ## Add gtest based cpp test target and link libraries 165 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_position_pid.cpp) 166 | # if(TARGET ${PROJECT_NAME}-test) 167 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 168 | # endif() 169 | 170 | ## Add folders to be run by python nosetests 171 | # catkin_add_nosetests(test) 172 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Benjamin Hepp and Tobias Naegeli. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # UWB multi-range tracking 2 | 3 | A ROS package for tracking using _ultra-wideband_ (UWB) radios. The target needs one UWB tag and is localized by a tracker with multiple UWB radios (i.e. a robot). 4 | 5 | The package is made up of three nodes: 6 | - _uwb_serial_: Reads binary messages from a serial port. The corresponding UWB driver for an embedded board are provided at [https://developer.mbed.org/users/bhepp/code/AIT_UWB_Tracker/](https://developer.mbed.org/users/bhepp/code/AIT_UWB_Tracker/). 7 | - _uwb_multi_range_: Processes the multi-range timestamps from _uwb_serial_ and publishes calibrated and uncalibrated ranges. 8 | - _uwb_tracker_: Processes the calibrated ranges from _uwb_multi_range_ and tracks the target position using an Extended Kalman Filter. It publishes the state and covariance of the filter but also a corresponding transform. 9 | 10 | ## Dependencies 11 | 12 | - Boost (system and thread module) 13 | - numpy 14 | - scipy 15 | - rospy 16 | 17 | ## Installation 18 | 19 | Checkout the repository in your ROS _catkin_ workspace and build the workspace as usual. 20 | 21 | ## Usage 22 | 23 | You can start all three nodes using 24 | ``` 25 | roslaunch uwb uwb.launch 26 | ``` 27 | 28 | Many parameters like the serial port, baudrate, transform frames and parameter file can be specified as arguments. See the launch file for details. 29 | More low-level parameters can be defined via _rosparam_ (the default parameters should be fine in most cases). Check out the code of the nodes for details. 30 | 31 | ## Calibration 32 | 33 | The folder _utils_ contains the MATLAB script _calibrate.m_. This generates a _YAML_ file containing offsets for each UWB unit in the tracker coordinate frame as well as linear coefficients for the calibrated range-measurements. The _uwb_multi_range_ node uses the _YAML_ file to generate calibrated range measurements. 34 | 35 | A motion-capture system is useful for generating the calibration data but it could also be 36 | measured by hand. The file _calibrate_example.m_ in the _utils_ folder shows how the data for _calibrate.m_ can be generated from motion-capture data. 37 | 38 | __Note__: When recording data for calibration make sure to use the uncalibrated measurements from the _uwb_multi_range_ node (otherwise you might use already calibrated values and get a wrong calibration). 39 | 40 | ## Contact Information 41 | 42 | Benjamin Hepp 43 | Tobias Naegeli 44 | -------------------------------------------------------------------------------- /config/params.yaml: -------------------------------------------------------------------------------- 1 | num_of_units: 4 2 | unit_0: 3 | x: 0.190929 4 | y: -0.270226 5 | z: 0.011696 6 | p0: -0.331787 7 | p1: 0.942849 8 | unit_1: 9 | x: -0.205054 10 | y: -0.266330 11 | z: 0.017964 12 | p0: 0.291897 13 | p1: 0.956927 14 | unit_2: 15 | x: -0.211108 16 | y: 0.249160 17 | z: 0.009383 18 | p0: 0.765492 19 | p1: 0.957746 20 | unit_3: 21 | x: 0.186236 22 | y: 0.251674 23 | z: 0.005142 24 | p0: 0.600500 25 | p1: 0.942744 26 | -------------------------------------------------------------------------------- /include/UWBSerial.h: -------------------------------------------------------------------------------- 1 | // 2 | // Serial reader for multiple UWB ranging units. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. All rights reserved. 6 | // 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | 17 | namespace ait { 18 | 19 | class UWBSerial { 20 | public: 21 | UWBSerial(); 22 | 23 | virtual ~UWBSerial(); 24 | 25 | private: 26 | void handleMessage(const UWBMessage& msg); 27 | static void handleMessageWrapper(void* user_data, const UWBMessage& msg); 28 | 29 | void printInfoCallback(const ros::TimerEvent& ev); 30 | 31 | ros::NodeHandle nh_; 32 | ros::Publisher timestamps_pub_; 33 | Serial serial_; 34 | UWBLinkImpl uwb_link_; 35 | ros::Timer info_timer_; 36 | int msg_counter_; 37 | ros::Time last_info_time_; 38 | ros::Duration print_info_period_; 39 | }; 40 | 41 | } 42 | -------------------------------------------------------------------------------- /include/UWBTracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * UWBTracker.h 3 | * 4 | * Tracker for multiple UWB ranging units. 5 | * 6 | * Created on: Jan 12, 2016 7 | * Author: tobias.naegeli@inf.ethz.ch 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | #include "std_msgs/Float32MultiArray.h" 15 | #include 16 | #include 17 | #include 18 | 19 | // Matlab codegen includes 20 | #include "MultiRangeEstimation/MultiRangeEstimation_types.h" 21 | 22 | class UWBTracker 23 | { 24 | public: 25 | UWBTracker(); 26 | virtual ~UWBTracker(); 27 | 28 | void handleMultiRange(const uwb::UWBMultiRange& msg); 29 | void handleMultiRangeRaw(const uwb::UWBMultiRangeRaw& msg); 30 | 31 | private: 32 | typedef double scalar_type; 33 | struct UnitParameter { 34 | scalar_type x; 35 | scalar_type y; 36 | scalar_type z; 37 | scalar_type p0; 38 | scalar_type p1; 39 | }; 40 | std::vector getUnitParameters(); 41 | void loadTargetEstimationParameters(); 42 | void loadMultiRangeEstimationParameters(); 43 | 44 | ros::NodeHandle nh_; 45 | 46 | TargetEstimationParams target_estimation_params_; 47 | MultiRangeEstimationParams multi_range_estimation_params_; 48 | 49 | ros::Subscriber uwb_range_sub_; 50 | ros::Subscriber uwb_multi_range_sub_; 51 | ros::Subscriber uwb_multi_range_raw_sub_; 52 | 53 | ros::Publisher state_vector_pub_; 54 | ros::Publisher uwb_multi_range_pub_; 55 | 56 | uwb::UWBMultiRange uwb_multi_range_msg_; 57 | std_msgs::Float32MultiArray uwb_state_msg_; 58 | }; 59 | -------------------------------------------------------------------------------- /include/ait_link/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining 2 | a copy of this software and associated documentation files (the 3 | "Software"), to deal in the Software without restriction, including 4 | without limitation the rights to use, copy, modify, merge, publish, 5 | distribute, sublicense, and/or sell copies of the Software, and to 6 | permit persons to whom the Software is furnished to do so, subject to 7 | the following conditions: 8 | 9 | The above copyright notice and this permission notice shall be included 10 | in all copies or substantial portions of the Software. 11 | 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 13 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 14 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 15 | IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 16 | CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, 17 | TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 18 | SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 19 | 20 | 21 | License applies to work derived from Arduhdlc library: 22 | ============================================================================= 23 | Arduhdlc is Arduino HDLC library, which can be used over any interface. 24 | Copyright (C) 2015 Jarkko Hautakorpi 25 | 26 | This program is free software; you can redistribute it and/or 27 | modify it under the terms of the GNU General Public License 28 | as published by the Free Software Foundation; either version 2 29 | of the License, or (at your option) any later version. 30 | 31 | This program is distributed in the hope that it will be useful, 32 | but WITHOUT ANY WARRANTY; without even the implied warranty of 33 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 34 | GNU General Public License for more details. 35 | 36 | You should have received a copy of the GNU General Public License 37 | along with this program; if not, write to the Free Software 38 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 39 | 40 | 41 | 42 | 43 | Licese applies to work derived from Dust SmartMeshSDK C Library: 44 | ============================================================================= 45 | Copyright (c) 2012, Dust Networks 46 | All rights reserved. 47 | 48 | Redistribution and use in source and binary forms, with or without 49 | modification, are permitted provided that the following conditions are met: 50 | * Redistributions of source code must retain the above copyright 51 | notice, this list of conditions and the following disclaimer. 52 | * Redistributions in binary form must reproduce the above copyright 53 | notice, this list of conditions and the following disclaimer in the 54 | documentation and/or other materials provided with the distribution. 55 | * Neither the name of Dust Networks nor the 56 | names of its contributors may be used to endorse or promote products 57 | derived from this software without specific prior written permission. 58 | 59 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 60 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 61 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 62 | DISCLAIMED. IN NO EVENT SHALL DUST NETWORKS BE LIABLE FOR ANY 63 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 64 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 65 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 66 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 67 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 68 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 69 | 70 | 71 | 72 | 73 | Licese applies to work derived from Piconomic FW Library: 74 | ============================================================================= 75 | Copyright (c) 2006 Pieter Conradie [www.piconomic.co.za] 76 | All rights reserved. 77 | 78 | Redistribution and use in source and binary forms, with or without 79 | modification, are permitted provided that the following conditions are met: 80 | 81 | * Redistributions of source code must retain the above copyright 82 | notice, this list of conditions and the following disclaimer. 83 | 84 | * Redistributions in binary form must reproduce the above copyright 85 | notice, this list of conditions and the following disclaimer in 86 | the documentation and/or other materials provided with the 87 | distribution. 88 | 89 | * Neither the name of the copyright holders nor the names of 90 | contributors may be used to endorse or promote products derived 91 | from this software without specific prior written permission. 92 | 93 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 94 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 95 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 96 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 97 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 98 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 99 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 100 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 101 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 102 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 103 | POSSIBILITY OF SUCH DAMAGE. 104 | 105 | Title: HDLC encapsulation layer 106 | Author(s): Pieter Conradie 107 | Creation Date: 2007-03-31 108 | Revision Info: $Id: hdlc.h 117 2010-06-24 20:21:28Z pieterconradie $ 109 | 110 | 111 | 112 | 113 | Licese applies to work derived from HDLC byte stuffing package: 114 | ============================================================================= 115 | /***************************************************** 116 | * HDLC byte stuffing package * 117 | * * 118 | * Created on: Dec 2013 * 119 | * Author: jdn * 120 | * * 121 | ****************************************************** 122 | * * 123 | * * 124 | * (C) 2012,2013 * 125 | * * 126 | * Jens Dalsgaard Nielsen * 127 | * http://www.control.aau.dk/~jdn * 128 | * Studentspace/Satlab * 129 | * Section of Automation & Control * 130 | * Aalborg University, * 131 | * Denmark * 132 | * * 133 | * "THE BEER-WARE LICENSE" (frit efter PHK) * 134 | * wrote this file. As long as you * 135 | * retain this notice you can do whatever you want * 136 | * with this stuff. If we meet some day, and you think* 137 | * this stuff is worth it ... * 138 | * you can buy me a beer in return :-) * 139 | * or if you are real happy then ... * 140 | * single malt will be well received :-) * 141 | * * 142 | * Use it at your own risk - no warranty * 143 | *****************************************************/ -------------------------------------------------------------------------------- /include/ait_link/Serial.h: -------------------------------------------------------------------------------- 1 | // 2 | // Simple Serial Port class. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. All rights reserved. 6 | // 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | namespace ait { 14 | 15 | class Serial { 16 | public: 17 | 18 | Serial(const std::string &device, unsigned int baud_rate, size_t read_buffer_size = 8 * 1024); 19 | Serial(size_t read_buffer_size = 8 * 1024); 20 | virtual ~Serial(); 21 | 22 | void setReadCallback(const boost::function &read_callback); 23 | 24 | void open(const std::string &device, unsigned int baud_rate); 25 | void close(); 26 | void start(); 27 | void stop(); 28 | 29 | int write(char c); 30 | int write(const std::string &buf); 31 | int write(const uint8_t *buf, int size); 32 | 33 | void asyncReadSome(); 34 | 35 | private: 36 | void onRead(const boost::system::error_code &ec, size_t bytes_transferred); 37 | 38 | boost::asio::io_service io_; 39 | boost::asio::serial_port serial_port_; 40 | boost::thread async_thread_; 41 | boost::mutex mutex_; 42 | uint8_t* read_buffer_; 43 | boost::function read_callback_; 44 | }; 45 | 46 | } 47 | -------------------------------------------------------------------------------- /include/ait_link/ait_link.h: -------------------------------------------------------------------------------- 1 | // 2 | // HDLC based serial communication class. 3 | // 4 | // Adapted by Benjamin Hepp from ArduHDLC library (see LICENSE.txt) 5 | // Original work Copyright (c) 2015 Jarkko Hautakorpi et al. 6 | // Modified work Copyright (c) 2016 Benjamin Hepp. 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace ait { 15 | 16 | class AITLink { 17 | public: 18 | /* HDLC Asynchronous framing */ 19 | /* The frame boundary octet is 01111110, (7E in hexadecimal notation) */ 20 | const static uint8_t FRAME_BOUNDARY_OCTET = 0x7E; 21 | 22 | /* A "control escape octet", has the bit sequence '01111101', (7D hexadecimal) */ 23 | const static uint8_t CONTROL_ESCAPE_OCTET = 0x7D; 24 | 25 | /* If either of these two octets appears in the transmitted data, an escape octet is sent, */ 26 | /* followed by the original data octet with bit 5 inverted */ 27 | const static uint8_t INVERT_OCTET = 0x20; 28 | 29 | /* The frame check sequence (FCS) is a 16-bit CRC-CCITT */ 30 | /* AVR Libc CRC function is _crc_ccitt_update() */ 31 | /* Corresponding CRC function in Qt (www.qt.io) is qChecksum() */ 32 | const static uint16_t CRC16_CCITT_INIT_VAL = 0xFFFF; 33 | 34 | AITLink(size_t max_frame_length = 1024); 35 | virtual ~AITLink(); 36 | 37 | bool sendFrame(const uint8_t* frame_buffer, size_t frame_length); 38 | void inputReceivedChar(uint8_t data); 39 | 40 | /* User must define a function, that will process the valid received frame */ 41 | /* This function can act like a command router/dispatcher */ 42 | void registerFrameHandler(void (*callback)(void* user_data, const uint8_t* frame_buffer, size_t frame_length), void* user_data); 43 | 44 | protected: 45 | virtual void handleFrame(const uint8_t* frame_buffer, size_t frame_length); 46 | /* User must define a function to send a byte throug USART, I2C, SPI etc.*/ 47 | virtual void sendChar(uint8_t data) = 0; 48 | 49 | private: 50 | uint16_t crcCcittUpdate(uint16_t crc, uint8_t data); 51 | 52 | bool escape_character; 53 | size_t frame_position; 54 | // 16bit CRC sum for crcCcittUpdate 55 | uint16_t frame_checksum; 56 | size_t max_frame_length; 57 | uint8_t* receive_frame_buffer; 58 | void (*handle_frame_callback)(void* user_data, const uint8_t* frame_buffer, size_t frame_length); 59 | void* callback_user_data; 60 | }; 61 | 62 | } 63 | -------------------------------------------------------------------------------- /include/ait_link/ait_link_impl.h: -------------------------------------------------------------------------------- 1 | // 2 | // HDLC based serial communication class. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. All rights reserved. 6 | // 7 | 8 | #pragma once 9 | 10 | #include "ait_link.h" 11 | 12 | #ifdef __MBED__ 13 | 14 | #include "ait_link_mbed.h" 15 | 16 | #else 17 | 18 | #include "Serial.h" 19 | 20 | namespace ait { 21 | 22 | class AITLinkImpl : public AITLink { 23 | public: 24 | AITLinkImpl(Serial* serial, uint16_t max_frame_length = 1024) 25 | : AITLink(max_frame_length), serial_(serial) { 26 | } 27 | 28 | virtual ~AITLinkImpl() { 29 | } 30 | 31 | virtual void sendChar(uint8_t data) { 32 | serial_->write(data); 33 | } 34 | 35 | private: 36 | Serial* serial_; 37 | }; 38 | 39 | } 40 | 41 | #endif // __MBED__ 42 | -------------------------------------------------------------------------------- /include/ait_link/ait_link_mbed.h: -------------------------------------------------------------------------------- 1 | // 2 | // HDLC based serial communication class. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. All rights reserved. 6 | // 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "ait_link.h" 13 | 14 | #ifdef __MBED__ 15 | 16 | namespace ait { 17 | 18 | class AITLinkMbed : public AITLink { 19 | BufferedSerial* _serial; 20 | 21 | public: 22 | AITLinkMbed(BufferedSerial* serial, uint16_t max_frame_length = 1024) 23 | : AITLink(max_frame_length), _serial(serial) { 24 | } 25 | 26 | virtual ~AITLinkMbed() { 27 | } 28 | 29 | virtual void frameHandler(const uint8_t* frame_buffer, size_t frame_length) { 30 | } 31 | 32 | virtual void sendChar(uint8_t data) { 33 | _serial->putc(data); 34 | } 35 | }; 36 | 37 | } 38 | 39 | #endif // __MBED__ 40 | -------------------------------------------------------------------------------- /include/ait_link/uwb_link/uwb_link.h: -------------------------------------------------------------------------------- 1 | // 2 | // Simple message protocol for UWB. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. All rights reserved. 6 | // 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | namespace ait { 16 | 17 | class UWBMessageBody { 18 | public: 19 | virtual ~UWBMessageBody() {}; 20 | virtual int getSize() const = 0; 21 | virtual void buildMessage(uint8_t* buffer) const = 0; 22 | virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) = 0; 23 | }; 24 | 25 | class UWBMessageString : public UWBMessageBody { 26 | public: 27 | UWBMessageString() 28 | : str_length_(-1), str_(NULL) { 29 | } 30 | 31 | UWBMessageString(const char* str) 32 | : str_(str) { 33 | str_length_ = strlen(str); 34 | } 35 | 36 | virtual int getSize() const { 37 | return str_length_ + 1; 38 | } 39 | 40 | virtual void buildMessage(uint8_t* buffer) const { 41 | memcpy(buffer, str_, getSize()); 42 | } 43 | 44 | virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) { 45 | #ifdef __MBED__ 46 | // TODO: This can lead to a buffer overrun (something like strnlen should be used) 47 | str_length_ = strlen(reinterpret_cast(buffer)); 48 | #else 49 | str_length_ = strnlen(reinterpret_cast(buffer), buffer_size); 50 | if (str_length_ >= buffer_size) { 51 | return false; 52 | } 53 | #endif 54 | str_ = reinterpret_cast(buffer); 55 | return true; 56 | } 57 | 58 | int getStringLength() const { 59 | return str_length_; 60 | } 61 | 62 | const char* getString() const { 63 | return str_; 64 | } 65 | 66 | private: 67 | int str_length_; 68 | const char* str_; 69 | }; 70 | 71 | struct UWBMessageMultiRange : public UWBMessageBody { 72 | UWBMessageMultiRange() 73 | : address(0), remote_address(0) { 74 | } 75 | 76 | UWBMessageMultiRange(uint8_t address, uint8_t remote_address) 77 | : address(address), remote_address(remote_address) { 78 | } 79 | 80 | void clearMeasurements() { 81 | timestamp_master_request_1.clear(); 82 | timestamp_slave_reply.clear(); 83 | timestamp_master_request_2.clear(); 84 | } 85 | 86 | void addModuleMeasurement(uint64_t timestamp_master_request_1, uint64_t timestamp_slave_reply, uint64_t timestamp_master_request_2) { 87 | this->timestamp_master_request_1.push_back(timestamp_master_request_1); 88 | this->timestamp_slave_reply.push_back(timestamp_slave_reply); 89 | this->timestamp_master_request_2.push_back(timestamp_master_request_2); 90 | } 91 | 92 | void setSlaveMeasurement(uint64_t timestamp_master_request_1_recv, uint64_t timestamp_slave_reply_send, uint64_t timestamp_master_request_2_recv) { 93 | this->timestamp_master_request_1_recv = timestamp_master_request_1_recv; 94 | this->timestamp_slave_reply_send = timestamp_slave_reply_send; 95 | this->timestamp_master_request_2_recv = timestamp_master_request_2_recv; 96 | } 97 | 98 | int getNumOfModules() const { 99 | return timestamp_master_request_1.size(); 100 | } 101 | 102 | virtual int getSize() const { 103 | return sizeof(uint8_t) + sizeof(address) + sizeof(remote_address) + (getNumOfModules() + 1) * 3 * sizeof(uint64_t); 104 | } 105 | 106 | virtual void buildMessage(uint8_t* buffer) const { 107 | // Number of modules and addresses 108 | uint8_t num_of_modules = static_cast(getNumOfModules()); 109 | *buffer = num_of_modules; 110 | ++buffer; 111 | *buffer = address; 112 | ++buffer; 113 | *buffer = remote_address; 114 | ++buffer; 115 | // Slave timestamps 116 | uint64_t* buffer_64 = reinterpret_cast(buffer); 117 | *buffer_64 = timestamp_master_request_1_recv; 118 | ++buffer_64; 119 | *buffer_64 = timestamp_slave_reply_send; 120 | ++buffer_64; 121 | *buffer_64 = timestamp_master_request_2_recv; 122 | ++buffer_64; 123 | // Master and listener timestamps 124 | for (int i = 0; i < num_of_modules; ++i) { 125 | *buffer_64 = timestamp_master_request_1[i]; 126 | ++buffer_64; 127 | *buffer_64 = timestamp_slave_reply[i]; 128 | ++buffer_64; 129 | *buffer_64 = timestamp_master_request_2[i]; 130 | ++buffer_64; 131 | } 132 | } 133 | 134 | virtual bool decodeMessage(const uint8_t* buffer, size_t buffer_size) { 135 | if (buffer_size < sizeof(uint8_t) + sizeof(address) + sizeof(remote_address)) { 136 | return false; 137 | } 138 | 139 | clearMeasurements(); 140 | 141 | // Number of modules and addresses 142 | int num_of_modules = *buffer; 143 | ++buffer; 144 | address = *buffer; 145 | ++buffer; 146 | remote_address = *buffer; 147 | ++buffer; 148 | if (buffer_size < getSize()) { 149 | return false; 150 | } 151 | // Slave timestamps 152 | const uint64_t* buffer_64 = reinterpret_cast(buffer); 153 | timestamp_master_request_1_recv = *buffer_64; 154 | ++buffer_64; 155 | timestamp_slave_reply_send = *buffer_64; 156 | ++buffer_64; 157 | timestamp_master_request_2_recv = *buffer_64; 158 | ++buffer_64; 159 | // Master and listener timestamps 160 | for (int i = 0; i < num_of_modules; ++i) { 161 | timestamp_master_request_1.push_back(*buffer_64); 162 | ++buffer_64; 163 | timestamp_slave_reply.push_back(*buffer_64); 164 | ++buffer_64; 165 | timestamp_master_request_2.push_back(*buffer_64); 166 | ++buffer_64; 167 | } 168 | return true; 169 | } 170 | 171 | uint8_t address; 172 | uint8_t remote_address; 173 | 174 | uint64_t timestamp_master_request_1_recv; 175 | uint64_t timestamp_slave_reply_send; 176 | uint64_t timestamp_master_request_2_recv; 177 | std::vector timestamp_master_request_1; 178 | std::vector timestamp_slave_reply; 179 | std::vector timestamp_master_request_2; 180 | }; 181 | 182 | class UWBMessage { 183 | public: 184 | const static uint8_t UWB_MESSAGE_TYPE_NOP = 0x00; 185 | const static uint8_t UWB_MESSAGE_TYPE_STATUS = 0x01; 186 | const static uint8_t UWB_MESSAGE_TYPE_MULTI_RANGE = 0x02; 187 | 188 | UWBMessage() 189 | : type_(UWB_MESSAGE_TYPE_NOP), body_(NULL), part_allocated_(false) { 190 | } 191 | 192 | UWBMessage(uint8_t type) 193 | : type_(type), body_(NULL), part_allocated_(false) { 194 | } 195 | 196 | UWBMessage(uint8_t type, const UWBMessageBody* body) 197 | : type_(type), body_(body), part_allocated_(false) { 198 | } 199 | 200 | ~UWBMessage() { 201 | clearMessageBody(); 202 | } 203 | 204 | uint8_t getType() const { 205 | return type_; 206 | } 207 | 208 | const UWBMessageBody* getMessageBody() const { 209 | return body_; 210 | } 211 | 212 | void setMessageBody(const UWBMessageBody* body) { 213 | clearMessageBody(); 214 | body_ = body; 215 | } 216 | 217 | int getSize() const { 218 | int size = sizeof(type_); 219 | if (body_ != NULL) { 220 | size += body_->getSize(); 221 | } 222 | return size; 223 | } 224 | 225 | void buildMessage(uint8_t* buffer) const { 226 | buffer[0] = type_; 227 | if (body_ != NULL) { 228 | buffer += sizeof(type_); 229 | body_->buildMessage(buffer); 230 | } 231 | } 232 | 233 | bool decodeMessage(const uint8_t* buffer, size_t buffer_size) { 234 | clearMessageBody(); 235 | part_allocated_ = true; 236 | if (buffer_size < sizeof(type_)) { 237 | return false; 238 | } 239 | type_ = buffer[0]; 240 | buffer += sizeof(type_); 241 | buffer_size -= sizeof(type_); 242 | switch (type_) { 243 | case UWB_MESSAGE_TYPE_NOP: { 244 | break; 245 | } 246 | case UWB_MESSAGE_TYPE_STATUS: { 247 | UWBMessageString *msg_string = new UWBMessageString(); 248 | if (msg_string->decodeMessage(buffer, buffer_size)) { 249 | body_ = msg_string; 250 | } else { 251 | delete msg_string; 252 | return false; 253 | } 254 | break; 255 | } 256 | case UWB_MESSAGE_TYPE_MULTI_RANGE: { 257 | UWBMessageMultiRange *msg_multi_range = new UWBMessageMultiRange(); 258 | if (msg_multi_range->decodeMessage(buffer, buffer_size)) { 259 | body_ = msg_multi_range; 260 | } else { 261 | delete msg_multi_range; 262 | return false; 263 | } 264 | break; 265 | } 266 | default: 267 | return false; 268 | } 269 | return true; 270 | } 271 | 272 | private: 273 | void clearMessageBody() { 274 | if (part_allocated_) { 275 | delete body_; 276 | part_allocated_ = false; 277 | } 278 | body_ = NULL; 279 | } 280 | 281 | uint8_t type_; 282 | const UWBMessageBody* body_; 283 | bool part_allocated_; 284 | }; 285 | 286 | class UWBLink { 287 | public: 288 | UWBLink(AITLink* ait_link, int buffer_size = 1024) 289 | : handle_message_callback_(NULL), callback_user_data_(NULL), 290 | ait_link_(ait_link), buffer_size_(buffer_size) { 291 | buffer_ = new uint8_t[buffer_size]; 292 | ait_link_->registerFrameHandler(&UWBLink::handleFrameWrapper, this); 293 | } 294 | 295 | virtual ~UWBLink() { 296 | delete[] buffer_; 297 | } 298 | 299 | bool sendMessage(const UWBMessage& msg) { 300 | int size = msg.getSize(); 301 | if (size > buffer_size_) { 302 | return false; 303 | } 304 | msg.buildMessage(buffer_); 305 | ait_link_->sendFrame(buffer_, size); 306 | return true; 307 | } 308 | 309 | void registerMessageHandler(void (*callback)(void* user_data, const UWBMessage& msg), void* user_data) { 310 | handle_message_callback_ = callback; 311 | callback_user_data_ = user_data; 312 | } 313 | 314 | void inputReceivedChar(uint8_t data) { 315 | ait_link_->inputReceivedChar(data); 316 | } 317 | 318 | protected: 319 | virtual void handleMessage(const UWBMessage& msg) { 320 | if (handle_message_callback_ != NULL) { 321 | (*handle_message_callback_)(callback_user_data_, msg); 322 | } 323 | } 324 | 325 | void (*handle_message_callback_)(void* user_data, const UWBMessage& msg); 326 | void* callback_user_data_; 327 | 328 | private: 329 | void handleFrame(const uint8_t* frame_buffer, size_t frame_length) { 330 | UWBMessage msg; 331 | if (msg.decodeMessage(frame_buffer, frame_length)) { 332 | handleMessage(msg); 333 | } else { 334 | fprintf(stderr, "Failed to decode UWB message"); 335 | } 336 | } 337 | 338 | static void handleFrameWrapper(void* user_data, const uint8_t* frame_buffer, size_t frame_length) { 339 | UWBLink* uwb_link = reinterpret_cast(user_data); 340 | uwb_link->handleFrame(frame_buffer, frame_length); 341 | } 342 | 343 | AITLink* ait_link_; 344 | uint8_t* buffer_; 345 | int buffer_size_; 346 | }; 347 | 348 | } 349 | -------------------------------------------------------------------------------- /include/ait_link/uwb_link/uwb_link_impl.h: -------------------------------------------------------------------------------- 1 | // 2 | // Simple message protocol for UWB. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. All rights reserved. 6 | // 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "uwb_link.h" 12 | 13 | #include 14 | 15 | #ifdef __MBED__ 16 | 17 | #include "uwb_link_mbed.h" 18 | 19 | #else 20 | 21 | namespace ait { 22 | 23 | // We need this extra base-class to ensure initialization of AITLinkImpl when initializing UWBLink in UWBLinkImpl 24 | class UWBLinkImplBase { 25 | public: 26 | UWBLinkImplBase() 27 | : _ait_link(&serial_) { 28 | } 29 | protected: 30 | Serial serial_; 31 | AITLinkImpl _ait_link; 32 | }; 33 | 34 | class UWBLinkImpl : public UWBLinkImplBase, public UWBLink { 35 | public: 36 | UWBLinkImpl(int buffer_size = 1024) 37 | : UWBLink(&_ait_link, buffer_size) { 38 | serial_.setReadCallback(boost::bind(std::mem_fun(&UWBLink::inputReceivedChar), this, _1)); 39 | } 40 | 41 | UWBLinkImpl(const std::string& device, unsigned int baud_rate, int buffer_size = 1024) 42 | : UWBLink(&_ait_link, buffer_size) { 43 | serial_.setReadCallback(boost::bind(std::mem_fun(&UWBLink::inputReceivedChar), this, _1)); 44 | start(device, baud_rate); 45 | } 46 | 47 | void start(const std::string& device, unsigned int baud_rate) { 48 | serial_.open(device, baud_rate); 49 | serial_.start(); 50 | } 51 | }; 52 | 53 | } 54 | 55 | #endif // __MBED__ 56 | -------------------------------------------------------------------------------- /include/ait_link/uwb_link/uwb_link_mbed.h: -------------------------------------------------------------------------------- 1 | // 2 | // Simple message protocol for UWB. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. All rights reserved. 6 | // 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "uwb_link.h" 12 | 13 | #ifdef __MBED__ 14 | namespace ait { 15 | 16 | class UWBLinkMbed : public UWBLink { 17 | AITLinkMbed ait_link_; 18 | 19 | public: 20 | UWBLinkMbed(BufferedSerial* serial, int buffer_size = 1024) 21 | : UWBLink(&ait_link_, buffer_size), ait_link_(serial) { 22 | } 23 | }; 24 | 25 | } 26 | 27 | #endif // __MBED__ 28 | -------------------------------------------------------------------------------- /launch/uwb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /launch/uwb_multi_range.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/uwb_serial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/uwb_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /msg/UWBMultiRange.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 num_of_units 3 | uint8 address 4 | uint8 remote_address 5 | float32[] tofs 6 | float32[] ranges 7 | -------------------------------------------------------------------------------- /msg/UWBMultiRangeTimestamps.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 num_of_units 3 | uint8 address 4 | uint8 remote_address 5 | int64 timestamp_master_request_1_recv 6 | int64 timestamp_slave_reply_send 7 | int64 timestamp_master_request_2_recv 8 | int64[] timestamp_master_request_1 9 | int64[] timestamp_slave_reply 10 | int64[] timestamp_master_request_2 11 | -------------------------------------------------------------------------------- /msg/UWBMultiRangeWithOffsets.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 num_of_units 3 | uint8 address 4 | uint8 remote_address 5 | UWBRangeWithOffset[] ranges 6 | -------------------------------------------------------------------------------- /msg/UWBRangeWithOffset.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point32 offset 2 | float32 tof 3 | float32 range 4 | -------------------------------------------------------------------------------- /msg/UWBTracker.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 address 3 | uint8 remote_address 4 | # The state is [p_x, p_y, p_z, v_x, v_y, v_z], where p_i and v_i are the position and velocity in dimension i 5 | float32[] state 6 | # A 6x6 matrix (i.e. 36 elements) giving the covariance of the state (storage order is row-major) 7 | float32[] covariance 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uwb 4 | 0.0.1 5 | UWB localization 6 | 7 | Benjamin Hepp 8 | 9 | 10 | 11 | 12 | 13 | LGPLv3 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Tobias Naegeli 26 | Benjamin Hepp 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | catkin 41 | std_msgs 42 | message_generation 43 | rospy 44 | python-numpy 45 | 46 | catkin 47 | cmake_modules 48 | geometry_msgs 49 | message_generation 50 | roscpp 51 | sensor_msgs 52 | std_msgs 53 | tf 54 | rospy 55 | 56 | 57 | std_msgs 58 | message_runtime 59 | rospy 60 | python-numpy 61 | 62 | 63 | cmake_modules 64 | geometry_msgs 65 | message_runtime 66 | roscpp 67 | sensor_msgs 68 | std_msgs 69 | tf 70 | rospy 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /src/UWBSerial.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Serial reader for multiple UWB ranging units. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. 6 | // 7 | 8 | #include "UWBSerial.h" 9 | 10 | using ait::UWBSerial; 11 | 12 | UWBSerial::UWBSerial() 13 | : nh_("~"), msg_counter_(0), last_info_time_(ros::Time::now()) 14 | { 15 | uwb_link_.registerMessageHandler(UWBSerial::handleMessageWrapper, this); 16 | 17 | double info_print_rate = 2.0; 18 | nh_.getParam("info_print_rate", info_print_rate); 19 | print_info_period_ = ros::Duration(info_print_rate); 20 | info_timer_ = nh_.createTimer(print_info_period_, &UWBSerial::printInfoCallback, this); 21 | info_timer_.start(); 22 | 23 | std::string topic("/uwb/timestamps"); 24 | nh_.getParam("topic", topic); 25 | ROS_INFO_STREAM("Publishing to " << topic); 26 | timestamps_pub_ = nh_.advertise("/uwb/timestamps", 1); 27 | 28 | std::string device("/dev/ttyACM0"); 29 | int baud_rate = 115200; 30 | nh_.getParam("serial_port", device); 31 | nh_.getParam("baud_rate", baud_rate); 32 | ROS_INFO_STREAM("Reading from serial port " << device << " with baud-rate " << baud_rate); 33 | uwb_link_.start(device, baud_rate); 34 | } 35 | 36 | UWBSerial::~UWBSerial() { 37 | } 38 | 39 | void UWBSerial::handleMessage(const UWBMessage& msg) { 40 | ROS_DEBUG("Received UWB message type: 0x%x\n", msg.getType()); 41 | switch (msg.getType()) { 42 | case UWBMessage::UWB_MESSAGE_TYPE_NOP: 43 | break; 44 | case UWBMessage::UWB_MESSAGE_TYPE_STATUS: { 45 | const UWBMessageString *msg_str = dynamic_cast(msg.getMessageBody()); 46 | ROS_WARN_STREAM("STATUS: " << msg_str->getString()); 47 | break; 48 | } 49 | case UWBMessage::UWB_MESSAGE_TYPE_MULTI_RANGE: { 50 | const UWBMessageMultiRange* msg_multi_range = dynamic_cast(msg.getMessageBody()); 51 | uwb::UWBMultiRangeTimestamps timestamps_msg; 52 | timestamps_msg.header.stamp = ros::Time::now(); 53 | timestamps_msg.num_of_units = msg_multi_range->getNumOfModules(); 54 | timestamps_msg.address = msg_multi_range->address; 55 | timestamps_msg.remote_address = msg_multi_range->remote_address; 56 | timestamps_msg.timestamp_master_request_1_recv = msg_multi_range->timestamp_master_request_1_recv; 57 | timestamps_msg.timestamp_slave_reply_send = msg_multi_range->timestamp_slave_reply_send; 58 | timestamps_msg.timestamp_master_request_2_recv = msg_multi_range->timestamp_master_request_2_recv; 59 | for (int i = 0; i < msg_multi_range->getNumOfModules(); ++i) { 60 | timestamps_msg.timestamp_master_request_1.push_back(msg_multi_range->timestamp_master_request_1[i]); 61 | timestamps_msg.timestamp_slave_reply.push_back(msg_multi_range->timestamp_slave_reply[i]); 62 | timestamps_msg.timestamp_master_request_2.push_back(msg_multi_range->timestamp_master_request_2[i]); 63 | } 64 | timestamps_pub_.publish(timestamps_msg); 65 | ++msg_counter_; 66 | break; 67 | } 68 | } 69 | } 70 | 71 | void UWBSerial::printInfoCallback(const ros::TimerEvent& ev) { 72 | ros::Time now_time = ros::Time::now(); 73 | ros::Duration elapsed_duration = now_time - last_info_time_; 74 | if (elapsed_duration >= print_info_period_) { 75 | double msg_rate = msg_counter_ / elapsed_duration.toSec(); 76 | ROS_INFO_STREAM("Receiving UWBLink multi-range messages with rate " << msg_rate << " Hz"); 77 | msg_counter_ = 0; 78 | last_info_time_ = now_time; 79 | } 80 | } 81 | 82 | void UWBSerial::handleMessageWrapper(void* user_data, const UWBMessage& msg) { 83 | UWBSerial* uwb_serial = reinterpret_cast(user_data); 84 | uwb_serial->handleMessage(msg); 85 | } 86 | -------------------------------------------------------------------------------- /src/ait_link/Serial.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Simple Serial Port class. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. 6 | // 7 | 8 | #include "ait_link/Serial.h" 9 | 10 | using ait::Serial; 11 | 12 | Serial::Serial(const std::string &device, unsigned int baud_rate, size_t read_buffer_size) 13 | : io_(), serial_port_(io_), read_buffer_(new uint8_t[read_buffer_size]) { 14 | open(device, baud_rate); 15 | } 16 | 17 | Serial::Serial(size_t read_buffer_size) 18 | : io_(), serial_port_(io_), read_buffer_(new uint8_t[read_buffer_size]){ 19 | } 20 | 21 | Serial::~Serial() { 22 | stop(); 23 | close(); 24 | delete[] read_buffer_; 25 | } 26 | 27 | void Serial::setReadCallback(const boost::function &read_callback) { 28 | read_callback_ = read_callback; 29 | } 30 | 31 | void Serial::open(const std::string &device, unsigned int baud_rate) { 32 | boost::system::error_code ec; 33 | serial_port_.open(device, ec); 34 | if (ec) { 35 | throw std::runtime_error("Failed to open serial port: " + ec.message()); 36 | } 37 | serial_port_.set_option(boost::asio::serial_port_base::baud_rate(baud_rate)); 38 | serial_port_.set_option(boost::asio::serial_port_base::character_size(8)); 39 | serial_port_.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); 40 | serial_port_.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); 41 | serial_port_.set_option( 42 | boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none)); 43 | } 44 | 45 | void Serial::close() { 46 | if (serial_port_.is_open()) { 47 | serial_port_.cancel(); 48 | serial_port_.close(); 49 | } 50 | } 51 | 52 | void Serial::start() { 53 | // Give work to thread before starting it! 54 | asyncReadSome(); 55 | boost::thread t(boost::bind(&boost::asio::io_service::run, &io_)); 56 | async_thread_.swap(t); 57 | } 58 | 59 | void Serial::stop() { 60 | io_.stop(); 61 | async_thread_.join(); 62 | io_.reset(); 63 | } 64 | 65 | int Serial::write(char c) { 66 | return write(reinterpret_cast(&c), 1); 67 | } 68 | 69 | int Serial::write(const std::string &buf) { 70 | return write(reinterpret_cast(buf.c_str()), buf.size()); 71 | } 72 | 73 | int Serial::write(const uint8_t *buf, int size) { 74 | if (!serial_port_.is_open()) { 75 | throw std::runtime_error("Serial port has not been opened"); 76 | } 77 | 78 | boost::system::error_code ec; 79 | int written = serial_port_.write_some(boost::asio::buffer(buf, size), ec); 80 | if (ec) { 81 | throw std::runtime_error("Failed to write to serial port: " + ec.message()); 82 | } 83 | return written; 84 | } 85 | 86 | void Serial::asyncReadSome() { 87 | if (!serial_port_.is_open()) { 88 | return; 89 | } 90 | 91 | serial_port_.async_read_some( 92 | boost::asio::buffer(read_buffer_, sizeof(read_buffer_)), 93 | boost::bind( 94 | &Serial::onRead, 95 | this, 96 | boost::asio::placeholders::error, 97 | boost::asio::placeholders::bytes_transferred) 98 | ); 99 | } 100 | 101 | void Serial::onRead(const boost::system::error_code &ec, size_t bytes_transferred) { 102 | boost::mutex::scoped_lock look(mutex_); 103 | 104 | if (!ec) { 105 | for (int i = 0; i < bytes_transferred; ++i) { 106 | uint8_t c = read_buffer_[i]; 107 | if (!read_callback_.empty()) { 108 | read_callback_(c); 109 | } 110 | } 111 | } 112 | 113 | asyncReadSome(); 114 | } 115 | -------------------------------------------------------------------------------- /src/ait_link/ait_link.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // HDLC based serial communication class. 3 | // 4 | // Adapted by Benjamin Hepp from ArduHDLC library (see LICENSE.txt) 5 | // Original work Copyright (c) 2015 Jarkko Hautakorpi et al. 6 | // Modified work Copyright (c) 2016 Benjamin Hepp. 7 | 8 | #include "ait_link/ait_link.h" 9 | 10 | using namespace ait; 11 | 12 | /* 16bit low and high bytes copier */ 13 | #define low(x) ((x) & 0xFF) 14 | #define high(x) (((x)>>8) & 0xFF) 15 | 16 | AITLink::AITLink(size_t max_frame_length) 17 | { 18 | frame_checksum = CRC16_CCITT_INIT_VAL; 19 | escape_character = false; 20 | frame_position = 0; 21 | this->max_frame_length = max_frame_length; 22 | receive_frame_buffer = new uint8_t[max_frame_length+1]; // char* ab = (char*)malloc(12); 23 | handle_frame_callback = NULL; 24 | callback_user_data = NULL; 25 | } 26 | 27 | AITLink::~AITLink() 28 | { 29 | delete[] receive_frame_buffer; 30 | } 31 | 32 | /* Wrap given data in HDLC frame and send it out byte at a time*/ 33 | bool AITLink::sendFrame(const uint8_t* frame_buffer, size_t frame_length) 34 | { 35 | if (frame_length > max_frame_length) { 36 | return false; 37 | } 38 | 39 | uint8_t data; 40 | uint16_t fcs = CRC16_CCITT_INIT_VAL; 41 | 42 | sendChar(FRAME_BOUNDARY_OCTET); 43 | 44 | while (frame_length) 45 | { 46 | data = *frame_buffer++; 47 | fcs = crcCcittUpdate(fcs, data); 48 | if ((data == CONTROL_ESCAPE_OCTET) || (data == FRAME_BOUNDARY_OCTET)) 49 | { 50 | sendChar(CONTROL_ESCAPE_OCTET); 51 | data ^= INVERT_OCTET; 52 | } 53 | sendChar(data); 54 | --frame_length; 55 | } 56 | data = low(fcs); 57 | if ((data == CONTROL_ESCAPE_OCTET) || (data == FRAME_BOUNDARY_OCTET)) 58 | { 59 | sendChar(CONTROL_ESCAPE_OCTET); 60 | data ^= INVERT_OCTET; 61 | } 62 | sendChar(data); 63 | data = high(fcs); 64 | if ((data == CONTROL_ESCAPE_OCTET) || (data == FRAME_BOUNDARY_OCTET)) 65 | { 66 | sendChar(CONTROL_ESCAPE_OCTET); 67 | data ^= INVERT_OCTET; 68 | } 69 | sendChar(data); 70 | sendChar(FRAME_BOUNDARY_OCTET); 71 | 72 | return true; 73 | } 74 | 75 | /* Function to find valid HDLC frame from incoming data */ 76 | void AITLink::inputReceivedChar(uint8_t data) 77 | { 78 | /* FRAME FLAG */ 79 | if (data == FRAME_BOUNDARY_OCTET) 80 | { 81 | if (this->escape_character == true) 82 | { 83 | this->escape_character = false; 84 | } 85 | /* If a valid frame is detected */ 86 | else if ( (this->frame_position >= 2) && 87 | ( this->frame_checksum == ((this->receive_frame_buffer[this->frame_position-1] << 8 ) | (this->receive_frame_buffer[this->frame_position-2] & 0xff)) ) ) // (msb << 8 ) | (lsb & 0xff) 88 | { 89 | /* Call the user defined function and pass frame to it */ 90 | handleFrame(receive_frame_buffer, this->frame_position - 2); 91 | } 92 | this->frame_position = 0; 93 | this->frame_checksum = CRC16_CCITT_INIT_VAL; 94 | return; 95 | } 96 | 97 | if (this->escape_character) 98 | { 99 | this->escape_character = false; 100 | data ^= INVERT_OCTET; 101 | } 102 | else if (data == CONTROL_ESCAPE_OCTET) 103 | { 104 | this->escape_character = true; 105 | return; 106 | } 107 | 108 | receive_frame_buffer[this->frame_position] = data; 109 | 110 | if (this->frame_position >= 2) { 111 | this->frame_checksum = crcCcittUpdate(this->frame_checksum, receive_frame_buffer[this->frame_position - 2]); 112 | } 113 | 114 | this->frame_position++; 115 | 116 | if (this->frame_position >= this->max_frame_length) 117 | { 118 | this->frame_position = 0; 119 | this->frame_checksum = CRC16_CCITT_INIT_VAL; 120 | } 121 | } 122 | 123 | void AITLink::registerFrameHandler(void (*callback)(void* user_data, const uint8_t* frame_buffer, size_t frame_length), void* user_data) 124 | { 125 | handle_frame_callback = callback; 126 | callback_user_data = user_data; 127 | } 128 | 129 | void AITLink::handleFrame(const uint8_t* frame_buffer, size_t frame_length) 130 | { 131 | if (handle_frame_callback != NULL) { 132 | (*handle_frame_callback)(callback_user_data, frame_buffer, frame_length); 133 | } 134 | 135 | } 136 | 137 | uint16_t AITLink::crcCcittUpdate(uint16_t crc, uint8_t data) 138 | { 139 | data ^= low(crc); 140 | data ^= data << 4; 141 | 142 | return ( 143 | ( ((uint16_t)data << 8) | high(crc) ) ^ ((uint8_t)data >> 4) ^ ((uint16_t)data << 3) 144 | ); 145 | } 146 | -------------------------------------------------------------------------------- /src/gui_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pyqtgraph 3 | 4 | class MainWindow(pyqtgraph.GraphicsWindow): 5 | def __init__(self, *args, **kwargs): 6 | super(MainWindow, self).__init__(*args, **kwargs) 7 | 8 | def showEvent(self, event): 9 | super(MainWindow, self).showEvent(event) 10 | 11 | def closeEvent(self, event): 12 | super(MainWindow, self).closeEvent(event) 13 | 14 | 15 | class PlotData(object): 16 | 17 | def __init__(self, plot, max_data_length=None): 18 | self.plot = plot 19 | self.curves = [] 20 | self.data = [] 21 | self.max_data_length = max_data_length 22 | 23 | def add_curve(self, pen, initial_data=None, **kwargs): 24 | self.curves.append(self.plot.plot(pen=pen, **kwargs)) 25 | if initial_data is None: 26 | if self.max_data_length is None: 27 | initial_data = [] 28 | else: 29 | initial_data = np.zeros((self.max_data_length,)) 30 | self.data.append(initial_data) 31 | 32 | def add_point(self, index, value): 33 | assert(index < len(self.curves)) 34 | if self.max_data_length is None: 35 | self.data[index].append(value) 36 | else: 37 | self.data[index][:-1] = self.data[index][1:] 38 | self.data[index][-1] = value 39 | if len(self.data[index]) > self.max_data_length: 40 | self.data[index] = self.data[index][-self.max_data_length:len(self.data[index])] 41 | self.curves[index].setData(self.data[index]) 42 | 43 | def get_plot(self): 44 | return self.plot 45 | 46 | def __len__(self): 47 | return len(self.curves) 48 | -------------------------------------------------------------------------------- /src/nodes/uwb_serial_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Node for reading UWB multi-ranges from serial port. 3 | // 4 | // Created by Benjamin Hepp on 02.04.16. 5 | // Copyright (c) 2016 Benjamin Hepp. 6 | // 7 | 8 | 9 | #include 10 | 11 | #include "UWBSerial.h" 12 | 13 | using ait::UWBSerial; 14 | 15 | int main(int argc, char **argv) 16 | { 17 | ros::init(argc, argv, "uwb_serial"); 18 | // The node-handle is required for ros-logging support 19 | ros::NodeHandle nh_("~"); 20 | 21 | try { 22 | UWBSerial uwb_serial; 23 | ros::spin(); 24 | } catch (const std::exception& exc) { 25 | ROS_ERROR_STREAM("Error occured: " << exc.what()); 26 | } 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /src/uwb_multi_range_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """uwb_multi_range_node.py: Streams UWB multi-range measurements based on UWB timestamps.""" 4 | 5 | from __future__ import print_function 6 | 7 | __author__ = "Benjamin Hepp" 8 | __email__ = "benjamin.hepp@inf.ethz.ch" 9 | __copyright__ = "Copyright 2016 Benjamin Hepp" 10 | 11 | import select 12 | import numpy as np 13 | import roslib 14 | roslib.load_manifest('uwb') 15 | import rospy 16 | 17 | import uwb.msg 18 | 19 | 20 | class UWBMultiRange(object): 21 | 22 | INFO_PRINT_RATE = 2 23 | 24 | US_TO_DW_TIMEUNITS = 128. * 499.2 # conversion between microseconds to the decawave timeunits (ca 15.65ps). 25 | DW_TIMEUNITS_TO_US = 1 / US_TO_DW_TIMEUNITS # conversion between the decawave timeunits (~15.65ps) to microseconds. 26 | CONST_2POWER40 = 1099511627776 # Time register in DW1000 is 40 bit so this is needed to detect overflows. 27 | SPEED_OF_LIGHT_IN_M_PER_US = 299.792458 28 | SPEED_OF_LIGHT_IN_M_PER_DW_TIMEUNIT = SPEED_OF_LIGHT_IN_M_PER_US / US_TO_DW_TIMEUNITS 29 | 30 | VISUALIZATION_DATA_LENGTH = 500 31 | 32 | def __init__(self): 33 | self._read_configuration() 34 | 35 | if self.show_plots: 36 | self._setup_plots() 37 | 38 | rospy.loginfo("Receiving timestamp messages from {}".format(self.uwb_timestamps_topic)) 39 | rospy.loginfo("Publishing multi-range messages to {}".format(self.uwb_multi_range_topic)) 40 | rospy.loginfo("Publishing raw multi-range messages to {}".format(self.uwb_multi_range_raw_topic)) 41 | rospy.loginfo("Publishing multi-range-with-offsets messages to {}".format( 42 | self.uwb_multi_range_with_offsets_topic)) 43 | 44 | # ROS Publishers 45 | self.uwb_pub = rospy.Publisher(self.uwb_multi_range_topic, uwb.msg.UWBMultiRange, queue_size=1) 46 | self.uwb_raw_pub = rospy.Publisher(self.uwb_multi_range_raw_topic, uwb.msg.UWBMultiRange, queue_size=1) 47 | self.uwb_with_offsets_pub = rospy.Publisher(self.uwb_multi_range_with_offsets_topic, 48 | uwb.msg.UWBMultiRangeWithOffsets, queue_size=1) 49 | self.uwb_timestamps_sub = rospy.Subscriber(self.uwb_timestamps_topic, uwb.msg.UWBMultiRangeTimestamps, 50 | self.handle_timestamps_message) 51 | 52 | # Variables for rate display 53 | self.msg_count = 0 54 | self.last_now = rospy.get_time() 55 | 56 | def _read_configuration(self): 57 | self._read_unit_offsets() 58 | self.show_plots = rospy.get_param('~show_plots', True) 59 | self.show_slave_clock_offset = rospy.get_param('~show_slave_clock_offset', False) 60 | self.show_slave_clock_skew = rospy.get_param('~show_slave_clock_skew', False) 61 | self.uwb_timestamps_topic = rospy.get_param('~timestamps_topic', '/uwb/timestamps') 62 | self.uwb_multi_range_topic = rospy.get_param('~multi_range_topic', '/uwb/multi_range') 63 | self.uwb_multi_range_raw_topic = rospy.get_param('~multi_range_raw_topic', '/uwb/multi_range_raw') 64 | self.uwb_multi_range_with_offsets_topic = rospy.get_param('~multi_range_with_offsets_topic', 65 | '/uwb/multi_range_with_offsets') 66 | 67 | def _read_unit_offsets(self): 68 | if not rospy.has_param('~num_of_units'): 69 | rospy.logwarn("No unit offset parameters found!") 70 | num_of_units = rospy.get_param('~num_of_units', 0) 71 | self._unit_offsets = np.zeros((num_of_units, 3)) 72 | self._unit_coefficients = np.zeros((num_of_units, 2)) 73 | for i in xrange(num_of_units): 74 | unit_params = rospy.get_param('~unit_{}'.format(i)) 75 | x = unit_params['x'] 76 | y = unit_params['y'] 77 | z = unit_params['z'] 78 | self._unit_offsets[i, :] = [x, y, z] 79 | p0 = unit_params['p0'] 80 | p1 = unit_params['p1'] 81 | self._unit_coefficients[i, :] = [p0, p1] 82 | rospy.loginfo("Unit offsets: {}".format(self._unit_offsets)) 83 | rospy.loginfo("Unit coefficients: {}".format(self._unit_coefficients)) 84 | 85 | def _setup_plots(self): 86 | from gui_utils import MainWindow, PlotData 87 | self.window = MainWindow() 88 | self.range_plot = PlotData(self.window.addPlot(title="Ranges"), self.VISUALIZATION_DATA_LENGTH) 89 | self.range_plot.get_plot().addLegend() 90 | self.window.nextRow() 91 | self.clock_offset_plot = PlotData(self.window.addPlot(title="Clock offset"), self.VISUALIZATION_DATA_LENGTH) 92 | self.clock_offset_plot.get_plot().addLegend() 93 | self.window.nextRow() 94 | self.clock_skew_plot = PlotData(self.window.addPlot(title="Clock skew"), self.VISUALIZATION_DATA_LENGTH) 95 | self.clock_skew_plot.get_plot().addLegend() 96 | 97 | def handle_timestamps_message(self, multi_range_raw_msg): 98 | # Compute time-of-flight and ranges from timestamps measurements 99 | tofs, ranges, clock_offsets, clock_skews, slave_clock_offset, slave_clock_skew \ 100 | = self.process_timestamps_measurements(multi_range_raw_msg, self._unit_coefficients) 101 | 102 | # Publish multi-range message 103 | if self.uwb_pub.get_num_connections() > 0: 104 | ros_msg = uwb.msg.UWBMultiRange() 105 | ros_msg.header.stamp = rospy.Time.now() 106 | ros_msg.num_of_units = multi_range_raw_msg.num_of_units 107 | ros_msg.address = multi_range_raw_msg.address 108 | ros_msg.remote_address = multi_range_raw_msg.remote_address 109 | ros_msg.tofs = tofs 110 | ros_msg.ranges = ranges 111 | self.uwb_pub.publish(ros_msg) 112 | 113 | if self.uwb_with_offsets_pub.get_num_connections() > 0: 114 | ros_msg = uwb.msg.UWBMultiRangeWithOffsets() 115 | ros_msg.header.stamp = rospy.Time.now() 116 | ros_msg.num_of_units = multi_range_raw_msg.num_of_units 117 | ros_msg.address = multi_range_raw_msg.address 118 | ros_msg.remote_address = multi_range_raw_msg.remote_address 119 | if ros_msg.num_of_units != self._unit_offsets.shape[0]: 120 | rospy.logwarn("Number of units in timestamp message does not match number of unit offsets in parameters!") 121 | for i in xrange(ros_msg.num_of_units): 122 | range_msg = uwb.msg.UWBRangeWithOffset() 123 | range_msg.tof = tofs[i] 124 | range_msg.range = ranges[i] 125 | range_msg.offset.x = self._unit_offsets[i, 0] 126 | range_msg.offset.y = self._unit_offsets[i, 1] 127 | range_msg.offset.z = self._unit_offsets[i, 2] 128 | ros_msg.ranges.append(range_msg) 129 | self.uwb_with_offsets_pub.publish(ros_msg) 130 | 131 | if self.uwb_raw_pub.get_num_connections() > 0: 132 | # Compute raw (without rigid configuration model) time-of-flight and ranges from timestamps measurements 133 | raw_tofs, raw_ranges, _, _, _, _ = self.process_timestamps_measurements(multi_range_raw_msg) 134 | 135 | ros_msg = uwb.msg.UWBMultiRange() 136 | ros_msg.header.stamp = rospy.Time.now() 137 | ros_msg.num_of_units = multi_range_raw_msg.num_of_units 138 | ros_msg.address = multi_range_raw_msg.address 139 | ros_msg.remote_address = multi_range_raw_msg.remote_address 140 | ros_msg.tofs = raw_tofs 141 | ros_msg.ranges = raw_ranges 142 | self.uwb_raw_pub.publish(ros_msg) 143 | 144 | # Optionally: Update plots 145 | if self.show_plots: 146 | self.update_visualization(tofs, ranges, clock_offsets, clock_skews, 147 | slave_clock_offset, slave_clock_skew) 148 | 149 | # Increase rate-counter 150 | self.msg_count += 1 151 | 152 | # Display rate 153 | now = rospy.get_time() 154 | if now - self.last_now >= self.INFO_PRINT_RATE: 155 | msg_rate = self.msg_count / (now - self.last_now) 156 | rospy.loginfo("Receiving UWB timestamps with rate {} Hz".format(msg_rate)) 157 | self.last_now = now 158 | self.msg_count = 0 159 | 160 | def convert_dw_timeunits_to_microseconds(self, dw_timeunits): 161 | return dw_timeunits * self.DW_TIMEUNITS_TO_US 162 | 163 | def convert_time_of_flight_to_distance(self, tof): 164 | return self.SPEED_OF_LIGHT_IN_M_PER_US * tof 165 | 166 | def process_timestamps_measurements(self, uwb_multi_range_raw_msg, unit_coefficients=None): 167 | msg = uwb_multi_range_raw_msg 168 | num_of_units = msg.num_of_units 169 | 170 | # Set default distances and coefficients if not specified 171 | if unit_coefficients is None: 172 | unit_coefficients = np.zeros((num_of_units, 2)) 173 | for i in xrange(num_of_units): 174 | unit_coefficients[i, :] = [0., 1.] 175 | 176 | tofs = [] 177 | ranges = [] 178 | clock_offsets = [] 179 | clock_skews = [] 180 | 181 | timediff_slave = msg.timestamp_master_request_1_recv + msg.timestamp_master_request_2_recv \ 182 | - 2 * msg.timestamp_slave_reply_send 183 | 184 | timediff_master = 2 * msg.timestamp_slave_reply[0] - msg.timestamp_master_request_1[0] \ 185 | - msg.timestamp_master_request_2[0] 186 | tof_master_slave = (timediff_master + timediff_slave) / 4.0 187 | tof_master_slave_us = self.convert_dw_timeunits_to_microseconds(tof_master_slave) 188 | range_master_slave = self.convert_time_of_flight_to_distance(tof_master_slave_us) 189 | range_master_slave = range_master_slave * unit_coefficients[0, 1] + unit_coefficients[0, 0] 190 | 191 | tofs.append(tof_master_slave_us) 192 | ranges.append(range_master_slave) 193 | clock_offsets.append(0.0) 194 | clock_skews.append(0.0) 195 | 196 | adjusted_processing_time_slave = (msg.timestamp_slave_reply[0] - msg.timestamp_master_request_1[0]) \ 197 | - 2 * tof_master_slave 198 | 199 | for i in xrange(1, num_of_units): 200 | # Compute clock offset and skew of listener 201 | clock_offset, clock_skew = self.process_listener_measurement(i, msg) 202 | 203 | # Compute timediff from master -> slave -> listener 204 | rtt_master_slave_listener = \ 205 | (msg.timestamp_slave_reply[i] - msg.timestamp_master_request_1[i]) \ 206 | * (1 + clock_skew) 207 | 208 | # Compute TOF from slave -> listener 209 | tof_slave_listener = rtt_master_slave_listener - tof_master_slave - adjusted_processing_time_slave 210 | # Convert to microseconds and compute corresponding range 211 | tof_slave_listener_us = self.convert_dw_timeunits_to_microseconds(tof_slave_listener) 212 | range_slave_listener = self.convert_time_of_flight_to_distance(tof_slave_listener_us) 213 | range_slave_listener = range_slave_listener * unit_coefficients[i, 1] + unit_coefficients[i, 0] 214 | 215 | tofs.append(tof_slave_listener_us) 216 | ranges.append(range_slave_listener) 217 | clock_offsets.append(clock_offset) 218 | clock_skews.append(clock_skew) 219 | 220 | # Compute slave clock offset and skew for plotting 221 | slave_clock_offset, slave_clock_skew = self.process_slave_measurement(tof_master_slave, msg) 222 | 223 | return tofs, ranges, clock_offsets, clock_skews, slave_clock_offset, slave_clock_skew 224 | 225 | def process_slave_measurement(self, tof_master_slave, uwb_multi_range_raw_msg): 226 | msg = uwb_multi_range_raw_msg 227 | 228 | # clock offset 229 | timestamp_master = msg.timestamp_master_request_1[0] 230 | timestamp_slave = msg.timestamp_master_request_1_recv 231 | clock_offset = timestamp_slave - timestamp_master \ 232 | - tof_master_slave 233 | 234 | # clock skew 235 | clock_diff_1 = float(msg.timestamp_master_request_2[0] - msg.timestamp_master_request_1[0]) 236 | clock_diff_2 = float(msg.timestamp_master_request_2_recv - msg.timestamp_master_request_1_recv) 237 | clock_skew = (1 / clock_diff_2 - 1 / clock_diff_1) / (1 / clock_diff_1) 238 | 239 | return clock_offset, clock_skew 240 | 241 | def process_listener_measurement(self, index, uwb_multi_range_raw_msg): 242 | msg = uwb_multi_range_raw_msg 243 | 244 | # clock offset 245 | timestamp_master = msg.timestamp_master_request_1[0] 246 | timestamp_listener = msg.timestamp_master_request_1[index] 247 | clock_offset = timestamp_listener - timestamp_master 248 | 249 | # clock skew 250 | clock_diff_1 = float(msg.timestamp_master_request_2[0] - msg.timestamp_master_request_1[0]) 251 | clock_diff_2 = float(msg.timestamp_master_request_2[index] - msg.timestamp_master_request_1[index]) 252 | clock_skew = (1 / clock_diff_2 - 1 / clock_diff_1) / (1 / clock_diff_1) 253 | 254 | return clock_offset, clock_skew 255 | 256 | def update_visualization(self, tofs, ranges, clock_offsets, clock_skews, slave_clock_offset, slave_clock_skew): 257 | if not self.show_plots: 258 | return 259 | 260 | import pyqtgraph 261 | num_of_units = len(tofs) 262 | 263 | # ranges 264 | while len(self.range_plot) < num_of_units: 265 | curve_index = len(self.range_plot) 266 | pen = pyqtgraph.mkPen(curve_index) 267 | self.range_plot.add_curve(pen=pen, name="{}".format(curve_index)) 268 | for i in xrange(num_of_units): 269 | self.range_plot.add_point(i, ranges[i]) 270 | 271 | # clock offset 272 | while len(self.clock_offset_plot) < num_of_units: 273 | curve_index = len(self.clock_offset_plot) 274 | pen = pyqtgraph.mkPen(curve_index) 275 | self.clock_offset_plot.add_curve(pen=pen, name="{}".format(curve_index)) 276 | if self.show_slave_clock_offset: 277 | if len(self.clock_offset_plot) < num_of_units + 1: 278 | curve_index = len(self.clock_offset_plot) 279 | pen = pyqtgraph.mkPen(curve_index) 280 | self.clock_offset_plot.add_curve(pen=pen, name="slave") 281 | for i in xrange(num_of_units): 282 | self.clock_offset_plot.add_point(i, clock_offsets[i]) 283 | if self.show_slave_clock_offset: 284 | self.clock_offset_plot.add_point(len(self.clock_offset_plot) - 1, slave_clock_offset) 285 | 286 | # clock skew 287 | while len(self.clock_skew_plot) < num_of_units: 288 | curve_index = len(self.clock_skew_plot) 289 | pen = pyqtgraph.mkPen(curve_index) 290 | self.clock_skew_plot.add_curve(pen=pen, name="{}".format(curve_index)) 291 | if self.show_slave_clock_skew: 292 | if len(self.clock_skew_plot) < num_of_units + 1: 293 | curve_index = len(self.clock_skew_plot) 294 | pen = pyqtgraph.mkPen(curve_index) 295 | self.clock_skew_plot.add_curve(pen=pen, name="slave") 296 | for i in xrange(num_of_units): 297 | self.clock_skew_plot.add_point(i, clock_skews[i]) 298 | if self.show_slave_clock_skew: 299 | self.clock_skew_plot.add_point(len(self.clock_skew_plot) - 1, slave_clock_skew) 300 | 301 | def exec_(self): 302 | if self.show_plots: 303 | import sys 304 | import pyqtgraph 305 | if sys.flags.interactive != 1 or not hasattr(QtCore, 'PYQT_VERSION'): 306 | pyqtgraph.QtGui.QApplication.exec_() 307 | else: 308 | rospy.spin() 309 | 310 | def stop(self): 311 | if self.show_plots: 312 | import pyqtgraph 313 | pyqtgraph.QtGui.QApplication.quit() 314 | else: 315 | rospy.signal_shutdown('User request') 316 | 317 | 318 | def main(): 319 | import signal 320 | 321 | rospy.init_node('uwb_multi_range_node') 322 | u = UWBMultiRange() 323 | 324 | def sigint_handler(sig, _): 325 | if sig == signal.SIGINT: 326 | u.stop() 327 | signal.signal(signal.SIGINT, sigint_handler) 328 | 329 | try: 330 | u.exec_() 331 | except (rospy.ROSInterruptException, select.error): 332 | rospy.logwarn("Interrupted... Stopping.") 333 | 334 | 335 | if __name__ == '__main__': 336 | main() 337 | -------------------------------------------------------------------------------- /src/uwb_tracker_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """uwb_tracker_node.py: Streams tracked positions based on UWB multi-range messages.""" 4 | 5 | from __future__ import print_function 6 | 7 | __author__ = "Benjamin Hepp" 8 | __email__ = "benjamin.hepp@inf.ethz.ch" 9 | __copyright__ = "Copyright 2016 Benjamin Hepp" 10 | 11 | import select 12 | import numpy as np 13 | import roslib 14 | import scipy.stats 15 | roslib.load_manifest('uwb') 16 | import rospy 17 | import tf 18 | 19 | import uwb.msg 20 | 21 | 22 | class UWBTracker(object): 23 | """Position tracker for ultra-wideband range measurements. 24 | 25 | By default the z-coordinate of the state is ignored. 26 | This can be modified with the ROS parameter `ignore_z_position`. 27 | Topics and other options can also be modified with ROS parameters (see the `_read_configuration` method). 28 | """ 29 | 30 | class StateEstimate(object): 31 | """State estimate consisting of a state vector and a covariance matrix. 32 | """ 33 | def __init__(self, state, covariance): 34 | """Initialize a new state estimate. 35 | 36 | Args: 37 | state (numpy.ndarray): State vector. 38 | covariance (numpy.ndarray): Covariance matrix. 39 | """ 40 | self.state = state 41 | self.covariance = covariance 42 | 43 | def __init__(self): 44 | """Initialize tracker. 45 | """ 46 | self._read_configuration() 47 | 48 | self.estimates = {} 49 | self.estimate_times = {} 50 | self.ikf_prev_outlier_flags = {} 51 | self.ikf_outlier_counts = {} 52 | self.outlier_thresholds = {} 53 | 54 | rospy.loginfo("Receiving raw multi-range messages from: {}".format(self.uwb_multi_range_topic)) 55 | rospy.loginfo("Publishing tracker messages to {}".format(self.uwb_tracker_topic)) 56 | rospy.loginfo("Publishing tracker transform as {} -> {}".format(self.tracker_frame, self.target_frame)) 57 | 58 | # ROS publishers and subscribers 59 | self.tracker_frame = self.tracker_frame 60 | self.target_frame = self.target_frame 61 | self.uwb_pub = rospy.Publisher(self.uwb_tracker_topic, uwb.msg.UWBTracker, queue_size=1) 62 | self.tf_broadcaster = tf.TransformBroadcaster() 63 | self.uwb_multi_range_sub = rospy.Subscriber(self.uwb_multi_range_topic, uwb.msg.UWBMultiRangeWithOffsets, 64 | self.handle_multi_range_message) 65 | 66 | def _read_configuration(self): 67 | """Initialize configuration from ROS parameters. 68 | """ 69 | self.uwb_multi_range_topic = rospy.get_param('~multi_range_raw_topic', '/uwb/multi_range_with_offsets') 70 | self.uwb_tracker_topic = rospy.get_param('~tracker_topic', '/uwb/tracker') 71 | self.tracker_frame = rospy.get_param('~tracker_frame', 'uwb') 72 | self.target_frame = rospy.get_param('~target_frame', 'target') 73 | 74 | # Get parameters for covariance matrices 75 | self.initial_position_covariance = rospy.get_param('~initial_position_covariance', 10) 76 | self.process_covariance_position = rospy.get_param('~process_covariance_position', 0) 77 | self.process_covariance_velocity = rospy.get_param('~process_covariance_velocity', 1) 78 | self.measurement_covariance = rospy.get_param('~measurement_covariance', 0.1 ** 2) 79 | 80 | # Get parameters for filter update and initial gauss-newton estimation 81 | self.ignore_z_position = rospy.get_param('~ignore_z_position', True) 82 | # The default value of 7.779 represents the 0.9 quantile of a Chi-Square distribution 83 | # with 4 degrees of freedom (4 UWB measurements). 84 | self.outlier_threshold_quantile = rospy.get_param('~outlier_threshold_quantile', 0.1) 85 | self.ikf_iterations = rospy.get_param('~ikf_iterations', 2) 86 | self.initial_guess_position = np.empty((3, 1), dtype=np.float) 87 | self.initial_guess_position[0] = rospy.get_param('~initial_guess_position_x', 0) 88 | self.initial_guess_position[1] = rospy.get_param('~initial_guess_position_y', 0) 89 | self.initial_guess_position[2] = rospy.get_param('~initial_guess_position_z', 0) 90 | self.initial_guess_iterations = rospy.get_param('~initial_guess_iterations', 200) 91 | self.initial_guess_tolerance = rospy.get_param('~initial_guess_tolerance', 1e-5) 92 | self.initial_guess_residuals_threshold = rospy.get_param('~initial_guess_residuals_threshold', 0.1) 93 | self.ikf_max_outlier_count = rospy.get_param('~ikf_max_outlier_count', 200) 94 | 95 | def handle_multi_range_message(self, multi_range_msg): 96 | """Handle a ROS multi-range message by updating and publishing the state. 97 | 98 | Args: 99 | multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message. 100 | """ 101 | # Update tracker position based on time-of-flight measurements 102 | new_estimate = self.update_estimate(multi_range_msg) 103 | if new_estimate is None: 104 | rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format( 105 | multi_range_msg.address, multi_range_msg.remote_address)) 106 | else: 107 | # Publish tracker message 108 | ros_msg = uwb.msg.UWBTracker() 109 | ros_msg.header.stamp = rospy.get_rostime() 110 | ros_msg.address = multi_range_msg.address 111 | ros_msg.remote_address = multi_range_msg.remote_address 112 | ros_msg.state = new_estimate.state 113 | ros_msg.covariance = np.ravel(new_estimate.covariance) 114 | self.uwb_pub.publish(ros_msg) 115 | 116 | # Publish target transform (rotation is identity) 117 | self.tf_broadcaster.sendTransform( 118 | (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]), 119 | tf.transformations.quaternion_from_euler(0, 0, 0), 120 | rospy.get_rostime(), 121 | self.target_frame, 122 | self.tracker_frame 123 | ) 124 | 125 | def initialize_estimate(self, estimate_id, initial_state): 126 | """Initialize a state estimate with identity covariance. 127 | 128 | The initial estimate is saved in the `self.estimates` dictionary. 129 | The timestamp in the `self.estimate_times` is updated. 130 | 131 | Args: 132 | estimate_id (int): ID of the tracked target. 133 | initial_state (int): Initial state of the estimate. 134 | 135 | Returns: 136 | X (numpy.ndarray): Solution of equation. 137 | """ 138 | x = initial_state 139 | P = self.initial_position_covariance * np.eye(6) 140 | P[3:6, 3:6] = 0 141 | estimate = UWBTracker.StateEstimate(x, P) 142 | self.estimates[estimate_id] = estimate 143 | self.estimate_times[estimate_id] = rospy.get_time() 144 | self.ikf_prev_outlier_flags[estimate_id] = False 145 | self.ikf_outlier_counts[estimate_id] = 0 146 | 147 | def _solve_equation_least_squares(self, A, B): 148 | """Solve system of linear equations A X = B. 149 | Currently using Pseudo-inverse because it also allows for singular matrices. 150 | 151 | Args: 152 | A (numpy.ndarray): Left-hand side of equation. 153 | B (numpy.ndarray): Right-hand side of equation. 154 | 155 | Returns: 156 | X (numpy.ndarray): Solution of equation. 157 | """ 158 | # Pseudo-inverse 159 | X = np.dot(np.linalg.pinv(A), B) 160 | # LU decomposition 161 | # lu, piv = scipy.linalg.lu_factor(A) 162 | # X = scipy.linalg.lu_solve((lu, piv), B) 163 | # Vanilla least-squares from numpy 164 | # X, _, _, _ = np.linalg.lstsq(A, B) 165 | # QR decomposition 166 | # Q, R, P = scipy.linalg.qr(A, mode='economic', pivoting=True) 167 | # # Find first zero element in R 168 | # out = np.where(np.diag(R) == 0)[0] 169 | # if out.size == 0: 170 | # i = R.shape[0] 171 | # else: 172 | # i = out[0] 173 | # B_prime = np.dot(Q.T, B) 174 | # X = np.zeros((A.shape[1], B.shape[1]), dtype=A.dtype) 175 | # X[P[:i], :] = scipy.linalg.solve_triangular(R[:i, :i], B_prime[:i, :]) 176 | return X 177 | 178 | def _compute_measurements_and_jacobians(self, ranges, position, h, H, z): 179 | """Computes the predicted measurements and the jacobian of the measurement model based on the current state. 180 | 181 | Args: 182 | ranges (list of uwb.msg.UWBMultiRange): Range measurement message. 183 | position (numpy.ndarray): Current position state. 184 | h (``Output``) (numpy.ndarray): Vector for the predicted measurements. 185 | H (``Output``) (numpy.ndarray): Vector for the computed jacobian of the measurement model. 186 | z (``Output``) (numpy.ndarray): Vector for the range measurements. 187 | 188 | TODO: 189 | Could be sped up a bit using Cython 190 | """ 191 | for j in xrange(len(ranges)): 192 | offset = ranges[j].offset 193 | offset = np.array([[offset.x], [offset.y], [offset.z]]) 194 | # Observation 195 | if self.ignore_z_position: 196 | h[j] = np.linalg.norm(position[0:2] - offset[0:2]) 197 | else: 198 | h[j] = np.linalg.norm(position - offset) 199 | 200 | # Jacobians 201 | # squared distance by position 202 | hs_to_x = 2 * position - 2 * offset 203 | # distance by squared distance 204 | h_to_hs = 1 / (2 * h[j]) 205 | # distance by position 206 | h_to_x = h_to_hs[0] * hs_to_x 207 | H[j, 0:3] = h_to_x[:, 0] 208 | if self.ignore_z_position: 209 | H[j, 2] = 0 210 | z[j] = ranges[j].range 211 | 212 | def initial_guess(self, ranges): 213 | """Computes an initial position guess based on range measurements. 214 | 215 | The initial position is computed using Gauss-Newton method. 216 | The behavior can be modified with some parameters: `self.initial_guess_...`. 217 | 218 | Args: 219 | ranges (list of floats): Range measurements. 220 | 221 | Returns: 222 | initial_state (numpy.ndarray): Initial state vector (velocity components are zero). 223 | """ 224 | num_of_units = len(ranges) 225 | position = self.initial_guess_position 226 | H = np.zeros((num_of_units, position.size)) 227 | z = np.zeros((num_of_units, 1)) 228 | h = np.zeros((num_of_units, 1)) 229 | residuals = np.zeros((num_of_units, 1)) 230 | for i in xrange(self.initial_guess_iterations): 231 | self._compute_measurements_and_jacobians(ranges, position, h, H, z) 232 | new_residuals = z - h 233 | position = position + np.dot(self._solve_equation_least_squares(np.dot(H.T, H), H.T), new_residuals) 234 | if np.sum((new_residuals - residuals) ** 2) < self.initial_guess_tolerance: 235 | break 236 | residuals = new_residuals 237 | rospy.loginfo('initial guess residuals: {}'.format(residuals)) 238 | if np.any(np.abs(residuals) > self.initial_guess_residuals_threshold): 239 | # This initial guess is not good enough 240 | return None 241 | initial_state = np.zeros((6, 1)) 242 | initial_state[0:3] = position 243 | return initial_state 244 | 245 | def update_estimate(self, multi_range_msg): 246 | """Update tracker based on a multi-range message. 247 | 248 | Updates estimate and timestamp in the `self.estimate` and `self.estimate_times` dictionaries. 249 | 250 | Args: 251 | multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message. 252 | 253 | Returns: 254 | new_estimate (StateEstimate): Updated position estimate. 255 | """ 256 | estimate_id = (multi_range_msg.address, multi_range_msg.remote_address) 257 | if estimate_id not in self.estimates: 258 | initial_state = self.initial_guess(multi_range_msg.ranges) 259 | if initial_state is None: 260 | return None 261 | self.initialize_estimate(estimate_id, initial_state) 262 | 263 | current_time = rospy.get_time() 264 | timestep = current_time - self.estimate_times[estimate_id] 265 | estimate = self.estimates[estimate_id] 266 | new_estimate, outlier_flag = self.update_filter(timestep, estimate, multi_range_msg.ranges) 267 | if not outlier_flag: 268 | self.estimates[estimate_id] = new_estimate 269 | self.estimate_times[estimate_id] = current_time 270 | if self.ikf_prev_outlier_flags[estimate_id]: 271 | self.ikf_prev_outlier_flags[estimate_id] = False 272 | # If too many outliers are encountered in a row the estimate is deleted. 273 | # This will lead to a new initial guess for the next multi-range message. 274 | if outlier_flag: 275 | if not self.ikf_prev_outlier_flags[estimate_id]: 276 | self.ikf_prev_outlier_flags[estimate_id] = True 277 | self.ikf_outlier_counts[estimate_id] = 0 278 | self.ikf_outlier_counts[estimate_id] += 1 279 | if self.ikf_outlier_counts[estimate_id] >= self.ikf_max_outlier_count: 280 | del self.estimates[estimate_id] 281 | rospy.loginfo('Too many outliers in a row. Resetting estimate for address={}, remote_address={}'.format( 282 | multi_range_msg.address, multi_range_msg.remote_address 283 | )) 284 | 285 | return new_estimate 286 | 287 | def _ikf_iteration(self, x, n, ranges, h, H, z, estimate, R): 288 | """Update tracker based on a multi-range message. 289 | 290 | Args: 291 | multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message. 292 | 293 | Returns: 294 | new_estimate (StateEstimate): Updated position estimate. 295 | """ 296 | new_position = n[0:3] 297 | self._compute_measurements_and_jacobians(ranges, new_position, h, H, z) 298 | res = z - h 299 | S = np.dot(np.dot(H, estimate.covariance), H.T) + R 300 | K = np.dot(estimate.covariance, self._solve_equation_least_squares(S.T, H).T) 301 | mahalanobis = np.sqrt(np.dot(self._solve_equation_least_squares(S.T, res).T, res)) 302 | if res.size not in self.outlier_thresholds: 303 | self.outlier_thresholds[res.size] = scipy.stats.chi2.isf(self.outlier_threshold_quantile, res.size) 304 | outlier_threshold = self.outlier_thresholds[res.size] 305 | if mahalanobis < outlier_threshold: 306 | n = x + np.dot(K, (res - np.dot(H, x - n))) 307 | outlier_flag = False 308 | else: 309 | outlier_flag = True 310 | return n, K, outlier_flag 311 | 312 | def _compute_process_and_covariance_matrices(self, dt): 313 | """Computes the transition and covariance matrix of the process model and measurement model. 314 | 315 | Args: 316 | dt (float): Timestep of the discrete transition. 317 | 318 | Returns: 319 | F (numpy.ndarray): Transition matrix. 320 | Q (numpy.ndarray): Process covariance matrix. 321 | R (numpy.ndarray): Measurement covariance matrix. 322 | """ 323 | F = np.array(np.bmat([[np.eye(3), dt * np.eye(3)], [np.zeros((3, 3)), np.eye(3)]])) 324 | self.process_matrix = F 325 | q_p = self.process_covariance_position 326 | q_v = self.process_covariance_velocity 327 | Q = np.diag([q_p, q_p, q_p, q_v, q_v, q_v]) ** 2 * dt 328 | r = self.measurement_covariance 329 | R = r * np.eye(4) 330 | self.process_covariance = Q 331 | self.measurement_covariance = R 332 | return F, Q, R 333 | 334 | def update_filter(self, timestep, estimate, ranges): 335 | """Update position filter. 336 | 337 | Args: 338 | timestep (float): Time elapsed since last update. 339 | estimate (StateEstimate): Position estimate to update. 340 | ranges (list of floats): Range measurements. 341 | 342 | Returns: 343 | new_estimate (StateEstimate): Updated position estimate. 344 | outlier_flag (bool): Flag indicating whether the measurement was rejected as an outlier. 345 | """ 346 | num_of_units = len(ranges) 347 | x = estimate.state 348 | P = estimate.covariance 349 | # Compute process matrix and covariance matrices 350 | F, Q, R = self._compute_process_and_covariance_matrices(timestep) 351 | # rospy.logdebug('F: {}'.format(F)) 352 | # rospy.logdebug('Q: {}'.format(Q)) 353 | # rospy.logdebug('R: {}'.format(R)) 354 | # Prediction 355 | x = np.dot(F, x) 356 | P = np.dot(F, np.dot(P, F.T)) + Q 357 | # Update 358 | n = np.copy(x) 359 | H = np.zeros((num_of_units, x.size)) 360 | z = np.zeros((num_of_units, 1)) 361 | h = np.zeros((num_of_units, 1)) 362 | for i in xrange(self.ikf_iterations): 363 | n, K, outlier_flag = self._ikf_iteration(x, n, ranges, h, H, z, estimate, R) 364 | if outlier_flag: 365 | new_estimate = estimate 366 | else: 367 | new_state = n 368 | new_covariance = np.dot((np.eye(6) - np.dot(K, H)), P) 369 | new_estimate = UWBTracker.StateEstimate(new_state, new_covariance) 370 | return new_estimate, outlier_flag 371 | 372 | def exec_(self): 373 | rospy.spin() 374 | 375 | def stop(self): 376 | rospy.signal_shutdown('User request') 377 | 378 | 379 | def main(): 380 | import signal 381 | 382 | rospy.init_node('uwb_tracker_node') 383 | u = UWBTracker() 384 | 385 | def sigint_handler(sig, _): 386 | if sig == signal.SIGINT: 387 | u.stop() 388 | signal.signal(signal.SIGINT, sigint_handler) 389 | 390 | try: 391 | u.exec_() 392 | except (rospy.ROSInterruptException, select.error): 393 | rospy.logwarn("Interrupted... Stopping.") 394 | 395 | 396 | if __name__ == '__main__': 397 | main() 398 | -------------------------------------------------------------------------------- /utils/calibrate.m: -------------------------------------------------------------------------------- 1 | % Calibration script for UWB units. Fits a linear model to the data and 2 | % computes the spatial configuration of the UWB units. 3 | % 4 | % A Gauss-Newton fitting is run multiple times to update the estimated 5 | % variance and the corresponding outlier-threshold. 6 | 7 | %% Input and output 8 | % Input: 9 | % uwb_ranges: NxM array containing N range-measurements for the M UWB units 10 | % gt_ranges: NxM array containing N ground-truth ranges for the M UWB units 11 | % gt_positions: NxMx3 array containing N ground-truth positions for the M 12 | % UWB units 13 | % 14 | % Output: Writes a .mat and .yaml file containing the average positions of 15 | % the UWB units and the coefficients for a linear model. 16 | 17 | %% Visualization 18 | % Inliers are shown in colors. 19 | % Outliers are shown in black. 20 | % Ground truth is shown as crosses (x). 21 | % UWB measurements are shown as pluses (+). 22 | 23 | %% If UWB ranges and true positions are not already defined load them from a file 24 | if ~exist('uwb_ranges', 'var') || ~exist('gt_ranges', 'var') || ~exist('gt_positions', 'var') 25 | [FileName, PathName, ~] = uigetfile('*.mat', 'Select MAT file to open'); 26 | load(fullfile(PathName, FileName)); 27 | end 28 | 29 | %% Parameters 30 | num_of_measurements = size(uwb_ranges, 1); 31 | num_of_units = size(uwb_ranges, 2); 32 | num_of_iterations = 5; 33 | 34 | %% Plots for visualization 35 | figure(); 36 | cmap = colormap('lines'); 37 | uwb_lines = {}; 38 | gt_ranges_lines = {}; 39 | for j = 1:num_of_units 40 | color = cmap(j, :); 41 | uwb_lines{j} = animatedline('Marker', '+', 'LineStyle', 'none', 'Color', color); 42 | gt_ranges_lines{j} = animatedline('Marker', 'x', 'LineStyle', 'none', 'Color', color); 43 | end 44 | uwb_outliers = animatedline('Marker', '+', 'LineStyle', 'none', 'Color', 'k'); 45 | gt_outliers = animatedline('Marker', 'x', 'LineStyle', 'none', 'Color', 'k'); 46 | xlabel('measurement #'); 47 | ylabel('range'); 48 | drawnow; 49 | 50 | %% Initialize coefficients and variances 51 | % x contains the constant and linear coefficient for each unit 52 | num_of_units = size(uwb_ranges, 2); 53 | x = ones(2 * num_of_units, 1); 54 | x(1:2:end) = 0; 55 | uwb_variance = ones(size(uwb_ranges, 2), 1); 56 | inlier_mask = logical(ones(size(uwb_ranges, 2), 1)); 57 | 58 | % Estimate a first-order model: groundtruth = p0 + p1 * UWB 59 | for iterations = 1:num_of_iterations 60 | z_v_hat = []; 61 | z_v_true = []; 62 | H = []; 63 | 64 | gt_range_cell = cell(num_of_units, 1); 65 | measurement = cell(num_of_units, 1); 66 | 67 | % Visualization 68 | for j = 1:num_of_units 69 | clearpoints(uwb_lines{j}); 70 | clearpoints(gt_ranges_lines{j}); 71 | end 72 | clearpoints(uwb_outliers); 73 | clearpoints(gt_outliers); 74 | 75 | % Build Jacobian and model predictions 76 | for i=1:num_of_measurements 77 | for j = 1:num_of_units 78 | uwb_range = uwb_ranges(i, j); 79 | gt_range = gt_ranges(i, j); 80 | 81 | % Exclude invalid measurements 82 | if isempty(uwb_range) || (uwb_range < 0) || isnan(gt_range) 83 | continue; 84 | end 85 | % Coefficients for unit j 86 | p0 = x(j*2 - 1); 87 | p1 = x(j*2); 88 | 89 | % Model prediction 90 | z_hat = p0 + p1 * uwb_range; 91 | 92 | % Detect UWB outliers 93 | if abs(gt_range - z_hat) > 3*uwb_variance(j) 94 | % Visualize outlier 95 | addpoints(uwb_outliers, i, z_hat); 96 | addpoints(gt_outliers, i, gt_range); 97 | inlier_mask(i) = false; 98 | else 99 | % Visualize inlier 100 | addpoints(uwb_lines{j}, i, z_hat); 101 | addpoints(gt_ranges_lines{j}, i, gt_range); 102 | % Append prediction and Jacobian to full matrix 103 | z_v_hat = [z_v_hat; z_hat]; 104 | z_v_true = [z_v_true; gt_range]; 105 | % Jacobian 106 | H_p0 = 1; 107 | H_p1 = uwb_range; 108 | H_p = zeros(1, 4*2); 109 | H_p(2*j - 1:2*j) = [H_p0, H_p1]; 110 | H = [H; H_p]; 111 | % Save inlier ranges for adaptive variance computation 112 | gt_range_cell{j} = [gt_range_cell{j}, gt_range]; 113 | measurement{j} = [measurement{j}, uwb_range]; 114 | end 115 | drawnow limitrate; 116 | end 117 | end 118 | 119 | % Perform Gauss-Newton step 120 | x = x + (H'*H) \ H'*(z_v_true - z_v_hat); 121 | 122 | % Update estimate of UWB variance 123 | for j = 1:numel(gt_range_cell) 124 | p0 = x(j*2 - 1); 125 | p1 = x(j*2); 126 | new_estimate = p0 + p1 * measurement{j}; 127 | uwb_variance(j) = std((new_estimate - gt_range_cell{j})); 128 | end 129 | 130 | uwb_variance 131 | end 132 | 133 | 134 | %% Create configuration file 135 | 136 | params = struct(); 137 | 138 | % Write UWB positions into parameter structure 139 | uwb_tracker_center = zeros(3, 1); 140 | for i = 1:3 141 | uwb_tracker_center(i) = mean(mean(gt_positions(inlier_mask, :, i))); 142 | end 143 | for j = 1:num_of_units 144 | rel_uwb_pos = squeeze(mean(gt_positions(inlier_mask, j, :))); 145 | params = setfield(params, ['m_uwb', int2str(j)], rel_uwb_pos); 146 | a = 0; 147 | b = 1; 148 | params = setfield(params, ['UWB', int2str(j), 'p0'], a); 149 | params = setfield(params, ['UWB', int2str(j), 'p1'], b); 150 | end 151 | 152 | % Visualize UWB positions 153 | figure(2); 154 | hold on; 155 | plot3(params.m_uwb1(1), params.m_uwb1(2), params.m_uwb1(3),'r+'); 156 | plot3(params.m_uwb2(1), params.m_uwb2(2), params.m_uwb2(3),'g+'); 157 | plot3(params.m_uwb3(1), params.m_uwb3(2), params.m_uwb3(3),'b+'); 158 | plot3(params.m_uwb4(1), params.m_uwb4(2), params.m_uwb4(3),'c+'); 159 | xlabel('x'); 160 | xlabel('y'); 161 | xlabel('z'); 162 | box on; 163 | grid on; 164 | grid minor; 165 | daspect([1, 1, 1]); 166 | 167 | % Write coefficients into parameter structure 168 | for j = 0:num_of_units-1 169 | p0 = x(j*2 + 1); 170 | p1 = x(j*2 + 2); 171 | params = setfield(params, ['UWB', int2str(j), 'p0'], p0); 172 | params = setfield(params, ['UWB', int2str(j), 'p1'], p1); 173 | end 174 | 175 | % Save as .mat file 176 | save('params', 'params') 177 | 178 | % Generate YAML configuration 179 | display('Generating yaml config'); 180 | f = fopen('config.yaml', 'w'); 181 | fprintf(f, 'num_of_units: %d\n', num_of_units); 182 | for i = 1:num_of_units 183 | unit_id = i - 1; 184 | uwb_pos = getfield(params, ['m_uwb', int2str(i)]); 185 | uwb_p0 = getfield(params, ['UWB', int2str(i), 'p0']); 186 | uwb_p1 = getfield(params, ['UWB', int2str(i), 'p1']); 187 | fprintf(f, 'unit_%d:\n', unit_id); 188 | fprintf(f, ' x: %f\n', uwb_pos(1)); 189 | fprintf(f, ' y: %f\n', uwb_pos(2)); 190 | fprintf(f, ' z: %f\n', uwb_pos(3)); 191 | fprintf(f, ' p0: %f\n', uwb_p0); 192 | fprintf(f, ' p1: %f\n', uwb_p1); 193 | end 194 | fclose(f); 195 | -------------------------------------------------------------------------------- /utils/calibrate_example.m: -------------------------------------------------------------------------------- 1 | %% Load file with data 2 | % 3 | % The file should contain the following fields: 4 | % tracker_pos: Ground-truth positions of individual UWB units 5 | % tracker_transform.pos: Ground-truth position of tracker coordinate frame 6 | % tracker_transform.rot: Ground-truth quaternion of tracker coordinate frame 7 | % uwb_ranges: UWB ranges from tracker UWB units to target unit 8 | % vicon_ranges: Ground-truth ranges from tracker UWB units to target unit 9 | % 10 | [FileName, PathName, ~] = uigetfile('*.mat', 'Select MAT file to open'); 11 | data = load(fullfile(PathName, FileName)); 12 | 13 | %% Parameters 14 | VICON_OUTLIER_DISTANCE = 0.5; 15 | num_of_units = data.num_of_units; 16 | 17 | uwb_ranges = []; 18 | gt_ranges = []; 19 | gt_positions = zeros(0, num_of_units, 3); 20 | for i=1:numel(data.time) 21 | %% Detect vicon outliers 22 | outlier = false; 23 | for j = 1:num_of_units 24 | d = norm(data.tracker_transform.pos(:, i) - squeeze(data.tracker_pos(i, j, :))); 25 | if d > VICON_OUTLIER_DISTANCE 26 | outlier = true; 27 | break; 28 | end 29 | end 30 | if ~outlier 31 | % Ground truth range 32 | gt_ranges = [gt_ranges; data.vicon_ranges(i, :)]; 33 | % UWB range 34 | uwb_ranges = [uwb_ranges; data.uwb_ranges(i, :)]; 35 | % UWB position in tracker coordinate frame 36 | rel_position = squeeze(data.tracker_pos(i, :, :)) - repmat(data.tracker_transform.pos(:, i)', [num_of_units, 1]); 37 | tracker_tf_quaternion = data.tracker_transform.rot(:, i)'; 38 | rel_position = quatrotate(tracker_tf_quaternion, rel_position); 39 | gt_positions(end+1, :, :) = rel_position; 40 | end 41 | end 42 | 43 | calibrate(); 44 | --------------------------------------------------------------------------------