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

Migration to the ROS Kinetic.

parent 5344cc43
No related branches found
No related tags found
1 merge request!1Kinetic migration
cmake_minimum_required(VERSION 2.8.3)
project(bno055_imu)
project(iri_bno055_imu_driver)
## Find catkin macros and libraries
find_package(catkin REQUIRED)
......
......@@ -31,9 +31,9 @@
# Author:
PACKAGE='bno055_imu'
PACKAGE='iri_bno055_imu_driver'
from driver_base.msg import SensorLevels
from iri_base_driver.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
......
......@@ -26,7 +26,7 @@
#define _bno055_imu_driver_h_
#include <iri_base_driver/iri_base_driver.h>
#include <bno055_imu/Bno055ImuConfig.h>
#include <iri_bno055_imu_driver/Bno055ImuConfig.h>
//include bno055_imu_driver main library
#include "bno055_imu_driver.h"
......@@ -64,7 +64,7 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver
* Define a Config type with the Bno055ImuConfig. All driver implementations
* will then use the same variable type Config.
*/
typedef bno055_imu::Bno055ImuConfig Config;
typedef iri_bno055_imu_driver::Bno055ImuConfig Config;
/**
* \brief config variable
......
<?xml version="1.0"?>
<package>
<name>bno055_imu</name>
<name>iri_bno055_imu_driver</name>
<version>0.0.0</version>
<description>The bno055_imu package</description>
<description>The iri_bno055_imu_driver package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
......@@ -19,7 +19,7 @@
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/bno055_imu</url> -->
<!-- <url type="website">http://wiki.ros.org/iri_bno055_imu_driver</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
......@@ -51,4 +51,4 @@
<!-- Other tools can request additional information be placed here -->
</export>
</package>
\ No newline at end of file
</package>
......@@ -19,6 +19,7 @@ bool Bno055ImuDriver::openDriver(void)
try{
this->imu_device=new CBNO055IMUDriver("BNO055_driver");
this->imu_device->open(this->config_.serial_device);
ROS_INFO("IMU successfully opened");
return true;
}catch(CException &e){
if(this->imu_device!=NULL)
......@@ -50,6 +51,7 @@ bool Bno055ImuDriver::startDriver(void)
this->imu_device->load_calibration(this->config_.cal_filename);
this->imu_device->set_operation_mode(ndof_mode);
this->imu_device->set_data_rate(this->config_.update_rate);
ROS_INFO("IMU successfully started");
return true;
}catch(CException &e){
ROS_WARN_STREAM(e.what());
......@@ -62,6 +64,7 @@ bool Bno055ImuDriver::startDriver(void)
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");
return true;
}catch(CException &e){
ROS_WARN_STREAM(e.what());
......@@ -84,14 +87,14 @@ void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
// update driver with new_cfg data
switch(this->getState())
{
case Bno055ImuDriver::CLOSED:
case iri_base_driver::CLOSED:
this->frame_id=new_cfg.tf_prefix+"/"+new_cfg.frame_id;
break;
case Bno055ImuDriver::OPENED:
case iri_base_driver::OPENED:
break;
case Bno055ImuDriver::RUNNING:
case iri_base_driver::RUNNING:
break;
}
......
......@@ -7,7 +7,7 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) :
unsigned int i;
//init class attributes if necessary
this->loop_rate_ = 500;//in [Hz]
this->setRate(500);//in [Hz]
// [init publishers]
this->imu_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("imu", 1);
......@@ -117,5 +117,5 @@ Bno055ImuDriverNode::~Bno055ImuDriverNode(void)
/* main function */
int main(int argc,char *argv[])
{
return driver_base::main<Bno055ImuDriverNode>(argc, argv, "bno055_imu_driver_node");
return iri_base_driver::main<Bno055ImuDriverNode>(argc, argv, "bno055_imu_driver_node");
}
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