Package Summary
Standalone QT-based IMU/GPS decoder nodes for Witmotion UART-compatible sensor devices
- Maintainer status: maintained
- Maintainer: Andrey Vukolov <andrey.vukolov AT elettra DOT eu>
- Author: Andrey Vukolov <andrey.vukolov AT elettra DOT eu>
- License: MIT
- Bug / feature tracker: https://github.com/ElettraSciComp/witmotion_IMU_ros/issues
- Source: git https://github.com/ElettraSciComp/witmotion_IMU_ros.git (branch: main)
Package Summary
Standalone QT-based IMU/GPS decoder nodes for Witmotion UART-compatible sensor devices
- Maintainer status: maintained
- Maintainer: Andrey Vukolov <andrey.vukolov AT elettra DOT eu>
- Author: Andrey Vukolov <andrey.vukolov AT elettra DOT eu>
- License: MIT
- Bug / feature tracker: https://github.com/ElettraSciComp/witmotion_IMU_ros/issues
- Source: git https://github.com/ElettraSciComp/witmotion_IMU_ros.git (branch: main)
Contents
Overview
This package implements a ROS 1 wrapper for Witmotion IMU driver library. It reads the data from the family of TTL-compatible inertial pose estimation units (IMUs) manufactured by WitMotion Shenzhen Co.,Ltd publishing the information in a ROS-native way using sensor_msgs, geometry_msgs and std_msgs message definition packages. The port access model is implemented in a monopolistic way according to UNIX specifications, so only one instance of the module can be executed for the dedicated virtual device. The library allows programming of any control over the Witmotion sensors. There is native support for baud rates up to 115200 baud, and polling frequencies up to 200 Hz. Please refer to the example controller application to learn how to program a sensor that is not supported now.
Package URL: https://github.com/ElettraSciComp/witmotion_IMU_ros
Underlying library URL: https://github.com/ElettraSciComp/witmotion_IMU_QT
Online autogenerated documentation URL: https://elettrascicomp.github.io/witmotion_IMU_QT
The package was developed under Remotization and Robotization Initiative at Elettra Sincrotrone Trieste by the members of the ICT Group.
Supported/tested Devices
The initial tests of the module were done using the following Witmotion sensor devices:
WT31N 3-Axis Accelerometer/Gyroscope(Linear accelerations + 2-axis Euler angles gravity tracking) - tested on baud rates 9600 and 115200 baud, polling frequencies up to 100 Hz.
JY901B 9-Axis Combined IMU/Magnetometer/Altimeter (Linear accelerations, angular velocities, Euler angles, magnetic field, barometry, altitude)
WT901 9-Axis Combined Open Circuit IMU/Magnetometer (Linear accelerations, angular velocities, Euler angles, magnetic field) - tested on baud rates from 2400 to 115200 baud, polling frequencies up to 150 Hz. NOTE: The polling frequency of 200 Hz declared by Witmotion is actually a packet drop frequency which is slightly higher than the maximal possible measurement frequency.
Community-driven tests
HWT905 TTL 9-Axis IP67 Enclosed Combined IMU/Magnetometer/Altimeter/Inclinometer (Linear accelerations, angular velocities, Euler angles, magnetic field, barometry, altitude, calibrated native quaternion), equipped with original USB transceiver - tested by Vincent Foucault on 115200 baud, measurement frequencies from 10 up to 200 Hz and polling frequencies up to 100 Hz.
Actual bandwidth and measurement frequencies
According to the official Witmotion command table, the available bandwidth values and connected measurement frequencies are the following:
Measurement frequency |
256 (if supported) |
184 |
94 |
44 |
21 |
10 |
5 |
Hz |
ID value |
0x00 |
0x01 |
0x02 |
0x03 |
0x04 |
0x05 |
0x06 |
HEX |
Datasheets and official documentation
The module is developed according to the specifications released by Witmotion, the presented snapshot has a download date is 23.02.2022. The official website https://wiki.wit-motion.com is not always accessible, so the PDF snapshots are placed under IPFS web directory:
As additional material, there is the command table, consisting of the list of special commands used to configure 9XX series of sensors.
Published message types
The module published the following message types (once the incoming data is available from the sensor, all the topic names and publisher availability are configurable):
sensor_msgs/Imu for accelerometer/gyroscope data
sensor_msgs/Temperature for thermal observation
sensor_msgs/MagneticField for magnetometer data
sensor_msgs/FluidPressure for barometer output
std_msgs/Float64 for altimeter output
geometry_msgs/Quaternion for orientation output
sensor_msgs/NavSatFix for GPS position output
geometry_msgs/Twist for GPS ground speed output
geometry_msgs/Vector3 for GPS accuracy (diagonal covariance) output
std_msgs/UInt32 for GPS active satellite IDs counter messages
std_msgs/Float32 for GPS altitude output
rosgraph_msgs/Clock for realtime clock output
Nodes
NOTE: All the topic names here are configurable. One can adjust the topic names using a config file. Also, all publishers of the node except IMU, orientation and GPS publishers, are ready for linear calibration because there can be differences in decoding coefficients between the sensors (proven for WT31N and JY901B sensors). To set the linear calibration for a publisher, use coefficient and addition parameters in the dedicated configuration namespace (see the sample config file).
witmotion_imu
Opens a serial port to read data from Witmotion sensor.Published Topics
imu (sensor_msgs/Imu)- IMU measurement output.
- Temperature observation.
- Magnetometer output.
- Athmospheric pressure.
- Geographic altitude.
- Raw orientation.
- GPS fix position.
- GPS altimeter.
- Number of the GPS active satellite IDs.
- GPS position variance vector (ENU orientation).
- GPS ground speed.
- Realtime clock information provided by the sensor (if available).
Services
restart_imu (std_srvs/Empty)- Call this service to restart the UART kernel bridge after sensor error is occurred.
Parameters
restart_service_name (string, default: /restart_imu)- Service name to advertise the after-error-restart function.
- The virtual kernel device name for a serial port.
- Port rate value to be used by the library for opening the port.
- The sensor polling interval in milliseconds. If this parameter is omitted, the default value is set up by the library (50 ms).
- Topic name for IMU data publisher.
- IMU message header frame.
- Instructs the node to use the native quaternion orientation measurement from the sensor instead of the one synthesized from Euler angles.
- Includes presence control for IMU components: acceleration, angular_velocity, orientation. Every component includes enabled flag and row-major 3x3 matrix defining covariance (see the sample config file)
- Temperature topic name.
- Temperature message header frame.
- Enables/disables temperature measurement extraction.
- Message type string to determine from which type of Witmotion measurement message the temperature data should be extracted (please refer to the original documentation for a detailed description). The possible values are: acceleration, angular_vel, orientation or magnetometer.
- Constant variance, if applicable, otherwise 0.
- Magnetometer topic name.
- Magnetometer message header frame.
- Enables/disables magnetic measurement extraction.
- Static covariance, all zeros for unknown
- Enables/disables barometer measurement extraction.
- Constant variance, if applicable, otherwise 0.
- Pressure message header frame.
- Barometer topic name.
- Enables/disables altimeter measurement extraction.
- Altimeter topic name.
- Orientation topic name.
- Enables/disables orientation measurement extraction.
- Enables/disables all GPS receiver measurements extraction.
- Frame ID for GPS fixed position publisher.
- Topic name for GPS fixed position publisher.
- Topic name for GPS altitude publisher.
- Topic name for GPS active satellites number publisher.
- Topic name for GPS diagonal variance publisher.
- Topic name for GPS ground speed publisher.
- Enables/disables realtime clock decoder/publisher.
- Topic name for realtime clock information publisher.
- Instructs the node to perform an attempt to pre-synchronize the sensor's internal realtime clock.
Report a Bug
ROS driver - Use GitHub to report bugs or submit feature requests. [View active issues]
Underlying library - Use GitHub to report bugs or submit feature requests. [View active issues]
Troubleshooting
Message builder setup
If the node does not publish any IMU information, ensure at first that you set up enabled parameters properly for all the inertial measurements your sensor actually provides! Otherwise, the IMU data pack will never be considered complete and the node will not publish on the imu topic. If, for example, WT31N sensor does not provide angular velocities, set the enabled parameter in imu_publisher/measurements/angular_velocity config file section to false.
Absence of quaternion orientation output
If imu_publisher/use_native_orientation parameter is set to true and the sensor does not provide orientation information in native quaternion format, the IMU information will never be published, as it is demonstrated in this issue. If the native quaternion is required, enable it in the sensor's internal settings using the controller application.
Polling interval crash
If you see a lot of messages like QSocketNotifier: Invalid socket <N> and type 'read', disabling..., check the value of the polling_interval parameter. These messages appear when the polling interval is too small (usually less than 20 ms), and it is not enough for the kernel port driver to become ready for accumulating data.
Locked configuration registers
On some Witmotion sensors from 9xx series, the configuration registers are "locked". These sensors require unlocking for every reconfiguration session, so the special command should be sent before any other configuration packet to let it work. According to the document shared by Witmotion by request, the binary formulation of the command that should be sent to "unlock" the configuration registers is the following (5 bytes should be sent via UART channel):
- 0xFF 0xAA 0x69 0x88 0xB5
Non-standard baudrates
The baud rates over 256000 baud are considered non-standard and they are not supported officially by the underlying Qt backend. However, the official Windows controller application by Witmotion allows to set up the baudrate to values up to 921600 baud. If this was occasionally done, and the node cannot connect to the sensor, the user is strongly advised to reduce the baud rate via the same Windows controller application to 115200 baud, and then to try to run the node again.
Usage of controller applications
The underlying library is shipped with a set of controller applications. They allow the users to set up sensors' internal parameters and specify output data. These applications could be run using rosrun. Here only the brief list and set of usage examples is presented. Please refer to the underlying library documentation for a detailed description of controller applications and their options.
Message Enumerator
This application enumerates and catalogizes all messages emitted by the sensor once they comply with Witmotion 11-byte serial protocol. Use this message_enumerator to determine the desired baudrate, port and exact message set your sensor produces. The following example demonstrates the running of enumeration from /dev/ttyUSB0 port on 115200 baud with a polling interval of 10 ms.
rosrun witmotion_ros message-enumerator -d ttyUSB0 -b 115200 -p 10
Citation
ROS module
DOI: 10.5281/zenodo.7682518
Modern BibTeX
@software{witmotion_ros_2023, author = {Vukolov, Andrey and Kitagawa, Shingo and ElettraSciComp and Baskara and zacharykratochvil}, title = {{ElettraSciComp/witmotion\_IMU\_ros: Version 1.2.27}}, month = August, year = 2023, publisher = {Zenodo}, version = {1.2.27}, doi = {10.5281/zenodo.7682518}, url = {https://doi.org/10.5281/zenodo.7682518} }
Plain BibTeX
@misc{witmotion_ros_2023, author = {Vukolov, Andrey and Kitagawa, Shingo and ElettraSciComp and Baskara and zacharykratochvil}, title = {{ElettraSciComp/witmotion\_IMU\_ros: Version 1.2.27}}, year = 2023, publisher = {Zenodo}, doi = {10.5281/zenodo.7682518}, howpublished = {Github Releases \url{https://github.com/ElettraSciComp/witmotion_IMU_ros/tree/1.2.27}} }
Library
DOI: 10.5281/zenodo.7017118
Modern BibTeX
@software{andrei_vukolov_2022_7017118, author = {Vukolov, Andrey}, title = {ElettraSciComp/witmotion\_IMU\_QT: Version 0.9.17}, month = August, year = 2022, publisher = {Zenodo}, version = {v0.9.17-alpha}, doi = {10.5281/zenodo.7017118}, url = {https://doi.org/10.5281/zenodo.7017118} }
Plain BibTeX
@misc{andrei_vukolov_2022_7017118, author = {Vukolov, Andrey}, title = {ElettraSciComp/witmotion\_IMU\_QT: Version 0.9.17}, year = 2022, publisher = {Zenodo}, doi = {10.5281/zenodo.7017118}, howpublished = {Github Releases \url{https://github.com/ElettraSciComp/witmotion_IMU_QT/releases/tag/v0.9.17-alpha}} }