diff --git a/CMakeLists.txt b/CMakeLists.txt index d44b7b0..9d085d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(imu_bno055) -add_compile_options(-std=c++11 -li2c) +add_compile_options(-std=c++14 -li2c) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs diagnostic_msgs) @@ -9,7 +9,7 @@ catkin_package(INCLUDE_DIRS include) include_directories(include ${catkin_INCLUDE_DIRS}) -add_executable(bno055_i2c_node src/bno055_i2c_activity.cpp src/bno055_i2c_node.cpp src/watchdog.cpp) +add_executable(bno055_i2c_node src/bno055_i2c_driver.cpp src/bno055_i2c_node.cpp src/watchdog.cpp) #add_dependencies(bno055_i2c_node diagnostic_msgs_gencpp) include_directories(${catkin_INCLUDE_DIRS}) diff --git a/include/imu_bno055/bno055_i2c_activity.h b/include/imu_bno055/bno055_i2c_driver.h similarity index 85% rename from include/imu_bno055/bno055_i2c_activity.h rename to include/imu_bno055/bno055_i2c_driver.h index d370934..8944639 100644 --- a/include/imu_bno055/bno055_i2c_activity.h +++ b/include/imu_bno055/bno055_i2c_driver.h @@ -1,21 +1,16 @@ -#ifndef _bno055_i2c_activity_dot_h -#define _bno055_i2c_activity_dot_h +#ifndef _bno055_i2c_driver_dot_h +#define _bno055_i2c_driver_dot_h #include #include #include #include +#include #include #include - -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include #include #include @@ -231,46 +226,23 @@ typedef struct { uint8_t system_error_code; } IMURecord; -class BNO055I2CActivity { +class BNO055I2CDriver { public: - BNO055I2CActivity(ros::NodeHandle &_nh, ros::NodeHandle &_nh_priv); - - bool start(); - bool stop(); - bool spinOnce(); - - bool onServiceReset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); - bool onServiceCalibrate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); - - private: + BNO055I2CDriver(std::string device_, int address_); + void init(); bool reset(); + IMURecord read(); + private: + // class variables - uint32_t seq = 0; int file; - diagnostic_msgs::DiagnosticStatus current_status; - - // ROS parameters - std::string param_frame_id; - std::string param_device; - int param_address; - - // ROS node handles - ros::NodeHandle nh; - ros::NodeHandle nh_priv; - - // ROS publishers - ros::Publisher pub_data; - ros::Publisher pub_raw; - ros::Publisher pub_mag; - ros::Publisher pub_temp; - ros::Publisher pub_status; - - // ROS subscribers - ros::ServiceServer service_calibrate; - ros::ServiceServer service_reset; + + // parameters + std::string device; + int address; }; } -#endif // _bno055_i2c_activity_dot_h +#endif // _bno055_i2c_driver_dot_h diff --git a/package.xml b/package.xml index 1fe371c..274f1f0 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ imu_bno055 - 1.0.0 + 2.0.0 I2C driver for a BNO055 IMU. Tested on Jetson TX2. dheera MIT diff --git a/src/bno055_i2c_activity.cpp b/src/bno055_i2c_activity.cpp deleted file mode 100644 index b8565c4..0000000 --- a/src/bno055_i2c_activity.cpp +++ /dev/null @@ -1,254 +0,0 @@ -/* bno055_i2c_activity.cpp - * Author: Dheera Venkatraman - * - * Defines a BNO055I2C Activity class, constructed with node handles - * and which handles all ROS duties. - */ - -#include "imu_bno055/bno055_i2c_activity.h" - -namespace imu_bno055 { - -// ******** constructors ******** // - -BNO055I2CActivity::BNO055I2CActivity(ros::NodeHandle &_nh, ros::NodeHandle &_nh_priv) : - nh(_nh), nh_priv(_nh_priv) { - ROS_INFO("initializing"); - nh_priv.param("device", param_device, (std::string)"/dev/i2c-1"); - nh_priv.param("address", param_address, (int)BNO055_ADDRESS_A); - nh_priv.param("frame_id", param_frame_id, (std::string)"imu"); - - current_status.level = 0; - current_status.name = "BNO055 IMU"; - current_status.hardware_id = "bno055_i2c"; - - diagnostic_msgs::KeyValue calib_stat; - calib_stat.key = "Calibration status"; - calib_stat.value = ""; - current_status.values.push_back(calib_stat); - - diagnostic_msgs::KeyValue selftest_result; - selftest_result.key = "Self-test result"; - selftest_result.value = ""; - current_status.values.push_back(selftest_result); - - diagnostic_msgs::KeyValue intr_stat; - intr_stat.key = "Interrupt status"; - intr_stat.value = ""; - current_status.values.push_back(intr_stat); - - diagnostic_msgs::KeyValue sys_clk_stat; - sys_clk_stat.key = "System clock status"; - sys_clk_stat.value = ""; - current_status.values.push_back(sys_clk_stat); - - diagnostic_msgs::KeyValue sys_stat; - sys_stat.key = "System status"; - sys_stat.value = ""; - current_status.values.push_back(sys_stat); - - diagnostic_msgs::KeyValue sys_err; - sys_err.key = "System error"; - sys_err.value = ""; - current_status.values.push_back(sys_err); -} - -// ******** private methods ******** // - -bool BNO055I2CActivity::reset() { - int i = 0; - - _i2c_smbus_write_byte_data(file, BNO055_OPR_MODE_ADDR, BNO055_OPERATION_MODE_CONFIG); - ros::Duration(0.025).sleep(); - - // reset - _i2c_smbus_write_byte_data(file, BNO055_SYS_TRIGGER_ADDR, 0x20); - ros::Duration(0.025).sleep(); - - // wait for chip to come back online - while(_i2c_smbus_read_byte_data(file, BNO055_CHIP_ID_ADDR) != BNO055_ID) { - ros::Duration(0.010).sleep(); - if(i++ > 500) { - ROS_ERROR_STREAM("chip did not come back online in 5 seconds after reset"); - return false; - } - } - ros::Duration(0.100).sleep(); - - // normal power mode - _i2c_smbus_write_byte_data(file, BNO055_PWR_MODE_ADDR, BNO055_POWER_MODE_NORMAL); - ros::Duration(0.010).sleep(); - - _i2c_smbus_write_byte_data(file, BNO055_PAGE_ID_ADDR, 0); - _i2c_smbus_write_byte_data(file, BNO055_SYS_TRIGGER_ADDR, 0); - ros::Duration(0.025).sleep(); - - _i2c_smbus_write_byte_data(file, BNO055_OPR_MODE_ADDR, BNO055_OPERATION_MODE_NDOF); - ros::Duration(0.025).sleep(); - - return true; -} - -// ******** public methods ******** // - -bool BNO055I2CActivity::start() { - ROS_INFO("starting"); - - if(!pub_data) pub_data = nh.advertise("data", 1); - if(!pub_raw) pub_raw = nh.advertise("raw", 1); - if(!pub_mag) pub_mag = nh.advertise("mag", 1); - if(!pub_temp) pub_temp = nh.advertise("temp", 1); - if(!pub_status) pub_status = nh.advertise("status", 1); - - if(!service_calibrate) service_calibrate = nh.advertiseService( - "calibrate", - &BNO055I2CActivity::onServiceCalibrate, - this - ); - - if(!service_reset) service_reset = nh.advertiseService( - "reset", - &BNO055I2CActivity::onServiceReset, - this - ); - - file = open(param_device.c_str(), O_RDWR); - if(ioctl(file, I2C_SLAVE, param_address) < 0) { - ROS_ERROR("i2c device open failed"); - return false; - } - - if(_i2c_smbus_read_byte_data(file, BNO055_CHIP_ID_ADDR) != BNO055_ID) { - ROS_ERROR("incorrect chip ID"); - return false; - } - - ROS_INFO_STREAM("rev ids:" - << " accel:" << _i2c_smbus_read_byte_data(file, BNO055_ACCEL_REV_ID_ADDR) - << " mag:" << _i2c_smbus_read_byte_data(file, BNO055_MAG_REV_ID_ADDR) - << " gyro:" << _i2c_smbus_read_byte_data(file, BNO055_GYRO_REV_ID_ADDR) - << " sw:" << _i2c_smbus_read_word_data(file, BNO055_SW_REV_ID_LSB_ADDR) - << " bl:" << _i2c_smbus_read_byte_data(file, BNO055_BL_REV_ID_ADDR)); - - if(!reset()) { - ROS_ERROR("chip reset and setup failed"); - return false; - } - - return true; -} - -bool BNO055I2CActivity::spinOnce() { - ros::spinOnce(); - - ros::Time time = ros::Time::now(); - - IMURecord record; - - unsigned char c = 0; - - seq++; - - // can only read a length of 0x20 at a time, so do it in 2 reads - // BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR is the start of the data block that aligns with the IMURecord struct - if(_i2c_smbus_read_i2c_block_data(file, BNO055_ACCEL_DATA_X_LSB_ADDR, 0x20, (uint8_t*)&record) != 0x20) { - return false; - } - if(_i2c_smbus_read_i2c_block_data(file, BNO055_ACCEL_DATA_X_LSB_ADDR + 0x20, 0x13, (uint8_t*)&record + 0x20) != 0x13) { - return false; - } - - sensor_msgs::Imu msg_raw; - msg_raw.header.stamp = time; - msg_raw.header.frame_id = param_frame_id; - msg_raw.header.seq = seq; - msg_raw.linear_acceleration.x = (double)record.raw_linear_acceleration_x / 100.0; - msg_raw.linear_acceleration.y = (double)record.raw_linear_acceleration_y / 100.0; - msg_raw.linear_acceleration.z = (double)record.raw_linear_acceleration_z / 100.0; - msg_raw.angular_velocity.x = (double)record.raw_angular_velocity_x / 900.0; - msg_raw.angular_velocity.y = (double)record.raw_angular_velocity_y / 900.0; - msg_raw.angular_velocity.z = (double)record.raw_angular_velocity_z / 900.0; - - sensor_msgs::MagneticField msg_mag; - msg_mag.header.stamp = time; - msg_mag.header.frame_id = param_frame_id; - msg_mag.header.seq = seq; - msg_mag.magnetic_field.x = (double)record.raw_magnetic_field_x / 16.0; - msg_mag.magnetic_field.y = (double)record.raw_magnetic_field_y / 16.0; - msg_mag.magnetic_field.z = (double)record.raw_magnetic_field_z / 16.0; - - sensor_msgs::Imu msg_data; - msg_data.header.stamp = time; - msg_data.header.frame_id = param_frame_id; - msg_data.header.seq = seq; - - double fused_orientation_norm = std::pow( - std::pow(record.fused_orientation_w, 2) + - std::pow(record.fused_orientation_x, 2) + - std::pow(record.fused_orientation_y, 2) + - std::pow(record.fused_orientation_z, 2), 0.5); - - msg_data.orientation.w = (double)record.fused_orientation_w / fused_orientation_norm; - msg_data.orientation.x = (double)record.fused_orientation_x / fused_orientation_norm; - msg_data.orientation.y = (double)record.fused_orientation_y / fused_orientation_norm; - msg_data.orientation.z = (double)record.fused_orientation_z / fused_orientation_norm; - msg_data.linear_acceleration.x = (double)record.fused_linear_acceleration_x / 100.0; - msg_data.linear_acceleration.y = (double)record.fused_linear_acceleration_y / 100.0; - msg_data.linear_acceleration.z = (double)record.fused_linear_acceleration_z / 100.0; - msg_data.angular_velocity.x = (double)record.raw_angular_velocity_x / 900.0; - msg_data.angular_velocity.y = (double)record.raw_angular_velocity_y / 900.0; - msg_data.angular_velocity.z = (double)record.raw_angular_velocity_z / 900.0; - - sensor_msgs::Temperature msg_temp; - msg_temp.header.stamp = time; - msg_temp.header.frame_id = param_frame_id; - msg_temp.header.seq = seq; - msg_temp.temperature = (double)record.temperature; - - pub_data.publish(msg_data); - pub_raw.publish(msg_raw); - pub_mag.publish(msg_mag); - pub_temp.publish(msg_temp); - - if(seq % 50 == 0) { - current_status.values[DIAG_CALIB_STAT].value = std::to_string(record.calibration_status); - current_status.values[DIAG_SELFTEST_RESULT].value = std::to_string(record.self_test_result); - current_status.values[DIAG_INTR_STAT].value = std::to_string(record.interrupt_status); - current_status.values[DIAG_SYS_CLK_STAT].value = std::to_string(record.system_clock_status); - current_status.values[DIAG_SYS_STAT].value = std::to_string(record.system_status); - current_status.values[DIAG_SYS_ERR].value = std::to_string(record.system_error_code); - pub_status.publish(current_status); - } - - return true; -} - -bool BNO055I2CActivity::stop() { - ROS_INFO("stopping"); - - if(pub_data) pub_data.shutdown(); - if(pub_raw) pub_raw.shutdown(); - if(pub_mag) pub_mag.shutdown(); - if(pub_temp) pub_temp.shutdown(); - if(pub_status) pub_status.shutdown(); - - if(service_calibrate) service_calibrate.shutdown(); - if(service_reset) service_reset.shutdown(); - - return true; -} - -bool BNO055I2CActivity::onServiceReset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { - if(!reset()) { - ROS_ERROR("chip reset and setup failed"); - return false; - } - return true; -} - -bool BNO055I2CActivity::onServiceCalibrate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { - // TODO implement this - return true; -} - -} diff --git a/src/bno055_i2c_driver.cpp b/src/bno055_i2c_driver.cpp new file mode 100644 index 0000000..7619441 --- /dev/null +++ b/src/bno055_i2c_driver.cpp @@ -0,0 +1,88 @@ +/* bno055_i2c_driver.cpp + * Author: Dheera Venkatraman + */ + +#include "imu_bno055/bno055_i2c_driver.h" + +namespace imu_bno055 { + +BNO055I2CDriver::BNO055I2CDriver(std::string device_, int address_) { + device = device_; + address = address_; +} + +bool BNO055I2CDriver::reset() { + int i = 0; + + _i2c_smbus_write_byte_data(file, BNO055_OPR_MODE_ADDR, BNO055_OPERATION_MODE_CONFIG); + std::this_thread::sleep_for(std::chrono::milliseconds(25)); + + // reset + _i2c_smbus_write_byte_data(file, BNO055_SYS_TRIGGER_ADDR, 0x20); + std::this_thread::sleep_for(std::chrono::milliseconds(25)); + + // wait for chip to come back online + while(_i2c_smbus_read_byte_data(file, BNO055_CHIP_ID_ADDR) != BNO055_ID) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + if(i++ > 500) { + throw std::runtime_error("chip did not come back online within 5 seconds of reset"); + return false; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // normal power mode + _i2c_smbus_write_byte_data(file, BNO055_PWR_MODE_ADDR, BNO055_POWER_MODE_NORMAL); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + _i2c_smbus_write_byte_data(file, BNO055_PAGE_ID_ADDR, 0); + _i2c_smbus_write_byte_data(file, BNO055_SYS_TRIGGER_ADDR, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(25)); + + _i2c_smbus_write_byte_data(file, BNO055_OPR_MODE_ADDR, BNO055_OPERATION_MODE_NDOF); + std::this_thread::sleep_for(std::chrono::milliseconds(25)); + + return true; +} + +void BNO055I2CDriver::init() { + + file = open(device.c_str(), O_RDWR); + + if(ioctl(file, I2C_SLAVE, address) < 0) { + throw std::runtime_error("i2c device open failed"); + } + + if(_i2c_smbus_read_byte_data(file, BNO055_CHIP_ID_ADDR) != BNO055_ID) { + throw std::runtime_error("incorrect chip ID"); + } + + std::cerr << "rev ids:" + << " accel:" << _i2c_smbus_read_byte_data(file, BNO055_ACCEL_REV_ID_ADDR) + << " mag:" << _i2c_smbus_read_byte_data(file, BNO055_MAG_REV_ID_ADDR) + << " gyro:" << _i2c_smbus_read_byte_data(file, BNO055_GYRO_REV_ID_ADDR) + << " sw:" << _i2c_smbus_read_word_data(file, BNO055_SW_REV_ID_LSB_ADDR) + << " bl:" << _i2c_smbus_read_byte_data(file, BNO055_BL_REV_ID_ADDR) << std::endl; + + if(!reset()) { + throw std::runtime_error("chip init failed"); + } +} + +IMURecord BNO055I2CDriver::read() { + IMURecord record; + unsigned char c = 0; + + // can only read a length of 0x20 at a time, so do it in 2 reads + // BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR is the start of the data block that aligns with the IMURecord struct + if(_i2c_smbus_read_i2c_block_data(file, BNO055_ACCEL_DATA_X_LSB_ADDR, 0x20, (uint8_t*)&record) != 0x20) { + throw std::runtime_error("read error"); + } + if(_i2c_smbus_read_i2c_block_data(file, BNO055_ACCEL_DATA_X_LSB_ADDR + 0x20, 0x13, (uint8_t*)&record + 0x20) != 0x13) { + throw std::runtime_error("read error"); + } + + return record; +} + +} diff --git a/src/bno055_i2c_node.cpp b/src/bno055_i2c_node.cpp index dec7266..5ce1791 100644 --- a/src/bno055_i2c_node.cpp +++ b/src/bno055_i2c_node.cpp @@ -1,78 +1,234 @@ /* bno055_i2c_node.cpp * Author: Dheera Venkatraman * - * Instantiates a BNO055I2C Activity class, as well as + * Instantiates a BNO055I2CDriver class, as well as * a Watchdog that causes this node to die if things aren't * working. */ -#include +#include #include "watchdog/watchdog.h" #include -int main(int argc, char *argv[]) { - ros::NodeHandle* nh = NULL; - ros::NodeHandle* nh_priv = NULL; +#include +#include +#include +#include +#include +#include +#include +#include +#include - imu_bno055::BNO055I2CActivity* activity = NULL; - watchdog::Watchdog* watchdog = NULL; +#include - ros::init(argc, argv, "bno055_node"); +class BNO055I2CNode { + public: + BNO055I2CNode(int argc, char* argv[]); + void run(); + bool readAndPublish(); + void stop(); + bool onSrvReset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + private: + ros::NodeHandle* nh; + ros::NodeHandle* nh_priv; + std::unique_ptr imu; - nh = new ros::NodeHandle(); - if(!nh) { - ROS_FATAL("Failed to initialize NodeHandle"); - ros::shutdown(); - return -1; - } + std::string param_device; + int param_address; + double param_rate; + std::string param_frame_id; + + diagnostic_msgs::DiagnosticStatus current_status; + ros::Publisher pub_data; + ros::Publisher pub_raw; + ros::Publisher pub_mag; + ros::Publisher pub_temp; + ros::Publisher pub_status; + ros::ServiceServer srv_reset; + + std::unique_ptr rate; + + watchdog::Watchdog watchdog; + + int seq; +}; + +BNO055I2CNode::BNO055I2CNode(int argc, char* argv[]) { + ros::init(argc, argv, "bno055_node"); + nh = new ros::NodeHandle(); nh_priv = new ros::NodeHandle("~"); - if(!nh_priv) { - ROS_FATAL("Failed to initialize private NodeHandle"); - delete nh; + + if(!nh || !nh_priv) { + ROS_FATAL("Failed to initialize node handles"); ros::shutdown(); - return -2; + return; } - activity = new imu_bno055::BNO055I2CActivity(*nh, *nh_priv); - watchdog = new watchdog::Watchdog(); + nh_priv->param("device", param_device, (std::string)"/dev/i2c-1"); + nh_priv->param("address", param_address, (int)BNO055_ADDRESS_A); + nh_priv->param("frame_id", param_frame_id, (std::string)"imu"); + nh_priv->param("rate", param_rate, (double)100); + + imu = std::make_unique(param_device, param_address); - if(!activity) { - ROS_FATAL("Failed to initialize driver"); - delete nh_priv; - delete nh; - ros::shutdown(); - return -3; - } + imu->init(); - if(!activity->start()) { - ROS_ERROR("Failed to start activity"); - delete nh_priv; - delete nh; - ros::shutdown(); - return -4; - } + pub_data = nh->advertise("data", 1); + pub_raw = nh->advertise("raw", 1); + pub_mag = nh->advertise("mag", 1); + pub_temp = nh->advertise("temp", 1); + pub_status = nh->advertise("status", 1); + + srv_reset = nh->advertiseService("reset", &BNO055I2CNode::onSrvReset, this); + + seq = 0; + + + current_status.level = 0; + current_status.name = "BNO055 IMU"; + current_status.hardware_id = "bno055_i2c"; + + diagnostic_msgs::KeyValue calib_stat; + calib_stat.key = "Calibration status"; + calib_stat.value = ""; + current_status.values.push_back(calib_stat); + + diagnostic_msgs::KeyValue selftest_result; + selftest_result.key = "Self-test result"; + selftest_result.value = ""; + current_status.values.push_back(selftest_result); + + diagnostic_msgs::KeyValue intr_stat; + intr_stat.key = "Interrupt status"; + intr_stat.value = ""; + current_status.values.push_back(intr_stat); - watchdog->start(5000); + diagnostic_msgs::KeyValue sys_clk_stat; + sys_clk_stat.key = "System clock status"; + sys_clk_stat.value = ""; + current_status.values.push_back(sys_clk_stat); - int param_rate; - nh_priv->param("rate", param_rate, (int)100); + diagnostic_msgs::KeyValue sys_stat; + sys_stat.key = "System status"; + sys_stat.value = ""; + current_status.values.push_back(sys_stat); - ros::Rate rate(param_rate); + diagnostic_msgs::KeyValue sys_err; + sys_err.key = "System error"; + sys_err.value = ""; + current_status.values.push_back(sys_err); + + + rate =std::make_unique(param_rate); +} + +void BNO055I2CNode::run() { while(ros::ok()) { - rate.sleep(); - if(activity->spinOnce()) { - watchdog->refresh(); + rate->sleep(); + if(readAndPublish()) { + watchdog.refresh(); } } +} + +bool BNO055I2CNode::readAndPublish() { + imu_bno055::IMURecord record; + + try { + record = imu->read(); + } catch(const std::runtime_error& e) { + ROS_WARN_STREAM(e.what()); + } + + ros::Time time = ros::Time::now(); + + sensor_msgs::Imu msg_raw; + msg_raw.header.stamp = time; + msg_raw.header.frame_id = param_frame_id; + msg_raw.header.seq = seq; + msg_raw.linear_acceleration.x = (double)record.raw_linear_acceleration_x / 100.0; + msg_raw.linear_acceleration.y = (double)record.raw_linear_acceleration_y / 100.0; + msg_raw.linear_acceleration.z = (double)record.raw_linear_acceleration_z / 100.0; + msg_raw.angular_velocity.x = (double)record.raw_angular_velocity_x / 900.0; + msg_raw.angular_velocity.y = (double)record.raw_angular_velocity_y / 900.0; + msg_raw.angular_velocity.z = (double)record.raw_angular_velocity_z / 900.0; + + sensor_msgs::MagneticField msg_mag; + msg_mag.header.stamp = time; + msg_mag.header.frame_id = param_frame_id; + msg_mag.header.seq = seq; + msg_mag.magnetic_field.x = (double)record.raw_magnetic_field_x / 16.0; + msg_mag.magnetic_field.y = (double)record.raw_magnetic_field_y / 16.0; + msg_mag.magnetic_field.z = (double)record.raw_magnetic_field_z / 16.0; + + sensor_msgs::Imu msg_data; + msg_data.header.stamp = time; + msg_data.header.frame_id = param_frame_id; + msg_data.header.seq = seq; + + double fused_orientation_norm = std::pow( + std::pow(record.fused_orientation_w, 2) + + std::pow(record.fused_orientation_x, 2) + + std::pow(record.fused_orientation_y, 2) + + std::pow(record.fused_orientation_z, 2), 0.5); + + msg_data.orientation.w = (double)record.fused_orientation_w / fused_orientation_norm; + msg_data.orientation.x = (double)record.fused_orientation_x / fused_orientation_norm; + msg_data.orientation.y = (double)record.fused_orientation_y / fused_orientation_norm; + msg_data.orientation.z = (double)record.fused_orientation_z / fused_orientation_norm; + msg_data.linear_acceleration.x = (double)record.fused_linear_acceleration_x / 100.0; + msg_data.linear_acceleration.y = (double)record.fused_linear_acceleration_y / 100.0; + msg_data.linear_acceleration.z = (double)record.fused_linear_acceleration_z / 100.0; + msg_data.angular_velocity.x = (double)record.raw_angular_velocity_x / 900.0; + msg_data.angular_velocity.y = (double)record.raw_angular_velocity_y / 900.0; + msg_data.angular_velocity.z = (double)record.raw_angular_velocity_z / 900.0; + + sensor_msgs::Temperature msg_temp; + msg_temp.header.stamp = time; + msg_temp.header.frame_id = param_frame_id; + msg_temp.header.seq = seq; + msg_temp.temperature = (double)record.temperature; + + pub_data.publish(msg_data); + pub_raw.publish(msg_raw); + pub_mag.publish(msg_mag); + pub_temp.publish(msg_temp); + + if((seq++) % 50 == 0) { + current_status.values[DIAG_CALIB_STAT].value = std::to_string(record.calibration_status); + current_status.values[DIAG_SELFTEST_RESULT].value = std::to_string(record.self_test_result); + current_status.values[DIAG_INTR_STAT].value = std::to_string(record.interrupt_status); + current_status.values[DIAG_SYS_CLK_STAT].value = std::to_string(record.system_clock_status); + current_status.values[DIAG_SYS_STAT].value = std::to_string(record.system_status); + current_status.values[DIAG_SYS_ERR].value = std::to_string(record.system_error_code); + pub_status.publish(current_status); + } - activity->stop(); - watchdog->stop(); + return true; +} + +void BNO055I2CNode::stop() { + ROS_INFO("stopping"); + if(pub_data) pub_data.shutdown(); + if(pub_raw) pub_raw.shutdown(); + if(pub_mag) pub_mag.shutdown(); + if(pub_temp) pub_temp.shutdown(); + if(pub_status) pub_status.shutdown(); + if(srv_reset) srv_reset.shutdown(); +} - delete watchdog; - delete activity; - delete nh_priv; - delete nh; +bool BNO055I2CNode::onSrvReset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { + if(!(imu->reset())) { + throw std::runtime_error("chip reset failed"); + return false; + } + return true; +} +int main(int argc, char *argv[]) { + BNO055I2CNode node(argc, argv); + node.run(); return 0; }