diff --git a/thormang3_imu_3dm_gx4/.travis.yml b/thormang3_imu_3dm_gx4/.travis.yml deleted file mode 100644 index 7cfc53d0..00000000 --- a/thormang3_imu_3dm_gx4/.travis.yml +++ /dev/null @@ -1,32 +0,0 @@ -language: - - cpp -compiler: - - gcc -notifications: - email: - recipients: - - sunke.polyu@gmail.com -before_install: # Use this to prepare the system to install prerequisites or dependencies - - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' - - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - - sudo apt-get update -qq -install: # Use this to install any prerequisites or dependencies necessary to run your build - - export ROS_DISTRO=hydro - - sudo apt-get install -qq -y ros-$ROS_DISTRO-ros-base ros-$ROS_DISTRO-common-msgs ros-$ROS_DISTRO-diagnostic-updater - # Setup rosdep - - sudo rosdep init - - rosdep update -before_script: # Use this to prepare your build for testing e.g. copy database configurations, environment variables, etc. - - export CI_SOURCE_PATH=$(pwd) - - export REPOSITORY_NAME=${PWD##*/} - - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - - source /opt/ros/$ROS_DISTRO/setup.bash - # Create workspace - - mkdir -p ~/ros/catkin_ws/src - - cd ~/ros/catkin_ws/src - - catkin_init_workspace - - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace - - cd ../ - - export ROS_PARALLEL_JOBS='-j4 -l4' # Limit parallel jobs -script: # All commands must exit with code 0 on success. Anything else is considered failure. - - catkin_make --cmake-args -DCMAKE_BUILD_TYPE=Release diff --git a/thormang3_imu_3dm_gx4/CHANGELOG.rst b/thormang3_imu_3dm_gx4/CHANGELOG.rst deleted file mode 100644 index ca04e4db..00000000 --- a/thormang3_imu_3dm_gx4/CHANGELOG.rst +++ /dev/null @@ -1,25 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package imu_3dm_gx4 -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.3 (2017-05-23) ------------ -* updated cmake file for ros install -* Contributors: SCH - -0.1.2 (2017-04-24) ------------ -* none - -0.1.1 (2016-08-19) ------------ -* bug fix -* package name changed - imu_3dm_gx4 -> thormang3_imu_3dm_gx4 -* Contributors: Jay Song - -0.1.0 (2016-08-18) ------------ -* modified package information for release -* modified the original sources (imu_3dm_gx4) -* Contributors: Jay Song, Zerom, Pyo diff --git a/thormang3_imu_3dm_gx4/CMakeLists.txt b/thormang3_imu_3dm_gx4/CMakeLists.txt deleted file mode 100644 index d2df4c71..00000000 --- a/thormang3_imu_3dm_gx4/CMakeLists.txt +++ /dev/null @@ -1,84 +0,0 @@ -################################################################################ -# CMake -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(thormang3_imu_3dm_gx4) - -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") - -################################################################################ -# Packages -################################################################################ -find_package(catkin REQUIRED COMPONENTS - diagnostic_updater - message_generation - roscpp - geometry_msgs - sensor_msgs -) - -find_package(Boost REQUIRED) - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ -add_message_files(DIRECTORY msg) - -generate_messages(DEPENDENCIES geometry_msgs) - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Catkin specific configuration -################################################################################ -catkin_package( - CATKIN_DEPENDS message_runtime geometry_msgs sensor_msgs -) - -################################################################################ -# Build -################################################################################ -include_directories( - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIR} -) - -add_definitions("-std=c++0x -Wall -Werror") - -add_executable(${PROJECT_NAME} src/imu_3dm_gx4.cpp src/imu.cpp) - -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) - -################################################################################ -# Install -################################################################################ -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY calib/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY script/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ diff --git a/thormang3_imu_3dm_gx4/LICENSE.txt b/thormang3_imu_3dm_gx4/LICENSE.txt deleted file mode 100644 index a287ab53..00000000 --- a/thormang3_imu_3dm_gx4/LICENSE.txt +++ /dev/null @@ -1,13 +0,0 @@ -Copyright 2014 Kumar Robotics - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. diff --git a/thormang3_imu_3dm_gx4/README.md b/thormang3_imu_3dm_gx4/README.md deleted file mode 100644 index f65f8f29..00000000 --- a/thormang3_imu_3dm_gx4/README.md +++ /dev/null @@ -1,90 +0,0 @@ -# imu_3dm_gx4 - -![Picture of IMU](http://www.microstrain.com/sites/default/files/styles/product_image_main/public/products/GX4-25_ProductImage1.00.jpg?itok=gkOa-CBI) - -The `imu_3dm_gx4` package provides support for the [Lord Corporation](http://www.microstrain.com) Microstrain [3DM-GX4-25](http://www.microstrain.com/inertial/3dm-gx4-25) series IMU. The package employs the MIP packet format, so it could conceivably be adapted to support other versions of Microstrain products with relatively little effort. At present, the 15 and 45 series AHRS systems are not supported. - -This package works on Ubuntu 12.04 and 14.04. - -## Version History - -* **0.0.4**: - - Fixed issue where packets would be dropped if the header checksum was broken up - into multiple packets. - - Added `verbose` option. -* **0.0.3**: - - Replaced `decimation` options with `rate` options. Decimation is automatically calculated. - - Added a file for use with Kalibr. - - Added a udev rule to configure permissions. -* **0.0.2**: - - Units of acceleration are now m/s^2, in agreement with ROS convention. - - Cleaned up code base, replaced error codes with exceptions. - - Status can now be viewed from `rqt_runtime_monitor`. - - Filter output is now in a single, custom message with covariances and important status flags. - - Added option to enable/disable accelerometer update in the estimator. - - Removed TF broadcast option. - - Reformatted code base to clang-llvm convention. -* **0.0.1**: - - First release. - -## Options - -The `imu_3dm_gx4` node supports the following base options: -* `device`: Path to the device in `/dev`. Defaults to `/dev/ttyACM0`. -* `baudrate`: Baudrate to employ with serial communication. Defaults to `115200`. -* `frame_id`: Frame to use in headers. -* `imu_rate`: IMU rate to use, in Hz. Default is 100. -* `verbose`: If true, packet reads and mismatched checksums will be logged. - -The following additional options are present for leveraging the 3DM's onboard estimation filter: -* `enable_filter`: If true, the IMU estimation filter is enabled. Default is false. -* `filter_rate`: Filter rate to use, in Hz. Default is 100. -* `enable_mag_update`: If true, the IMU will use the magnetometer to correct the heading angle estimate. Default is false. -* `enable_accel_update`: If true, the IMU will use the accelerometer to correct -the roll/pitch angle estimates. Default is true. - -**In order to launch the node** (streaming IMU data at 100Hz), execute: - -`$ roslaunch imu_3dm_gx4 imu.launch` - -### Note on rate parameters: - -The `_rate` options command the requested frequency by calculating a 'decimation value', which is defined by the relation: - -``` - frequency = base_frequency / decimation -``` - -Where the base frequency is 1kHz for the GX4. Since decimation values are integers, certain frequencies cannot be expressed by the above relation. 800Hz, for example, cannot be specified by an integer decimation rate. In these cases, you will receive whichever frequency is obtained by rounding decimation down to the nearest integer. Hence, requesting 800Hz will produce 1kHz. - -## Output - -On launch, the node will configure the IMU according to the parameters and then enable streaming node. At least three topics are published: - -* `/imu_3dm_gx4/imu`: An instance of `sensor_msgs/Imu`. Orientation quaternion not provided in this message since is already part of the filter message. -* `/imu_3dm_gx4/magnetic_field`: An instance of `sensor_msgs/MagneticField`. -* `/imu_3dm_gx4/pressure`: An instance of `sensor_msgs/FluidPressure`. - -All of the above topics are published with synchronized timestamps. - -Additional topics will be published if `enable_filter` is true: - -* `/imu_3dm_gx4/filter`: An instance of `imu_3dm_gx4/FilterOuput`. Custom message indicating all the onboard filter outputs. - -## Known Issues - -* Even when the `enable_mag_update` option is set to false (and the device acknowledges the setting with a positive ACK), the `quat_status` field is received as 3. This has not been fully debugged yet. - -## Specifications - -* Specifications can be found at [3DM-GX4-25](http://www.microstrain.com/inertial/3dm-gx4-25). - -We provide YAML file to work with [Kalibr](https://github.com/ethz-asl/kalibr), which can be found in the `calib` folder. - -## FAQs - -1. What data rates can I use? -The driver supports up to 1000Hz for IMU data, and up to 500Hz for filter data. For high data rates, it is recommended that you use a baudrate of 921600. - -2. The driver can't open my device, even though the path is specified correctly - what gives?? -Make sure you have ownership of the device in `/dev`, or are part of the dialout group. diff --git a/thormang3_imu_3dm_gx4/calib/imu_3dm_gx4_25.yaml b/thormang3_imu_3dm_gx4/calib/imu_3dm_gx4_25.yaml deleted file mode 100644 index 41e7916c..00000000 --- a/thormang3_imu_3dm_gx4/calib/imu_3dm_gx4_25.yaml +++ /dev/null @@ -1,11 +0,0 @@ -#Accelerometers -accelerometer_noise_density: 1.0e-03 #Noise density (continuous-time) -accelerometer_random_walk: 0.039e-02 #Bias random walk - -#Gyroscopes -gyroscope_noise_density: 8.73e-05 #Noise density (continuous-time) -gyroscope_random_walk: 4.8e-05 #Bias random walk - -update_rate: 200.0 #Hz (for discretization of the values above) -rostopic: /imu_3dm_gx4/imu - diff --git a/thormang3_imu_3dm_gx4/launch/imu.launch b/thormang3_imu_3dm_gx4/launch/imu.launch deleted file mode 100644 index ed3bf8e8..00000000 --- a/thormang3_imu_3dm_gx4/launch/imu.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/thormang3_imu_3dm_gx4/msg/FilterOutput.msg b/thormang3_imu_3dm_gx4/msg/FilterOutput.msg deleted file mode 100644 index c70eda1c..00000000 --- a/thormang3_imu_3dm_gx4/msg/FilterOutput.msg +++ /dev/null @@ -1,33 +0,0 @@ -# Output from the 3DM-GX4 attitude estimation filter. -std_msgs/Header header - -# Node on status flags: -# Status flags are implemented as bit-fields. -# 0 = invalid -# 1 = valid -# 2 = valid and geo-magnetically referenced (quat_status only) -# -# Note that covariance on orientation becomes invalid as pitch angle exceeds 70 # degrees. This will be indicated by the status flag. - -# Quaternion status: -uint16 quat_status - -# Bias status -uint16 bias_status - -# Orientation estimate and diagonal covariance -geometry_msgs/Quaternion orientation -float64[9] orientation_covariance - -# Gyro bias and diagonal covariance -geometry_msgs/Vector3 bias -float64[9] bias_covariance - -# Covariance statuses -uint16 bias_covariance_status -uint16 orientation_covariance_status - -# Constants -uint16 STATUS_INVALID = 0 -uint16 STATUS_VALID = 1 -uint16 STATUS_VALID_REFERENCED = 2 diff --git a/thormang3_imu_3dm_gx4/package.xml b/thormang3_imu_3dm_gx4/package.xml deleted file mode 100644 index 42391adc..00000000 --- a/thormang3_imu_3dm_gx4/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - thormang3_imu_3dm_gx4 - 0.1.3 - - Driver for Microstrain 3DM-GX4-25 IMU - This package is modified by robotis. - The original version is Kumar Robotics's imu_3dm_gx4 package. - - Apache 2 - Gareth Cross - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC - http://wiki.ros.org/thormang3_imu_3dm_gx4 - catkin - message_generation - message_runtime - diagnostic_updater - geometry_msgs - roscpp - sensor_msgs - diff --git a/thormang3_imu_3dm_gx4/script/README.md b/thormang3_imu_3dm_gx4/script/README.md deleted file mode 100644 index c47fe02c..00000000 --- a/thormang3_imu_3dm_gx4/script/README.md +++ /dev/null @@ -1,6 +0,0 @@ -This script adds a udev rule to set the permissions correctly when the imu is plugged in. - -Run the following: -sudo ./add_rule - -Enter y at the prompt diff --git a/thormang3_imu_3dm_gx4/script/add_rule b/thormang3_imu_3dm_gx4/script/add_rule deleted file mode 100644 index b15da6ab..00000000 --- a/thormang3_imu_3dm_gx4/script/add_rule +++ /dev/null @@ -1,23 +0,0 @@ -#!/bin/bash - - -if [[ $UID != 0 ]]; then - echo "Please start the script as root or sudo!" - exit 1 -fi - -read -p "This will add $SUDO_USER to group imu are you sure? y/n: " -n 1 -r -echo #newline -if [[ $REPLY =~ ^[Yy]$ ]] -then - # add group and user - groupadd imu - adduser $SUDO_USER imu - - # addudev rule - DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # get directory where this script is stored - cp $DIR/imu_3dm_gx4.rules /etc/udev/rules.d/ - - #reload udev - udevadm control --reload-rules -fi diff --git a/thormang3_imu_3dm_gx4/script/imu_3dm_gx4.rules b/thormang3_imu_3dm_gx4/script/imu_3dm_gx4.rules deleted file mode 100644 index 4daf61c1..00000000 --- a/thormang3_imu_3dm_gx4/script/imu_3dm_gx4.rules +++ /dev/null @@ -1 +0,0 @@ -SUBSYSTEMS=="usb", ATTRS{manufacturer}=="Lord Microstrain",ATTRS{product}=="Lord Inertial Sensor", MODE="0777", GROUP="imu" diff --git a/thormang3_imu_3dm_gx4/src/imu.cpp b/thormang3_imu_3dm_gx4/src/imu.cpp deleted file mode 100644 index dba57dfc..00000000 --- a/thormang3_imu_3dm_gx4/src/imu.cpp +++ /dev/null @@ -1,1221 +0,0 @@ -/* - * imu.hpp - * - * Copyright (c) 2014 Kumar Robotics. All rights reserved. - * - * This file is part of galt. - * - * Created on: 2/6/2014 - * Author: gareth - */ - -#include "imu.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -extern "C" { -#include -#include -#include -#include -#include -#include -#include -#include -#include // close -#include // strerror -} - -#define kDefaultTimeout (300) -#define kBufferSize (10) // keep this small, or 1000Hz is not attainable - -#define u8(x) static_cast((x)) - -#define COMMAND_CLASS_BASE u8(0x01) -#define COMMAND_CLASS_3DM u8(0x0C) -#define COMMAND_CLASS_FILTER u8(0x0D) - -#define DATA_CLASS_IMU u8(0x80) -#define DATA_CLASS_FILTER u8(0x82) - -#define FUNCTION_APPLY u8(0x01) - -#define SELECTOR_IMU u8(0x01) -#define SELECTOR_FILTER u8(0x03) - -// base commands -#define DEVICE_PING u8(0x01) -#define DEVICE_IDLE u8(0x02) -#define DEVICE_RESUME u8(0x06) - -// 3DM and FILTER commands -#define COMMAND_GET_DEVICE_INFO u8(0x03) -#define COMMAND_GET_IMU_BASE_RATE u8(0x06) -#define COMMAND_GET_FILTER_BASE_RATE u8(0x0B) -#define COMMAND_IMU_MESSAGE_FORMAT u8(0x08) -#define COMAMND_FILTER_MESSAGE_FORMAT u8(0x0A) -#define COMMAND_ENABLE_DATA_STREAM u8(0x11) -#define COMMAND_FILTER_CONTROL_FLAGS u8(0x14) -#define COMMAND_UART_BAUD_RATE u8(0x40) -#define COMMAND_SET_HARD_IRON u8(0x3A) -#define COMMAND_SET_SOFT_IRON u8(0x3B) -#define COMMAND_ENABLE_MEASUREMENTS u8(0x41) -#define COMMAND_DEVICE_STATUS u8(0x64) - -// supported fields -#define FIELD_QUATERNION u8(0x03) -#define FIELD_ACCELEROMETER u8(0x04) -#define FIELD_GYROSCOPE u8(0x05) -#define FIELD_GYRO_BIAS u8(0x06) -#define FIELD_MAGNETOMETER u8(0x06) -#define FIELD_ANGLE_UNCERTAINTY u8(0x0A) -#define FIELD_BIAS_UNCERTAINTY u8(0x0B) -#define FIELD_BAROMETER u8(0x17) -#define FIELD_DEVICE_INFO u8(0x81) -#define FIELD_IMU_BASERATE u8(0x83) -#define FIELD_FILTER_BASERATE u8(0x8A) -#define FIELD_STATUS_REPORT u8(0x90) -#define FIELD_ACK_OR_NACK u8(0xF1) - -using namespace imu_3dm_gx4; - -// trim from start -static inline std::string ltrim(std::string s) { - s.erase(s.begin(), - std::find_if(s.begin(), s.end(), - std::not1(std::ptr_fun(std::isspace)))); - return s; -} - -// swap order of bytes if on a little endian platform -template void to_device_order(uint8_t buffer[sz]) { -#ifdef HOST_LITTLE_ENDIAN - for (size_t i = 0; i < sz / 2; i++) { - std::swap(buffer[i], buffer[sz - i - 1]); - } -#endif -} - -template <> void to_device_order<1>(uint8_t buffer[1]) {} - -template size_t encode(uint8_t *buffer, const T &t) { - static_assert(std::is_fundamental::value, - "Only fundamental types may be encoded"); - - const size_t szT = sizeof(T); - union { - T tc; - uint8_t bytes[szT]; - }; - tc = t; - - to_device_order(bytes); - - // append - for (size_t i = 0; i < szT; i++) { - buffer[i] = bytes[i]; - } - - return szT; -} - -template -size_t encode(uint8_t *buffer, const T &t, const Ts &... ts) { - const size_t sz = encode(buffer, t); - // recurse - return sz + encode(buffer + sizeof(T), ts...); -} - -template void decode(const uint8_t *buffer, size_t count, - T *output) { - const size_t szT = sizeof(T); - static_assert(std::is_fundamental::value, - "Only fundamental types may be decoded"); - union { - T tc; - uint8_t bytes[szT]; - }; - - for (size_t i = 0; i < count; i++) { - for (size_t j = 0; j < szT; j++) { - bytes[j] = buffer[j]; - } - to_device_order(&bytes[0]); - output[i] = tc; - - buffer += szT; - } -} - -/** - * @brief Tool for encoding packets more easily by simply appending fields. - */ -class PacketEncoder { -public: - PacketEncoder(Imu::Packet& p) : p_(p), fs_(0), enc_(false) { - p.length = 0; - } - virtual ~PacketEncoder() { - // no destruction without completion - assert(!enc_); - } - - void beginField(uint8_t desc) { - assert(!enc_); - assert(p_.length < sizeof(p_.payload) - 2); // 2 bytes per field minimum - fs_ = p_.length; - p_.payload[fs_+1] = desc; - p_.length += 2; - enc_ = true; - } - - template - void append(const Ts& ...args) { - assert(enc_); // todo: check argument length here - p_.length += encode(p_.payload+p_.length, args...); - } - - void endField() { - assert(enc_); - p_.payload[fs_] = p_.length - fs_; - enc_ = false; - } - -private: - Imu::Packet& p_; - uint8_t fs_; - bool enc_; -}; - -/** - * @brief Tool for decoding packets by iterating through fields. - */ -class PacketDecoder { -public: - PacketDecoder(const Imu::Packet& p) : p_(p), fs_(0), pos_(2) { - assert(p.length > 0); - } - - int fieldDescriptor() const { - if (fs_ > sizeof(p_.payload)-2) { - return -1; - } - if (p_.payload[fs_] == 0) { - return -1; // no field - } - return p_.payload[fs_ + 1]; // descriptor after length - } - - int fieldLength() const { - assert(fs_ < sizeof(p_.payload)); - return p_.payload[fs_]; - } - - bool fieldIsAckOrNack() const { - const int desc = fieldDescriptor(); - if (desc == static_cast(FIELD_ACK_OR_NACK)) { - return true; - } - return false; - } - - bool advanceTo(uint8_t field) { - for (int d; (d = fieldDescriptor()) > 0; advance()) { - if (d == static_cast(field)) { - return true; - } - } - return false; - } - - void advance() { - fs_ += p_.payload[fs_]; - pos_ = 2; // skip length and descriptor - } - - template - void extract(size_t count, T* output) { - BOOST_VERIFY(fs_ + pos_ + sizeof(T) * count <= sizeof(p_.payload)); - decode(&p_.payload[fs_ + pos_], count, output); - pos_ += sizeof(T) * count; - } - -private: - const Imu::Packet& p_; - uint8_t fs_; - uint8_t pos_; -}; - -bool Imu::Packet::isIMUData() const { - return descriptor == DATA_CLASS_IMU; -} - -bool Imu::Packet::isFilterData() const { - return descriptor == DATA_CLASS_FILTER; -} - -int Imu::Packet::ackErrorCodeFor(const Packet &command) const { - PacketDecoder decoder(*this); - const uint8_t sentDesc = command.descriptor; - const uint8_t sentField = command.payload[1]; - - if (sentDesc != this->descriptor) { - // not for this packet - return -1; - } - - // look for a matching ACK - for (int d; (d = decoder.fieldDescriptor()) > 0; decoder.advance()) { - if (decoder.fieldIsAckOrNack()) { - uint8_t cmd, code; - decoder.extract(1, &cmd); - decoder.extract(1, &code); - if (cmd == sentField) { - // match - return code; - } - } - } - // not an ACK/NACK - return -1; -} - -void Imu::Packet::calcChecksum() { - uint8_t byte1 = 0, byte2 = 0; - -#define add_byte(x) \ - byte1 += (x); \ - byte2 += byte1; - - add_byte(syncMSB); - add_byte(syncLSB); - add_byte(descriptor); - add_byte(length); - - for (size_t i = 0; i < length; i++) { - add_byte(payload[i]); - } -#undef add_byte - - checksum = (static_cast(byte1) << 8) + static_cast(byte2); -#ifdef HOST_LITTLE_ENDIAN - uint8_t temp = checkLSB; - checkLSB = checkMSB; - checkMSB = temp; -#endif -} - -Imu::Packet::Packet(uint8_t desc) -: syncMSB(kSyncMSB), syncLSB(kSyncLSB), descriptor(desc), length(0), - checksum(0) { - memset(&payload, 0, sizeof(payload)); -} - -std::string Imu::Packet::toString() const { - std::stringstream ss; - ss << std::hex; - ss << "SyncMSB: " << static_cast(syncMSB) << "\n"; - ss << "SyncLSB: " << static_cast(syncLSB) << "\n"; - ss << "Descriptor: " << static_cast(descriptor) << "\n"; - ss << "Length: " << static_cast(length) << "\n"; - ss << "Payload: "; - for (size_t s=0; s < length; s++) { - ss << static_cast(payload[s]) << " "; - } - ss << "\nCheck MSB: " << static_cast(checkMSB) << "\n"; - ss << "Check LSB: " << static_cast(checkLSB); - return ss.str(); -} - -std::map Imu::Info::toMap() const { - std::map map; - map["Firmware version"] = std::to_string(firmwareVersion); - map["Model name"] = modelName; - map["Model number"] = modelNumber; - map["Serial number"] = serialNumber; - map["Device options"] = deviceOptions; - // omit lot number since it is empty - return map; -} - -std::map Imu::DiagnosticFields::toMap() const { - std::map map; - map["Model number"] = modelNumber; - map["Selector"] = selector; - map["Status flags"] = statusFlags; - map["System timer"] = systemTimer; - map["Num 1PPS Pulses"] = num1PPSPulses; - map["Last 1PPS Pulse"] = last1PPSPulse; - map["Imu stream enabled"] = imuStreamEnabled; - map["Filter stream enabled"] = filterStreamEnabled; - map["Imu packets dropped"] = imuPacketsDropped; - map["Filter packets dropped"] = filterPacketsDropped; - map["Com bytes written"] = comBytesWritten; - map["Com bytes read"] = comBytesRead; - map["Com num write overruns"] = comNumReadOverruns; - map["Com num read overruns"] = comNumReadOverruns; - map["Usb bytes written"] = usbBytesWritten; - map["Usb bytes read"] = usbBytesRead; - map["Usb num write overruns"] = usbNumWriteOverruns; - map["Usb num read overruns"] = usbNumReadOverruns; - map["Num imu parse errors"] = numIMUParseErrors; - map["Total imu messages"] = totalIMUMessages; - map["Last imu message"] = lastIMUMessage; - return map; -} - -Imu::command_error::command_error(const Packet& p, uint8_t code) : - std::runtime_error(generateString(p, code)) {} - -std::string Imu::command_error::generateString(const Packet& p, uint8_t code) { - std::stringstream ss; - ss << "Received NACK with error code " << std::hex << static_cast(code); - ss << ". Command Packet:\n" << p.toString(); - return ss.str(); -} - -Imu::timeout_error::timeout_error(bool write, unsigned int to) -: std::runtime_error(generateString(write,to)) {} - -std::string Imu::timeout_error::generateString(bool write, - unsigned int to) { - std::stringstream ss; - ss << "Timed-out while " << ((write) ? "writing" : "reading") << ". "; - ss << "Time-out limit is " << to << "ms."; - return ss.str(); -} - -Imu::Imu(const std::string &device, bool verbose) : device_(device), verbose_(verbose), - fd_(0), - rwTimeout_(kDefaultTimeout), - srcIndex_(0), dstIndex_(0), - state_(Idle) { - // buffer for storing reads - buffer_.resize(kBufferSize); -} - -Imu::~Imu() { disconnect(); } - -void Imu::connect() { - if (fd_ > 0) { - throw std::runtime_error("Socket is already open"); - } - - const char *path = device_.c_str(); - fd_ = ::open(path, O_RDWR | O_NOCTTY | O_NDELAY); // read/write, no - // controlling terminal, non - // blocking access - if (fd_ < 0) { - throw std::runtime_error("Failed to open device : " + device_); - } - - // make sure it is non-blocking - if (fcntl(fd_, F_SETFL, FNONBLOCK) < 0) { - disconnect(); - throw io_error(strerror(errno)); - } - - struct termios toptions; - if (tcgetattr(fd_, &toptions) < 0) { - disconnect(); - throw io_error(strerror(errno)); - } - - // set default baud rate - cfsetispeed(&toptions, B115200); - cfsetospeed(&toptions, B115200); - - toptions.c_cflag &= ~PARENB; // no parity bit - toptions.c_cflag &= ~CSTOPB; // disable 2 stop bits (only one used instead) - toptions.c_cflag &= ~CSIZE; // disable any previous size settings - toptions.c_cflag |= HUPCL; // enable modem disconnect - toptions.c_cflag |= CS8; // bytes are 8 bits long - - toptions.c_cflag &= ~CRTSCTS; // no hardware RTS/CTS switch - - toptions.c_cflag |= - CREAD | - CLOCAL; // enable receiving of data, forbid control lines (CLOCAL) - toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // no software flow control - toptions.c_iflag &= ~(INLCR|ICRNL); // disable NL->CR and CR->NL - - // disable the following - // ICANON = input is read on a per-line basis - // ECHO/ECHOE = echo enabled - // ISIG = interrupt signals - toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); - toptions.c_oflag &= ~OPOST; // disable pre-processing of input data - toptions.c_oflag &= ~(ONLCR|OCRNL); // disable NL->CR and CR->NL - - toptions.c_cc[VMIN] = 0; // no minimum number of bytes when reading - toptions.c_cc[VTIME] = 0; // no time blocking - - // TCSAFLUSH = make change after flushing i/o buffers - if (tcsetattr(fd_, TCSAFLUSH, &toptions) < 0) { - disconnect(); - throw io_error(strerror(errno)); - } -} - -void Imu::disconnect() { - if (fd_ > 0) { - // send the idle command first - idle(false); // we don't care about reply here - close(fd_); - } - fd_ = 0; -} - -bool Imu::termiosBaudRate(unsigned int baud) { - struct termios toptions; - if (tcgetattr(fd_, &toptions) < 0) { - return false; - } - - speed_t speed; - switch (baud) { - case 9600: - speed = B9600; - break; - case 19200: - speed = B19200; - break; - case 115200: - speed = B115200; - break; - case 230400: - speed = B230400; - break; - case 460800: - speed = B460800; - break; - case 921600: - speed = B921600; - break; - default: - throw std::invalid_argument("Invalid Baud Rate" ); - } - - // modify only the baud rate - cfsetispeed(&toptions, speed); - cfsetospeed(&toptions, speed); - - if (tcsetattr(fd_, TCSAFLUSH, &toptions) < 0) { - return false; - } - - usleep(200000); // wait for connection to be negotiated - // found this length through experimentation - return true; -} - -void Imu::runOnce() { - int sig = pollInput(5); - if (sig < 0) { - // failure in poll/read, device disconnected - throw io_error(strerror(errno)); - } -} - -void Imu::selectBaudRate(unsigned int baud) { - // baud rates supported by the 3DM-GX4-25 - const size_t num_rates = 6; - unsigned int rates[num_rates] = { - 9600, 19200, 115200, 230400, 460800, 921600 - }; - - if (!std::binary_search(rates, rates + num_rates, baud)) { - // invalid baud rate - std::stringstream ss; - ss << "Baud rate unsupported: " << baud; - throw std::invalid_argument(ss.str()); - } - - Imu::Packet pp(COMMAND_CLASS_BASE); // was 0x02 - { - PacketEncoder encoder(pp); - encoder.beginField(DEVICE_PING); - encoder.endField(); - } - pp.calcChecksum(); - - size_t i; - bool foundRate = false; - for (i = 0; i < num_rates; i++) { - if (verbose_){ - std::cout << "Switching to baud rate " << rates[i] << std::endl; - } - if (!termiosBaudRate(rates[i])) { - throw io_error(strerror(errno)); - } - - if (verbose_) { - std::cout << "Switched baud rate to " << rates[i] << std::endl; - std::cout << "Sending a ping packet.\n" << std::flush; - } - - // send ping and wait for first response - sendPacket(pp, 100); - try { - receiveResponse(pp, 500); - } catch (timeout_error&) { - if (verbose_) { - std::cout << "Timed out waiting for ping response.\n" << std::flush; - } - continue; - } catch (command_error&) { - if (verbose_) { - std::cout << "IMU returned error code for ping.\n" << std::flush; - } - continue; - } // do not catch io_error - - if (verbose_) { - std::cout << "Found correct baudrate.\n" << std::flush; - } - - // no error in receiveResponse, this is correct baud rate - foundRate = true; - break; - } - - if (!foundRate) { - throw std::runtime_error("Failed to reach device " + device_); - } - - // we are on the correct baud rate, now change to the new rate - Packet comm(COMMAND_CLASS_3DM); // was 0x07 - { - PacketEncoder encoder(comm); - encoder.beginField(COMMAND_UART_BAUD_RATE); - encoder.append(FUNCTION_APPLY); - encoder.append(static_cast(baud)); - encoder.endField(); - assert(comm.length == 0x07); - } - comm.calcChecksum(); - - try { - if (verbose_) { - std::cout << "Instructing device to change to " << baud << std::endl - << std::flush; - } - sendCommand(comm); - } catch (std::exception& e) { - std::stringstream ss; - ss << "Device rejected baud rate " << baud << ".\n"; - ss << e.what(); - throw std::runtime_error(ss.str()); - } - - // device has switched baud rate, now we should also - if (!termiosBaudRate(baud)) { - throw io_error(strerror(errno)); - } - - // ping - try { - ping(); - } catch (std::exception& e) { - std::string err("Device did not respond to ping.\n"); - err += e.what(); - throw std::runtime_error(err); - } -} - -void Imu::ping() { - Imu::Packet p(COMMAND_CLASS_BASE); // was 0x02 - PacketEncoder encoder(p); - encoder.beginField(DEVICE_PING); - encoder.endField(); - p.calcChecksum(); - assert(p.checkMSB == 0xE0 && p.checkLSB == 0xC6); - sendCommand(p); -} - -void Imu::idle(bool needReply) { - Imu::Packet p(COMMAND_CLASS_BASE); // was 0x02 - PacketEncoder encoder(p); - encoder.beginField(DEVICE_IDLE); - encoder.endField(); - p.calcChecksum(); - assert(p.checkMSB == 0xE1 && p.checkLSB == 0xC7); - sendCommand(p, needReply); -} - -void Imu::resume() { - Imu::Packet p(COMMAND_CLASS_BASE); // was 0x02 - PacketEncoder encoder(p); - encoder.beginField(DEVICE_RESUME); - encoder.endField(); - p.calcChecksum(); - assert(p.checkMSB == 0xE5 && p.checkLSB == 0xCB); - sendCommand(p); -} - -void Imu::getDeviceInfo(Imu::Info &info) { - Imu::Packet p(COMMAND_CLASS_BASE); // was 0x02 - PacketEncoder encoder(p); - encoder.beginField(COMMAND_GET_DEVICE_INFO); - encoder.endField(); - p.calcChecksum(); - assert(p.checkMSB == 0xE2 && p.checkLSB == 0xC8); - - sendCommand(p); - { - PacketDecoder decoder(packet_); - BOOST_VERIFY(decoder.advanceTo(FIELD_DEVICE_INFO)); - char buffer[16]; - - decoder.extract(1, &info.firmwareVersion); - // decode all strings and trim left whitespace - decoder.extract(sizeof(buffer), &buffer[0]); - info.modelName = ltrim(std::string(buffer,16)); - decoder.extract(sizeof(buffer), &buffer[0]); - info.modelNumber = ltrim(std::string(buffer,16)); - decoder.extract(sizeof(buffer), &buffer[0]); - info.serialNumber = ltrim(std::string(buffer,16)); - decoder.extract(sizeof(buffer), &buffer[0]); - info.lotNumber = ltrim(std::string(buffer,16)); - decoder.extract(sizeof(buffer), &buffer[0]); - info.deviceOptions = ltrim(std::string(buffer,16)); - } -} - -void Imu::getIMUDataBaseRate(uint16_t &baseRate) { - Packet p(COMMAND_CLASS_3DM); // was 0x02 - PacketEncoder encoder(p); - encoder.beginField(COMMAND_GET_IMU_BASE_RATE); - encoder.endField(); - p.calcChecksum(); - - sendCommand(p); - { - PacketDecoder decoder(packet_); - BOOST_VERIFY(decoder.advanceTo(FIELD_IMU_BASERATE)); - decoder.extract(1, &baseRate); - } -} - -void Imu::getFilterDataBaseRate(uint16_t &baseRate) { - Packet p(COMMAND_CLASS_3DM); // was 0x02 - PacketEncoder encoder(p); - encoder.beginField(COMMAND_GET_FILTER_BASE_RATE); - encoder.endField(); - p.calcChecksum(); - - sendCommand(p); - decode(&packet_.payload[6], 1, &baseRate); -} - -void Imu::getDiagnosticInfo(Imu::DiagnosticFields &fields) { - Packet p(COMMAND_CLASS_3DM); - PacketEncoder encoder(p); - encoder.beginField(COMMAND_DEVICE_STATUS); - encoder.append(static_cast(6234)); // device model number - encoder.append(u8(0x02)); // diagnostic mode - encoder.endField(); - p.calcChecksum(); - - sendCommand(p); - { - PacketDecoder decoder(packet_); - BOOST_VERIFY(decoder.advanceTo(FIELD_STATUS_REPORT)); - - decoder.extract(1, &fields.modelNumber); - decoder.extract(1, &fields.selector); - decoder.extract(4, &fields.statusFlags); - decoder.extract(2, &fields.imuStreamEnabled); - decoder.extract(13, &fields.imuPacketsDropped); - } -} - -void Imu::setIMUDataRate(uint16_t decimation, - const std::bitset<4>& sources) { - Imu::Packet p(COMMAND_CLASS_3DM); // was 0x04 - PacketEncoder encoder(p); - - // valid field descriptors: accel, gyro, mag, pressure - static const uint8_t fieldDescs[] = { FIELD_ACCELEROMETER, - FIELD_GYROSCOPE, - FIELD_MAGNETOMETER, - FIELD_BAROMETER }; - assert(sizeof(fieldDescs) == sources.size()); - std::vector fields; - - for (size_t i=0; i < sources.size(); i++) { - if (sources[i]) { - fields.push_back(fieldDescs[i]); - } - } - encoder.beginField(COMMAND_IMU_MESSAGE_FORMAT); - encoder.append(FUNCTION_APPLY, u8(fields.size())); - - for (const uint8_t& field : fields) { - encoder.append(field, decimation); - } - - encoder.endField(); - p.calcChecksum(); - sendCommand(p); -} - -void Imu::setFilterDataRate(uint16_t decimation, const std::bitset<4>& sources) { - Imu::Packet p(COMMAND_CLASS_3DM); // was 0x04 - PacketEncoder encoder(p); - - static const uint8_t fieldDescs[] = { FIELD_QUATERNION, - FIELD_GYRO_BIAS, - FIELD_ANGLE_UNCERTAINTY, - FIELD_BIAS_UNCERTAINTY }; - assert(sizeof(fieldDescs) == sources.size()); - std::vector fields; - - for (size_t i=0; i < sources.size(); i++) { - if (sources[i]) { - fields.push_back(fieldDescs[i]); - } - } - - encoder.beginField(COMAMND_FILTER_MESSAGE_FORMAT); - encoder.append(FUNCTION_APPLY, u8(fields.size())); - - for (const uint8_t& field : fields) { - encoder.append(field, decimation); - } - encoder.endField(); - p.calcChecksum(); - sendCommand(p); -} - -void Imu::enableMeasurements(bool accel, bool magnetometer) { - Imu::Packet p(COMMAND_CLASS_FILTER); - PacketEncoder encoder(p); - encoder.beginField(COMMAND_ENABLE_MEASUREMENTS); - uint16_t flag=0; - if (accel) { - flag |= 0x01; - } - if (magnetometer) { - flag |= 0x02; - } - encoder.append(FUNCTION_APPLY, flag); - encoder.endField(); - p.calcChecksum(); - sendCommand(p); -} - -void Imu::enableBiasEstimation(bool enabled) { - Imu::Packet p(COMMAND_CLASS_FILTER); - PacketEncoder encoder(p); - encoder.beginField(COMMAND_FILTER_CONTROL_FLAGS); - uint16_t flag = 0xFFFE; - if (enabled) { - flag = 0xFFFF; - } - encoder.append(FUNCTION_APPLY, flag); - encoder.endField(); - p.calcChecksum(); - sendCommand(p); -} - -void Imu::setHardIronOffset(float offset[3]) { - Imu::Packet p(COMMAND_CLASS_3DM); - PacketEncoder encoder(p); - encoder.beginField(COMMAND_SET_HARD_IRON); - encoder.append(FUNCTION_APPLY, offset[0], offset[1], offset[2]); - encoder.endField(); - assert(p.length == 0x0F); - p.calcChecksum(); - sendCommand(p); -} - -void Imu::setSoftIronMatrix(float matrix[9]) { - Imu::Packet p(COMMAND_CLASS_3DM); - PacketEncoder encoder(p); - encoder.beginField(COMMAND_SET_SOFT_IRON); - encoder.append(FUNCTION_APPLY); - for (int i=0; i < 9; i++) { - encoder.append(matrix[i]); - } - encoder.endField(); - assert(p.length == 0x27); - p.calcChecksum(); - sendCommand(p); -} - -void Imu::enableIMUStream(bool enabled) { - Packet p(COMMAND_CLASS_3DM); - PacketEncoder encoder(p); - encoder.beginField(COMMAND_ENABLE_DATA_STREAM); - encoder.append(FUNCTION_APPLY, SELECTOR_IMU); - encoder.append(u8(enabled)); - encoder.endField(); - p.calcChecksum(); - if (enabled) { - assert(p.checkMSB == 0x04 && p.checkLSB == 0x1A); - } - sendCommand(p); -} - -void Imu::enableFilterStream(bool enabled) { - Packet p(COMMAND_CLASS_3DM); - PacketEncoder encoder(p); - encoder.beginField(COMMAND_ENABLE_DATA_STREAM); - encoder.append(FUNCTION_APPLY, SELECTOR_FILTER); - encoder.append(u8(enabled)); - encoder.endField(); - p.calcChecksum(); - if (enabled) { - assert(p.checkMSB == 0x06 && p.checkLSB == 0x1E); - } - sendCommand(p); -} - -void -Imu::setIMUDataCallback(const std::function &cb) { - imuDataCallback_ = cb; -} - -void Imu::setFilterDataCallback( - const std::function &cb) { - filterDataCallback_ = cb; -} - -int Imu::pollInput(unsigned int to) { - // poll socket for inputs - struct pollfd p; - p.fd = fd_; - p.events = POLLIN; - - int rPoll = poll(&p, 1, to); // timeout is in millis - if (rPoll > 0) { - const ssize_t amt = ::read(fd_, &buffer_[0], buffer_.size()); - if (amt > 0) { - return handleRead(amt); - } else if (amt == 0) { - // end-of-file, device disconnected - return -1; - } - } else if (rPoll == 0) { - return 0; // no socket can be read - } - - if (errno == EAGAIN || errno == EINTR) { - // treat these like timeout errors - return 0; - } - - // poll() or read() failed - return -1; -} - -/** - * @note handleByte will interpret a single byte. It will return the number - * of bytes which can be cleared from the queue. On early failure, this will - * always be 1 - ie. kSyncMSB should be cleared from the queue. On success, the - * total length of the valid packet should be cleared. - */ -std::size_t Imu::handleByte(const uint8_t& byte, bool& found) { - found = false; - if (state_ == Idle) { - // reset dstIndex_ to start of packet - dstIndex_ = 0; - if (byte == Imu::Packet::kSyncMSB) { - packet_.syncMSB = byte; - state_ = Reading; - // clear previous packet out - memset(&packet_.payload[0], 0, sizeof(packet_.payload)); - } else { - // byte is no good, stay in idle - return 1; - } - } - else if (state_ == Reading) { - const size_t end = Packet::kHeaderLength + packet_.length; - // fill out fields of packet structure - if (dstIndex_ == 1) { - if (byte != Imu::Packet::kSyncLSB) { - // not a true header, throw away and go back to idle - state_ = Idle; - return 1; - } - packet_.syncLSB = byte; - } - else if (dstIndex_ == 2) { - packet_.descriptor = byte; - } - else if (dstIndex_ == 3) { - packet_.length = byte; - } - else if (dstIndex_ < end) { - packet_.payload[dstIndex_ - Packet::kHeaderLength] = byte; - } - else if (dstIndex_ == end) { - packet_.checkMSB = byte; - } - else if (dstIndex_ == end + 1) { - state_ = Idle; // finished packet - packet_.checkLSB = byte; - - // check checksum - const uint16_t sum = packet_.checksum; - packet_.calcChecksum(); - - if (sum != packet_.checksum) { - // invalid, go back to waiting for a marker in the stream - std::cout << "Warning: Dropped packet with mismatched checksum\n" - << std::flush; - if (verbose_) { - std::cout << "Expected " << std::hex << - static_cast(packet_.checksum) << " but received " << - static_cast(sum) << std::endl; - std::cout << "Packet content:\n" << packet_.toString() << std::endl; - std::cout << "Queue content: \n"; - for (const uint8_t& q : queue_) { - std::cout << static_cast(q) << " "; - } - std::cout << "\n" << std::flush; - } - return 1; - } else { - // successfully read a packet - processPacket(); - found = true; - return end+2; - } - } - } - - // advance to next byte in packet - dstIndex_++; - return 0; -} - -// parses packets out of the input buffer -int Imu::handleRead(size_t bytes_transferred) { - // read data into queue - std::stringstream ss; - ss << "Handling read : " << std::hex; - for (size_t i = 0; i < bytes_transferred; i++) { - queue_.push_back(buffer_[i]); - ss << static_cast(buffer_[i]) << " "; - } - ss << std::endl; - if (verbose_) { - std::cout << ss.str() << std::flush; - } - - bool found = false; - while (srcIndex_ < queue_.size() && !found) { - const uint8_t head = queue_[srcIndex_]; - const size_t clear = handleByte(head, found); - // pop 'clear' bytes from the queue - for (size_t i=0; i < clear; i++) { - queue_.pop_front(); - } - if (clear) { - // queue was shortened, return to head - srcIndex_=0; - } else { - // continue up the queue - srcIndex_++; - } - } - - // no packet - return found; -} - -void Imu::processPacket() { - IMUData data; - FilterData filterData; - PacketDecoder decoder(packet_); - - if (packet_.isIMUData()) { - // process all fields in the packet - for (int d; (d = decoder.fieldDescriptor()) > 0; decoder.advance()) { - switch (u8(d)) { - case FIELD_ACCELEROMETER: - decoder.extract(3, &data.accel[0]); - data.fields |= IMUData::Accelerometer; - break; - case FIELD_GYROSCOPE: - decoder.extract(3, &data.gyro[0]); - data.fields |= IMUData::Gyroscope; - break; - case FIELD_MAGNETOMETER: - decoder.extract(3, &data.mag[0]); - data.fields |= IMUData::Magnetometer; - break; - case FIELD_BAROMETER: - decoder.extract(1, &data.pressure); - data.fields |= IMUData::Barometer; - break; - default: - std::stringstream ss; - ss << "Unsupported field in IMU packet: " << std::hex << d; - throw std::runtime_error(ss.str()); - break; - } - } - - if (imuDataCallback_) { - imuDataCallback_(data); - } - } else if (packet_.isFilterData()) { - for (int d; (d = decoder.fieldDescriptor()) > 0; decoder.advance()) { - switch (u8(d)) { - case FIELD_QUATERNION: - decoder.extract(4, &filterData.quaternion[0]); - decoder.extract(1, &filterData.quaternionStatus); - filterData.fields |= FilterData::Quaternion; - break; - case FIELD_GYRO_BIAS: - decoder.extract(3, &filterData.bias[0]); - decoder.extract(1, &filterData.biasStatus); - filterData.fields |= FilterData::Bias; - break; - case FIELD_ANGLE_UNCERTAINTY: - decoder.extract(3, &filterData.angleUncertainty[0]); - decoder.extract(1, &filterData.angleUncertaintyStatus); - filterData.fields |= FilterData::AngleUnertainty; - break; - case FIELD_BIAS_UNCERTAINTY: - decoder.extract(3, &filterData.biasUncertainty[0]); - decoder.extract(1, &filterData.biasUncertaintyStatus); - filterData.fields |= FilterData::BiasUncertainty; - break; - default: - std::stringstream ss; - ss << "Unsupported field in filter packet: " << std::hex << d; - throw std::runtime_error(ss.str()); - break; - } - } - - if (filterDataCallback_) { - filterDataCallback_(filterData); - } - } else { - // find any NACK fields and log them - for (int d; (d = decoder.fieldDescriptor()) > 0; decoder.advance()) { - if (decoder.fieldIsAckOrNack()) { - uint8_t cmd_code[2]; // 0 = command echo, 1 = command code - decoder.extract(2, &cmd_code[0]); - if (cmd_code[1] != 0) { - // error occurred - std::cout << "Received NACK packet (class, command, code): "; - std::cout << std::hex << static_cast(packet_.descriptor) << ", "; - std::cout << static_cast(cmd_code[0]) << ", "; - std::cout << static_cast(cmd_code[1]) << "\n" << std::flush; - } - } - } - } -} - -int Imu::writePacket(const Packet &p, unsigned int to) { - using namespace std::chrono; - - // place into buffer - std::vector v; - v.reserve(Packet::kHeaderLength + sizeof(Packet::payload) + 2); - - v.push_back(p.syncMSB); - v.push_back(p.syncLSB); - v.push_back(p.descriptor); - v.push_back(p.length); - for (size_t i = 0; i < p.length; i++) { - v.push_back(p.payload[i]); - } - v.push_back(p.checkMSB); - v.push_back(p.checkLSB); - - auto tstart = high_resolution_clock::now(); - auto tstop = tstart + milliseconds(to); - - size_t written = 0; - while (written < v.size()) { - const ssize_t amt = ::write(fd_, &v[written], v.size() - written); - if (amt > 0) { - written += amt; - } else if (amt < 0) { - if (errno == EAGAIN || errno == EINTR) { - // blocked or interrupted - try again until timeout - } else { - return -1; // error while writing - } - } - - if (tstop < high_resolution_clock::now()) { - return 0; // timed out - } - } - - return static_cast(written); // wrote w/o issue -} - -void Imu::sendPacket(const Packet &p, unsigned int to) { - const int wrote = writePacket(p, to); - if (wrote < 0) { - throw io_error(strerror(errno)); - } else if (wrote == 0) { - throw timeout_error(true,to); - } -} - -void Imu::receiveResponse(const Packet &command, unsigned int to) { - // read back response - const auto tstart = std::chrono::high_resolution_clock::now(); - const auto tend = tstart + std::chrono::milliseconds(to); - - while (std::chrono::high_resolution_clock::now() <= tend) { - const int resp = pollInput(1); - if (resp > 0) { - // check if this is an ack - const int ack = packet_.ackErrorCodeFor(command); - - if (ack == 0) { - return; // success, exit - } else if (ack > 0) { - throw command_error(command, ack); - } else { - if (verbose_) { - std::cout << "Not interested in this [N]ACK!\n"; - std::cout << packet_.toString() << "\n"; - } - // this ack was not for us, keep spinning until timeout - } - } else if (resp < 0) { - throw io_error(strerror(errno)); - } else { - // resp == 0 keep reading until timeout - } - } - if (verbose_) { - std::cout << "Timed out reading response to:\n"; - std::cout << command.toString() << std::endl << std::flush; - } - // timed out - throw timeout_error(false, to); -} - -void Imu::sendCommand(const Packet &p, bool readReply) { - if (verbose_) { - std::cout << "Sending command:\n"; - std::cout << p.toString() << std::endl; - } - sendPacket(p, rwTimeout_); - if (readReply) { - receiveResponse(p, rwTimeout_); - } -} diff --git a/thormang3_imu_3dm_gx4/src/imu.hpp b/thormang3_imu_3dm_gx4/src/imu.hpp deleted file mode 100644 index 09b125f0..00000000 --- a/thormang3_imu_3dm_gx4/src/imu.hpp +++ /dev/null @@ -1,436 +0,0 @@ -/* - * imu.hpp - * - * Copyright (c) 2014 Kumar Robotics. All rights reserved. - * - * This file is part of galt. - * - * Created on: 2/6/2014 - * Author: gareth - */ - -#ifndef IMU_H_ -#define IMU_H_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ // will fail outside of gcc/clang -#define HOST_LITTLE_ENDIAN -#else -#define HOST_BIG_ENDIAN -#endif - -namespace imu_3dm_gx4 { - -/** - * @brief Imu Interface to the Microstrain 3DM-GX4-25 IMU - * @see http://www.microstrain.com/inertial/3dm-gx4-25 - * @author Gareth Cross - * - * @note Error handling: All methods which communicate with the device - * can throw the exceptions below: io_error, timeout_error, command_error and - * std::runtime_error. Additional exceptions are indicated on specific - * functions. - */ -class Imu { -public: - struct Packet { - static constexpr uint8_t kHeaderLength = 4; - static constexpr uint8_t kSyncMSB = 0x75; - static constexpr uint8_t kSyncLSB = 0x65; - - union { - struct { - uint8_t syncMSB; - uint8_t syncLSB; - }; - uint16_t sync; /**< Identifer of packet */ - }; - - uint8_t descriptor; /**< Type of packet */ - uint8_t length; /**< Length of the packet in bytes */ - - uint8_t payload[255]; /**< Payload of packet */ - - union { - struct { - uint8_t checkMSB; - uint8_t checkLSB; - }; - uint16_t checksum; /**< Packet checksum */ - }; - - /** - * @brief True if this packet corresponds to an imu data message. - */ - bool isIMUData() const; - - /** - * @brief True if this packet corresponds to a filter data message - */ - bool isFilterData() const; - - /** - * @brief Extract the ACK code from this packet. - * @param commmand Command packet to which this ACK should correspond. - * - * @return -1 if the packets do not correspond or this is not an ACK. The - * error code is returned otherwise. - */ - int ackErrorCodeFor(const Packet &command) const; - - /** - * @brief Calculate the packet checksum. Sets the checksum variable - */ - void calcChecksum(); - - /** - * @brief Constructor - * @param desc Major descriptor of this command. - * @param len Length of the packet payload. - */ - Packet(uint8_t desc = 0); - - /** - * @brief Make a 'human-readable' version of the packet. - * @return std::string - */ - std::string toString() const; - } __attribute__((packed)); - - /** - * @brief Info Structure generated by the getDeviceInfo command - */ - struct Info { - uint16_t firmwareVersion; /// Firmware version - std::string modelName; /// Model name - std::string modelNumber; /// Model number - std::string serialNumber; /// Serial number - std::string lotNumber; /// Lot number - appears to be unused - std::string deviceOptions; /// Device options (range of the sensor) - - /** - * @brief Conver to map of human readable strings. - */ - std::map toMap() const; - }; - - /** - * @brief DiagnosticFields struct (See 3DM documentation for these fields) - */ - struct DiagnosticFields { - uint16_t modelNumber; - uint8_t selector; - uint32_t statusFlags; - uint32_t systemTimer; - uint32_t num1PPSPulses; - uint32_t last1PPSPulse; - uint8_t imuStreamEnabled; - uint8_t filterStreamEnabled; - uint32_t imuPacketsDropped; - uint32_t filterPacketsDropped; - uint32_t comBytesWritten; - uint32_t comBytesRead; - uint32_t comNumWriteOverruns; - uint32_t comNumReadOverruns; - uint32_t usbBytesWritten; - uint32_t usbBytesRead; - uint32_t usbNumWriteOverruns; - uint32_t usbNumReadOverruns; - uint32_t numIMUParseErrors; - uint32_t totalIMUMessages; - uint32_t lastIMUMessage; - - /** - * @brief Convert to map of human readable strings and integers. - */ - std::map toMap() const; - - } __attribute__((packed)); - - /** - * @brief IMUData IMU readings produced by the sensor - */ - struct IMUData { - enum { - Accelerometer = (1 << 0), - Gyroscope = (1 << 1), - Magnetometer = (1 << 2), - Barometer = (1 << 3), - }; - - unsigned int fields; /**< Which fields are valid in the struct */ - - float accel[3]; /**< Acceleration, units of G */ - float gyro[3]; /**< Angular rates, units of rad/s */ - float mag[3]; /**< Magnetic field, units of gauss */ - float pressure; /**< Pressure, units of gauss */ - - IMUData() : fields(0) {} - }; - - /** - * @brief FilterData Estimator readings produced by the sensor - */ - struct FilterData { - enum { - Quaternion = (1 << 0), - Bias = (1 << 1), - AngleUnertainty = (1 << 2), - BiasUncertainty = (1 << 3), - }; - - unsigned int fields; /**< Which fields are present in the struct. */ - - float quaternion[4]; /**< Orientation quaternion (q0,q1,q2,q3) */ - uint16_t quaternionStatus; /**< Quaternion status */ - - float bias[3]; /**< Gyro bias */ - uint16_t biasStatus; /**< Bias status 0 = invalid, 1 = valid */ - - float angleUncertainty[3]; /**< 1-sigma angle uncertainty */ - uint16_t angleUncertaintyStatus; /**< 0 = invalid, 1 = valid */ - - float biasUncertainty[3]; /**< 1-sigma bias uncertainty */ - uint16_t biasUncertaintyStatus; /**< 0 = invalid, 1 = valid */ - - FilterData() : fields(0) {} - }; - - /* Exceptions */ - - /** - * @brief command_error Generated when device replies with NACK. - */ - struct command_error : public std::runtime_error { - command_error(const Packet &p, uint8_t code); - - private: - std::string generateString(const Packet &p, uint8_t code); - }; - - /** - * @brief io_error Generated when a low-level IO command fails. - */ - struct io_error : public std::runtime_error { - io_error(const std::string &desc) : std::runtime_error(desc) {} - }; - - /** - * @brief timeout_error Generated when read or write times out, usually - * indicates device hang up. - */ - struct timeout_error : public std::runtime_error { - timeout_error(bool write, unsigned int to); - - private: - std::string generateString(bool write, unsigned int to); - }; - - /** - * @brief Imu Constructor - * @param device Path to the device in /dev, eg. /dev/ttyACM0 - * @param verbose If true, packet reads are logged to console. - */ - Imu(const std::string &device, bool verbose); - - /** - * @brief ~Imu Destructor - * @note Calls disconnect() automatically. - */ - virtual ~Imu(); - - /** - * @brief connect Open a file descriptor to the serial device. - * @throw runtime_error if already open or path is invalid. - * io_error for termios failures. - */ - void connect(); - - /** - * @brief runOnce Poll for input and read packets if available. - */ - void runOnce(); - - /** - * @brief disconnect Close the file descriptor, sending the IDLE command - * first. - */ - void disconnect(); - - /** - * @brief selectBaudRate Select baud rate. - * @param baud The desired baud rate. Supported values are: - * 9600,19200,115200,230400,460800,921600. - * - * @note This command will attempt to communicate w/ the device using all - * possible baud rates. Once the current baud rate is determined, it will - * switch to 'baud' and send the UART command. - * - * @throw std::runtime_error for invalid baud rates. - */ - void selectBaudRate(unsigned int baud); - - /** - * @brief ping Ping the device. - */ - void ping(); - - /** - * @brief idle Switch the device to idle mode. - * @param needReply, if true we wait for reply. - */ - void idle(bool needReply = true); - - /** - * @brief resume Resume the device. - */ - void resume(); - - /** - * @brief getDeviceInfo Get hardware information about the device. - * @param info Struct into which information is placed. - */ - void getDeviceInfo(Imu::Info &info); - - /** - * @brief getIMUDataBaseRate Get the imu data base rate (should be 1000) - * @param baseRate On success, the base rate in Hz - */ - void getIMUDataBaseRate(uint16_t &baseRate); - - /** - * @brief getFilterDataBaseRate Get the filter data base rate (should be 500) - * @param baseRate On success, the base rate in Hz - */ - void getFilterDataBaseRate(uint16_t &baseRate); - - /** - * @brief getDiagnosticInfo Get diagnostic information from the IMU. - * @param fields On success, a filled out DiagnosticFields. - */ - void getDiagnosticInfo(Imu::DiagnosticFields &fields); - - /** - * @brief setIMUDataRate Set imu data rate for different sources. - * @param decimation Denominator in the update rate value: 1000/x - * @param sources Sources to apply this rate to. May be a bitwise combination - * of the values: Accelerometer, Gyroscope, Magnetometer, Barometer - * - * @throw invalid_argument if an invalid source is requested. - */ - void setIMUDataRate(uint16_t decimation, const std::bitset<4> &sources); - - /** - * @brief setFilterDataRate Set estimator data rate for different sources. - * @param decimation Denominator in the update rate value: 500/x - * @param sources Sources to apply this rate to. May be a bitwise combination - * of the values: Quaternion, GyroBias, AngleUncertainty, BiasUncertainty - * - * @throw invalid_argument if an invalid source is requested. - */ - void setFilterDataRate(uint16_t decimation, const std::bitset<4> &sources); - - /** - * @brief enableMeasurements Set which measurements to enable in the filter - * @param accel If true, acceleration measurements are enabled - * @param magnetometer If true, magnetometer measurements are enabled. - */ - void enableMeasurements(bool accel, bool magnetometer); - - /** - * @brief enableBiasEstimation Enable gyroscope bias estimation - * @param enabled If true, bias estimation is enabled - */ - void enableBiasEstimation(bool enabled); - - /** - * @brief setHardIronOffset Set the hard-iron bias vector for the - * magnetometer. - * @param offset 3x1 vector, units of gauss. - */ - void setHardIronOffset(float offset[3]); - - /** - * @brief setSoftIronMatrix Set the soft-iron matrix for the magnetometer. - * @param matrix 3x3 row-major matrix, default should be identity. - */ - void setSoftIronMatrix(float matrix[9]); - - /** - * @brief enableIMUStream Enable/disable streaming of IMU data - * @param enabled If true, streaming is enabled. - */ - void enableIMUStream(bool enabled); - - /** - * @brief enableFilterStream Enable/disable streaming of estimation filter - * data. - * @param enabled If true, streaming is enabled. - */ - void enableFilterStream(bool enabled); - - /** - * @brief Set the IMU data callback. - * @note The IMU data callback is called every time new IMU data is read. - */ - void setIMUDataCallback(const std::function &); - - /** - * @brief Set the onboard filter data callback. - * @note The filter data is called every time new orientation data is read. - */ - void - setFilterDataCallback(const std::function &); - - private: - // non-copyable - Imu(const Imu &) = delete; - Imu &operator=(const Imu &) = delete; - - int pollInput(unsigned int to); - - std::size_t handleByte(const uint8_t& byte, bool& found); - - int handleRead(size_t); - - void processPacket(); - - int writePacket(const Packet &p, unsigned int to); - - void sendPacket(const Packet &p, unsigned int to); - - void receiveResponse(const Packet &command, unsigned int to); - - void sendCommand(const Packet &p, bool readReply = true); - - bool termiosBaudRate(unsigned int baud); - - const std::string device_; - const bool verbose_; - int fd_; - unsigned int rwTimeout_; - - std::vector buffer_; - std::deque queue_; - size_t srcIndex_, dstIndex_; - - std::function - imuDataCallback_; /// Called with IMU data is ready - std::function - filterDataCallback_; /// Called when filter data is ready - - enum { Idle = 0, Reading, } state_; - Packet packet_; -}; - -} // imu_3dm_gx4 - -#endif // IMU_H_ diff --git a/thormang3_imu_3dm_gx4/src/imu_3dm_gx4.cpp b/thormang3_imu_3dm_gx4/src/imu_3dm_gx4.cpp deleted file mode 100644 index b8e7b690..00000000 --- a/thormang3_imu_3dm_gx4/src/imu_3dm_gx4.cpp +++ /dev/null @@ -1,306 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include "imu.hpp" - -using namespace imu_3dm_gx4; - -#define kEarthGravity (9.80665) - -ros::Publisher pubIMU; -ros::Publisher pubMag; -ros::Publisher pubPressure; -ros::Publisher pubFilter; -std::string frameId; -sensor_msgs::Imu imuFilterData; - -Imu::Info info; -Imu::DiagnosticFields fields; - -// diagnostic_updater resources -std::shared_ptr updater; -std::shared_ptr imuDiag; -std::shared_ptr filterDiag; - -void publishData(const Imu::IMUData &data) { - sensor_msgs::Imu imu; - sensor_msgs::MagneticField field; - sensor_msgs::FluidPressure pressure; - - // assume we have all of these since they were requested - /// @todo: Replace this with a mode graceful failure... - assert(data.fields & Imu::IMUData::Accelerometer); - assert(data.fields & Imu::IMUData::Magnetometer); - assert(data.fields & Imu::IMUData::Barometer); - assert(data.fields & Imu::IMUData::Gyroscope); - - // timestamp identically - imu.header.stamp = ros::Time::now(); - imu.header.frame_id = frameId; - field.header.stamp = imu.header.stamp; - field.header.frame_id = frameId; - pressure.header.stamp = imu.header.stamp; - pressure.header.frame_id = frameId; - - imu.orientation_covariance[0] = - -1; // orientation data is on a separate topic - - imu.orientation = imuFilterData.orientation; - imu.linear_acceleration.x = data.accel[0] * kEarthGravity; - imu.linear_acceleration.y = data.accel[1] * kEarthGravity; - imu.linear_acceleration.z = data.accel[2] * kEarthGravity; - imu.angular_velocity.x = data.gyro[0]; - imu.angular_velocity.y = data.gyro[1]; - imu.angular_velocity.z = data.gyro[2]; - - field.magnetic_field.x = data.mag[0]; - field.magnetic_field.y = data.mag[1]; - field.magnetic_field.z = data.mag[2]; - - pressure.fluid_pressure = data.pressure; - - // publish - pubIMU.publish(imu); - pubMag.publish(field); - pubPressure.publish(pressure); - if (imuDiag) { - imuDiag->tick(imu.header.stamp); - } -} - -void publishFilter(const Imu::FilterData &data) { - assert(data.fields & Imu::FilterData::Quaternion); - assert(data.fields & Imu::FilterData::Bias); - assert(data.fields & Imu::FilterData::AngleUnertainty); - assert(data.fields & Imu::FilterData::BiasUncertainty); - - thormang3_imu_3dm_gx4::FilterOutput output; - output.header.stamp = ros::Time::now(); - output.header.frame_id = frameId; - output.orientation.w = data.quaternion[0]; - output.orientation.x = data.quaternion[1]; - output.orientation.y = data.quaternion[2]; - output.orientation.z = data.quaternion[3]; - imuFilterData.orientation.w = data.quaternion[0]; - imuFilterData.orientation.x = data.quaternion[1]; - imuFilterData.orientation.y = data.quaternion[2]; - imuFilterData.orientation.z = data.quaternion[3]; - output.bias.x = data.bias[0]; - output.bias.y = data.bias[1]; - output.bias.z = data.bias[2]; - - output.bias_covariance[0] = data.biasUncertainty[0]*data.biasUncertainty[0]; - output.bias_covariance[4] = data.biasUncertainty[1]*data.biasUncertainty[1]; - output.bias_covariance[8] = data.biasUncertainty[2]*data.biasUncertainty[2]; - - output.orientation_covariance[0] = data.angleUncertainty[0]* - data.angleUncertainty[0]; - output.orientation_covariance[4] = data.angleUncertainty[1]* - data.angleUncertainty[1]; - output.orientation_covariance[8] = data.angleUncertainty[2]* - data.angleUncertainty[2]; - - output.quat_status = data.quaternionStatus; - output.bias_status = data.biasStatus; - output.orientation_covariance_status = data.angleUncertaintyStatus; - output.bias_covariance_status = data.biasUncertaintyStatus; - - pubFilter.publish(output); - if (filterDiag) { - filterDiag->tick(output.header.stamp); - } -} - -std::shared_ptr configTopicDiagnostic( - const std::string& name, double * target) { - std::shared_ptr diag; - const double period = 1.0 / *target; // for 1000Hz, period is 1e-3 - - diagnostic_updater::FrequencyStatusParam freqParam(target, target, 0.01, 10); - diagnostic_updater::TimeStampStatusParam timeParam(0, period * 0.5); - diag.reset(new diagnostic_updater::TopicDiagnostic(name, - *updater, - freqParam, - timeParam)); - return diag; -} - -void updateDiagnosticInfo(diagnostic_updater::DiagnosticStatusWrapper& stat, - imu_3dm_gx4::Imu* imu) { - // add base device info - std::map map = info.toMap(); - for (const std::pair& p : map) { - stat.add(p.first, p.second); - } - - try { - // try to read diagnostic info - imu->getDiagnosticInfo(fields); - - auto map = fields.toMap(); - for (const std::pair& p : map) { - stat.add(p.first, p.second); - } - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Read diagnostic info."); - } - catch (std::exception& e) { - const std::string message = std::string("Failed: ") + e.what(); - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, message); - } -} - -int main(int argc, char **argv) { - ros::init(argc, argv, "imu_3dm_gx4"); - ros::NodeHandle nh("~"); - - std::string device; - int baudrate; - bool enableFilter; - bool enableMagUpdate, enableAccelUpdate; - int requestedImuRate, requestedFilterRate; - bool verbose; - - // load parameters from launch file - nh.param("device", device, "/dev/ttyACM0"); - nh.param("baudrate", baudrate, 115200); - nh.param("frameId", frameId, std::string("imu")); - nh.param("imu_rate", requestedImuRate, 100); - nh.param("filter_rate", requestedFilterRate, 100); - nh.param("enable_filter", enableFilter, false); - nh.param("enable_mag_update", enableMagUpdate, false); - nh.param("enable_accel_update", enableAccelUpdate, true); - nh.param("verbose", verbose, false); - - if (requestedFilterRate < 0 || requestedImuRate < 0) { - ROS_ERROR("imu_rate and filter_rate must be > 0"); - return -1; - } - - pubIMU = nh.advertise("imu", 1); - pubMag = nh.advertise("magnetic_field", 1); - pubPressure = nh.advertise("pressure", 1); - - if (enableFilter) { - pubFilter = nh.advertise("filter", 1); - } - - // new instance of the IMU - Imu imu(device, verbose); - try { - imu.connect(); - - ROS_INFO("Selecting baud rate %u", baudrate); - imu.selectBaudRate(baudrate); - - ROS_INFO("Fetching device info."); - imu.getDeviceInfo(info); - std::map map = info.toMap(); - for (const std::pair& p : map) { - ROS_INFO("\t%s: %s", p.first.c_str(), p.second.c_str()); - } - - ROS_INFO("Idling the device"); - imu.idle(); - - // read back data rates - uint16_t imuBaseRate, filterBaseRate; - imu.getIMUDataBaseRate(imuBaseRate); - ROS_INFO("IMU data base rate: %u Hz", imuBaseRate); - imu.getFilterDataBaseRate(filterBaseRate); - ROS_INFO("Filter data base rate: %u Hz", filterBaseRate); - - // calculate decimation rates - if (static_cast(requestedImuRate) > imuBaseRate) { - throw std::runtime_error("imu_rate cannot exceed " + - std::to_string(imuBaseRate)); - } - if (static_cast(requestedFilterRate) > filterBaseRate) { - throw std::runtime_error("filter_rate cannot exceed " + - std::to_string(filterBaseRate)); - } - - const uint16_t imuDecimation = imuBaseRate / requestedImuRate; - const uint16_t filterDecimation = filterBaseRate / requestedFilterRate; - - ROS_INFO("Selecting IMU decimation: %u", imuDecimation); - imu.setIMUDataRate( - imuDecimation, Imu::IMUData::Accelerometer | - Imu::IMUData::Gyroscope | - Imu::IMUData::Magnetometer | - Imu::IMUData::Barometer); - - ROS_INFO("Selecting filter decimation: %u", filterDecimation); - imu.setFilterDataRate(filterDecimation, Imu::FilterData::Quaternion | - Imu::FilterData::Bias | - Imu::FilterData::AngleUnertainty | - Imu::FilterData::BiasUncertainty); - - ROS_INFO("Enabling IMU data stream"); - imu.enableIMUStream(true); - - if (enableFilter) { - ROS_INFO("Enabling filter data stream"); - imu.enableFilterStream(true); - - ROS_INFO("Enabling filter measurements"); - imu.enableMeasurements(enableAccelUpdate, enableMagUpdate); - - ROS_INFO("Enabling gyro bias estimation"); - imu.enableBiasEstimation(true); - } else { - ROS_INFO("Disabling filter data stream"); - imu.enableFilterStream(false); - } - imu.setIMUDataCallback(publishData); - imu.setFilterDataCallback(publishFilter); - - // configure diagnostic updater - if (!nh.hasParam("diagnostic_period")) { - nh.setParam("diagnostic_period", 0.2); // 5hz period - } - - updater.reset(new diagnostic_updater::Updater()); - const std::string hwId = info.modelName + "-" + info.modelNumber; - updater->setHardwareID(hwId); - - // calculate the actual rates we will get - double imuRate = imuBaseRate / (1.0 * imuDecimation); - double filterRate = filterBaseRate / (1.0 * filterDecimation); - imuDiag = configTopicDiagnostic("imu",&imuRate); - if (enableFilter) { - filterDiag = configTopicDiagnostic("filter",&filterRate); - } - - updater->add("diagnostic_info", - boost::bind(&updateDiagnosticInfo, _1, &imu)); - - ROS_INFO("Resuming the device"); - imu.resume(); - - while (ros::ok()) { - imu.runOnce(); - updater->update(); - } - imu.disconnect(); - } - catch (Imu::io_error &e) { - ROS_ERROR("IO error: %s\n", e.what()); - } - catch (Imu::timeout_error &e) { - ROS_ERROR("Timeout: %s\n", e.what()); - } - catch (std::exception &e) { - ROS_ERROR("Exception: %s\n", e.what()); - } - - return 0; -}