Skip to content
Snippets Groups Projects
Commit 0d96c67e authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a MagneticField topic publisher.

Added functions to get the raw sensor data.
Published all available data depending on the operational mode.
parent 47c88940
No related branches found
No related tags found
1 merge request!1Kinetic migration
...@@ -148,10 +148,14 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver ...@@ -148,10 +148,14 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver
// here define all bno055_imu_driver interface methods to retrieve and set // here define all bno055_imu_driver interface methods to retrieve and set
// the driver parameters // the driver parameters
std::string get_new_data_event_id(void); 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_quat(void);
std::vector<double> get_orientation_euler(void); std::vector<double> get_orientation_euler(void);
std::vector<double> get_linear_accel(void); std::vector<double> get_linear_accel(void);
std::vector<double> get_gravity(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); bool is_imu_calibrated(void);
std::string get_frame_id(void); std::string get_frame_id(void);
/** /**
......
...@@ -29,6 +29,7 @@ ...@@ -29,6 +29,7 @@
#include "bno055_imu_ros_driver.h" #include "bno055_imu_ros_driver.h"
// [publisher subscriber headers] // [publisher subscriber headers]
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/Imu.h> #include <sensor_msgs/Imu.h>
// [service client headers] // [service client headers]
...@@ -58,6 +59,9 @@ class Bno055ImuDriverNode : public iri_base_driver::IriBaseNodeDriver<Bno055ImuD ...@@ -58,6 +59,9 @@ class Bno055ImuDriverNode : public iri_base_driver::IriBaseNodeDriver<Bno055ImuD
{ {
private: private:
// [publisher attributes] // [publisher attributes]
ros::Publisher magnetometer_publisher_;
sensor_msgs::MagneticField magnetometer_MagneticField_msg_;
ros::Publisher imu_publisher_; ros::Publisher imu_publisher_;
sensor_msgs::Imu imu_Imu_msg_; sensor_msgs::Imu imu_Imu_msg_;
......
...@@ -11,6 +11,7 @@ Bno055ImuDriver::Bno055ImuDriver(void) ...@@ -11,6 +11,7 @@ Bno055ImuDriver::Bno055ImuDriver(void)
bool Bno055ImuDriver::openDriver(void) bool Bno055ImuDriver::openDriver(void)
{ {
//setDriverId(driver string id); //setDriverId(driver string id);
this->lock();
if(this->imu_device!=NULL) if(this->imu_device!=NULL)
{ {
delete this->imu_device; delete this->imu_device;
...@@ -20,6 +21,7 @@ bool Bno055ImuDriver::openDriver(void) ...@@ -20,6 +21,7 @@ bool Bno055ImuDriver::openDriver(void)
this->imu_device=new CBNO055IMUDriver("BNO055_driver"); this->imu_device=new CBNO055IMUDriver("BNO055_driver");
this->imu_device->open(this->config_.serial_device); this->imu_device->open(this->config_.serial_device);
ROS_INFO("IMU successfully opened"); ROS_INFO("IMU successfully opened");
this->unlock();
return true; return true;
}catch(CException &e){ }catch(CException &e){
if(this->imu_device!=NULL) if(this->imu_device!=NULL)
...@@ -28,33 +30,39 @@ bool Bno055ImuDriver::openDriver(void) ...@@ -28,33 +30,39 @@ bool Bno055ImuDriver::openDriver(void)
this->imu_device=NULL; this->imu_device=NULL;
} }
ROS_WARN_STREAM(e.what()); ROS_WARN_STREAM(e.what());
this->unlock();
return false; return false;
} }
} }
bool Bno055ImuDriver::closeDriver(void) bool Bno055ImuDriver::closeDriver(void)
{ {
this->lock();
if(this->imu_device!=NULL) if(this->imu_device!=NULL)
{ {
delete this->imu_device; delete this->imu_device;
this->imu_device=NULL; this->imu_device=NULL;
} }
this->unlock();
return true; return true;
} }
bool Bno055ImuDriver::startDriver(void) bool Bno055ImuDriver::startDriver(void)
{ {
this->lock();
if(this->config_.cal_filename.size()>0) if(this->config_.cal_filename.size()>0)
{ {
try{ try{
this->imu_device->set_operation_mode(config_mode); this->imu_device->set_operation_mode(config_mode);
this->imu_device->load_calibration(this->config_.cal_filename); 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); this->imu_device->set_data_rate(this->config_.update_rate);
ROS_INFO("IMU successfully started"); ROS_INFO("IMU successfully started");
this->unlock();
return true; return true;
}catch(CException &e){ }catch(CException &e){
ROS_WARN_STREAM(e.what()); ROS_WARN_STREAM(e.what());
this->unlock();
return false; return false;
} }
} }
...@@ -63,11 +71,13 @@ bool Bno055ImuDriver::startDriver(void) ...@@ -63,11 +71,13 @@ bool Bno055ImuDriver::startDriver(void)
try{ try{
this->imu_device->set_operation_mode(config_mode); this->imu_device->set_operation_mode(config_mode);
this->imu_device->set_data_rate(this->config_.update_rate); this->imu_device->set_data_rate(this->config_.update_rate);
this->imu_device->set_operation_mode(ndof_mode); this->imu_device->set_operation_mode((op_mode_t)this->config_.mode);
ROS_INFO("IMU successfully stopped"); ROS_INFO("IMU successfully started");
this->unlock();
return true; return true;
}catch(CException &e){ }catch(CException &e){
ROS_WARN_STREAM(e.what()); ROS_WARN_STREAM(e.what());
this->unlock();
return false; return false;
} }
} }
...@@ -75,10 +85,20 @@ bool Bno055ImuDriver::startDriver(void) ...@@ -75,10 +85,20 @@ bool Bno055ImuDriver::startDriver(void)
bool Bno055ImuDriver::stopDriver(void) bool Bno055ImuDriver::stopDriver(void)
{ {
this->lock();
this->imu_device->set_operation_mode(config_mode); this->imu_device->set_operation_mode(config_mode);
this->unlock();
return true; 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) void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
{ {
this->lock(); this->lock();
...@@ -144,6 +164,30 @@ std::vector<double> Bno055ImuDriver::get_gravity(void) ...@@ -144,6 +164,30 @@ std::vector<double> Bno055ImuDriver::get_gravity(void)
return std::vector<double>(); 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 Bno055ImuDriver::is_imu_calibrated(void)
{ {
bool accel_cal,mag_cal,gyro_cal; bool accel_cal,mag_cal,gyro_cal;
......
...@@ -10,7 +10,8 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) : ...@@ -10,7 +10,8 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) :
this->setRate(500);//in [Hz] this->setRate(500);//in [Hz]
// [init publishers] // [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] // [init subscribers]
...@@ -29,39 +30,67 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) : ...@@ -29,39 +30,67 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) :
this->imu_Imu_msg_.orientation_covariance[i]=0.0; this->imu_Imu_msg_.orientation_covariance[i]=0.0;
this->imu_Imu_msg_.angular_velocity_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->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) 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 //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{ try{
this->events.clear(); this->events.clear();
this->events.push_back(this->driver_.get_new_data_event_id()); this->events.push_back(this->driver_.get_new_data_event_id());
this->event_server->wait_all(events,2000); 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(); quaternion=this->driver_.get_orientation_quat();
linear_accel=this->driver_.get_linear_accel(); 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.stamp=ros::Time::now();
this->imu_Imu_msg_.header.frame_id=this->driver_.get_frame_id(); 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.x=quaternion[0];
this->imu_Imu_msg_.orientation.y=quaternion[1]; this->imu_Imu_msg_.orientation.y=quaternion[1];
this->imu_Imu_msg_.orientation.z=quaternion[2]; this->imu_Imu_msg_.orientation.z=quaternion[2];
this->imu_Imu_msg_.orientation.w=quaternion[3]; this->imu_Imu_msg_.orientation.w=quaternion[3];
this->imu_Imu_msg_.linear_acceleration.x=linear_accel[0]; this->imu_Imu_msg_.angular_velocity.x=gyro[0];
this->imu_Imu_msg_.linear_acceleration.y=linear_accel[1]; this->imu_Imu_msg_.angular_velocity.y=gyro[1];
this->imu_Imu_msg_.linear_acceleration.z=linear_accel[2]; 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_); this->imu_publisher_.publish(this->imu_Imu_msg_);
}catch(CException &e){ }catch(CException &e){
ROS_WARN_STREAM(e.what()); ROS_WARN_STREAM(e.what());
} }
} }
this->driver_.unlock();
// [fill msg Header if necessary] // [fill msg Header if necessary]
...@@ -74,7 +103,6 @@ void Bno055ImuDriverNode::mainNodeThread(void) ...@@ -74,7 +103,6 @@ void Bno055ImuDriverNode::mainNodeThread(void)
// [publish messages] // [publish messages]
//unlock access to driver if previously blocked //unlock access to driver if previously blocked
this->driver_.unlock();
} }
/* [subscriber callbacks] */ /* [subscriber callbacks] */
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment