From 0d96c67ee4ddf6992a5e717ba7ccc47641d1893c Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Mon, 3 Feb 2020 18:29:38 +0100 Subject: [PATCH] Added a MagneticField topic publisher. Added functions to get the raw sensor data. Published all available data depending on the operational mode. --- include/bno055_imu_ros_driver.h | 4 +++ include/bno055_imu_ros_driver_node.h | 4 +++ src/bno055_imu_ros_driver.cpp | 50 ++++++++++++++++++++++++++-- src/bno055_imu_ros_driver_node.cpp | 46 ++++++++++++++++++++----- 4 files changed, 92 insertions(+), 12 deletions(-) diff --git a/include/bno055_imu_ros_driver.h b/include/bno055_imu_ros_driver.h index 56f33f9..aa9a6bb 100644 --- a/include/bno055_imu_ros_driver.h +++ b/include/bno055_imu_ros_driver.h @@ -148,10 +148,14 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver // here define all bno055_imu_driver interface methods to retrieve and set // the driver parameters std::string get_new_data_event_id(void); + op_mode_t get_operation_mode(void); std::vector<double> get_orientation_quat(void); std::vector<double> get_orientation_euler(void); std::vector<double> get_linear_accel(void); std::vector<double> get_gravity(void); + std::vector<double> get_raw_accelerometer(void); + std::vector<double> get_raw_magnetometer(void); + std::vector<double> get_raw_gyroscope(void); bool is_imu_calibrated(void); std::string get_frame_id(void); /** diff --git a/include/bno055_imu_ros_driver_node.h b/include/bno055_imu_ros_driver_node.h index 66ab1b0..d4b7a9a 100644 --- a/include/bno055_imu_ros_driver_node.h +++ b/include/bno055_imu_ros_driver_node.h @@ -29,6 +29,7 @@ #include "bno055_imu_ros_driver.h" // [publisher subscriber headers] +#include <sensor_msgs/MagneticField.h> #include <sensor_msgs/Imu.h> // [service client headers] @@ -58,6 +59,9 @@ class Bno055ImuDriverNode : public iri_base_driver::IriBaseNodeDriver<Bno055ImuD { private: // [publisher attributes] + ros::Publisher magnetometer_publisher_; + sensor_msgs::MagneticField magnetometer_MagneticField_msg_; + ros::Publisher imu_publisher_; sensor_msgs::Imu imu_Imu_msg_; diff --git a/src/bno055_imu_ros_driver.cpp b/src/bno055_imu_ros_driver.cpp index f550c85..e232e12 100644 --- a/src/bno055_imu_ros_driver.cpp +++ b/src/bno055_imu_ros_driver.cpp @@ -11,6 +11,7 @@ Bno055ImuDriver::Bno055ImuDriver(void) bool Bno055ImuDriver::openDriver(void) { //setDriverId(driver string id); + this->lock(); if(this->imu_device!=NULL) { delete this->imu_device; @@ -20,6 +21,7 @@ bool Bno055ImuDriver::openDriver(void) this->imu_device=new CBNO055IMUDriver("BNO055_driver"); this->imu_device->open(this->config_.serial_device); ROS_INFO("IMU successfully opened"); + this->unlock(); return true; }catch(CException &e){ if(this->imu_device!=NULL) @@ -28,33 +30,39 @@ bool Bno055ImuDriver::openDriver(void) this->imu_device=NULL; } ROS_WARN_STREAM(e.what()); + this->unlock(); return false; } } bool Bno055ImuDriver::closeDriver(void) { + this->lock(); if(this->imu_device!=NULL) { delete this->imu_device; this->imu_device=NULL; } + this->unlock(); return true; } bool Bno055ImuDriver::startDriver(void) { + this->lock(); if(this->config_.cal_filename.size()>0) { try{ this->imu_device->set_operation_mode(config_mode); this->imu_device->load_calibration(this->config_.cal_filename); - this->imu_device->set_operation_mode(ndof_mode); + this->imu_device->set_operation_mode((op_mode_t)this->config_.mode); this->imu_device->set_data_rate(this->config_.update_rate); ROS_INFO("IMU successfully started"); + this->unlock(); return true; }catch(CException &e){ ROS_WARN_STREAM(e.what()); + this->unlock(); return false; } } @@ -63,11 +71,13 @@ bool Bno055ImuDriver::startDriver(void) try{ this->imu_device->set_operation_mode(config_mode); this->imu_device->set_data_rate(this->config_.update_rate); - this->imu_device->set_operation_mode(ndof_mode); - ROS_INFO("IMU successfully stopped"); + this->imu_device->set_operation_mode((op_mode_t)this->config_.mode); + ROS_INFO("IMU successfully started"); + this->unlock(); return true; }catch(CException &e){ ROS_WARN_STREAM(e.what()); + this->unlock(); return false; } } @@ -75,10 +85,20 @@ bool Bno055ImuDriver::startDriver(void) bool Bno055ImuDriver::stopDriver(void) { + this->lock(); this->imu_device->set_operation_mode(config_mode); + this->unlock(); return true; } +op_mode_t Bno055ImuDriver::get_operation_mode(void) +{ + if(this->imu_device!=NULL) + return this->imu_device->get_operation_mode(); + else + return config_mode; +} + void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level) { this->lock(); @@ -144,6 +164,30 @@ std::vector<double> Bno055ImuDriver::get_gravity(void) return std::vector<double>(); } +std::vector<double> Bno055ImuDriver::get_raw_accelerometer(void) +{ + if(this->imu_device!=NULL) + return this->imu_device->get_raw_accelerometer(); + else + return std::vector<double>(); +} + +std::vector<double> Bno055ImuDriver::get_raw_magnetometer(void) +{ + if(this->imu_device!=NULL) + return this->imu_device->get_raw_magnetometer(); + else + return std::vector<double>(); +} + +std::vector<double> Bno055ImuDriver::get_raw_gyroscope(void) +{ + if(this->imu_device!=NULL) + return this->imu_device->get_raw_gyroscope(); + else + return std::vector<double>(); +} + bool Bno055ImuDriver::is_imu_calibrated(void) { bool accel_cal,mag_cal,gyro_cal; diff --git a/src/bno055_imu_ros_driver_node.cpp b/src/bno055_imu_ros_driver_node.cpp index 4db5265..c89b5bf 100644 --- a/src/bno055_imu_ros_driver_node.cpp +++ b/src/bno055_imu_ros_driver_node.cpp @@ -10,7 +10,8 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) : this->setRate(500);//in [Hz] // [init publishers] - this->imu_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("imu", 1); + this->magnetometer_publisher_ = this->private_node_handle_.advertise<sensor_msgs::MagneticField>("magnetometer", 1); + this->imu_publisher_ = this->private_node_handle_.advertise<sensor_msgs::Imu>("imu", 1); // [init subscribers] @@ -29,39 +30,67 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) : this->imu_Imu_msg_.orientation_covariance[i]=0.0; this->imu_Imu_msg_.angular_velocity_covariance[i]=0.0; this->imu_Imu_msg_.linear_acceleration_covariance[i]=0.0; + this->magnetometer_MagneticField_msg_.magnetic_field_covariance[i]=0.0; } - this->imu_Imu_msg_.angular_velocity_covariance[0]=-1.0; } void Bno055ImuDriverNode::mainNodeThread(void) { - std::vector<double> quaternion,linear_accel; + std::vector<double> quaternion,linear_accel,mag,gyro,accel; + op_mode_t op_mode; //lock access to driver if necessary - this->driver_.lock(); - if(this->driver_.is_imu_calibrated()) + this->driver_.lock(); + if(this->driver_.getState()==iri_base_driver::RUNNING) { + this->driver_.is_imu_calibrated(); try{ this->events.clear(); this->events.push_back(this->driver_.get_new_data_event_id()); this->event_server->wait_all(events,2000); + op_mode=this->driver_.get_operation_mode(); + if(op_mode==mag_only_mode || op_mode==acc_mag_mode || op_mode==mag_gyro_mode || op_mode==acc_mag_gyro_mode || op_mode==compass_mode || op_mode==m4g_mode || op_mode==ndof_off_mode || op_mode==ndof_mode) + { + mag=this->driver_.get_raw_magnetometer(); + this->magnetometer_MagneticField_msg_.header.stamp=ros::Time::now(); + this->magnetometer_MagneticField_msg_.header.frame_id=this->driver_.get_frame_id(); + this->magnetometer_MagneticField_msg_.magnetic_field.x=mag[0]; + this->magnetometer_MagneticField_msg_.magnetic_field.y=mag[1]; + this->magnetometer_MagneticField_msg_.magnetic_field.z=mag[2]; + this->magnetometer_publisher_.publish(this->magnetometer_MagneticField_msg_); + } quaternion=this->driver_.get_orientation_quat(); linear_accel=this->driver_.get_linear_accel(); + accel=this->driver_.get_raw_accelerometer(); + gyro=this->driver_.get_raw_gyroscope(); this->imu_Imu_msg_.header.stamp=ros::Time::now(); this->imu_Imu_msg_.header.frame_id=this->driver_.get_frame_id(); this->imu_Imu_msg_.orientation.x=quaternion[0]; this->imu_Imu_msg_.orientation.y=quaternion[1]; this->imu_Imu_msg_.orientation.z=quaternion[2]; this->imu_Imu_msg_.orientation.w=quaternion[3]; - this->imu_Imu_msg_.linear_acceleration.x=linear_accel[0]; - this->imu_Imu_msg_.linear_acceleration.y=linear_accel[1]; - this->imu_Imu_msg_.linear_acceleration.z=linear_accel[2]; + this->imu_Imu_msg_.angular_velocity.x=gyro[0]; + this->imu_Imu_msg_.angular_velocity.y=gyro[1]; + this->imu_Imu_msg_.angular_velocity.z=gyro[2]; + if(op_mode>=imu_mode) + { + this->imu_Imu_msg_.linear_acceleration.x=linear_accel[0]; + this->imu_Imu_msg_.linear_acceleration.y=linear_accel[1]; + this->imu_Imu_msg_.linear_acceleration.z=linear_accel[2]; + } + else + { + this->imu_Imu_msg_.linear_acceleration.x=accel[0]; + this->imu_Imu_msg_.linear_acceleration.y=accel[1]; + this->imu_Imu_msg_.linear_acceleration.z=accel[2]; + } this->imu_publisher_.publish(this->imu_Imu_msg_); }catch(CException &e){ ROS_WARN_STREAM(e.what()); } } + this->driver_.unlock(); // [fill msg Header if necessary] @@ -74,7 +103,6 @@ void Bno055ImuDriverNode::mainNodeThread(void) // [publish messages] //unlock access to driver if previously blocked - this->driver_.unlock(); } /* [subscriber callbacks] */ -- GitLab