--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(bno055_driver)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ eigen_conversions
+ geometry_msgs
+ sensor_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(bno055 REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+add_library(bno055 STATIC IMPORTED) # or STATIC instead of SHARED
+set_target_properties(bno055 PROPERTIES
+ IMPORTED_LOCATION "$ENV{HOME}/doc/ufrgs/miotec/bno055/BNO055_driver/libbno055.a"
+ INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/doc/ufrgs/miotec/bno055/BNO055_driver"
+)
+add_library(bno055ifc STATIC IMPORTED) # or STATIC instead of SHARED
+set_target_properties(bno055ifc PROPERTIES
+ IMPORTED_LOCATION "$ENV{HOME}/lib/libbno055ifc.a"
+ INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/include"
+)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# geometry_msgs
+# sensor_msgs
+# eigen_conversions
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES bno055_driver
+# CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs eigen_conversions
+# DEPENDS bno055 Eigen3
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+# TODO: Check names of system library include directories (bno055, Eigen3)
+# ${bno055_INCLUDE_DIRS}
+ ${EIGEN3_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/bno055_driver.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(bno055_node src/bno055_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(bno055_node
+ ${catkin_LIBRARIES}
+# ${bno055_LIBRARIES}
+ bno055ifc
+ bno055
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_bno055_driver.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- /dev/null
+<launch>
+ <node pkg="bno055_driver" type="bno055_node" name="bno055">
+ <param name="device" value="/dev/ttyUSB0" />
+ </node>
+</launch>
+
\ No newline at end of file
--- /dev/null
+<launch>
+ <include file="$(find bno055_driver)/launch/bno055.launch"/>
+ <include file="$(find s7edge_description)/launch/s7edge.launch"/>
+ <node pkg="tf2_ros" type="static_transform_publisher"
+ name="imu_origin_publisher"
+ args="0 0 0 0 0 0 1 map imu" />
+ <node pkg="imu_odometry" type="imu2tf" name="imu2tf" args="origin_link" />
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find bno055_driver)/rviz/display.rviz" />
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<package format="2">
+ <name>bno055_driver</name>
+ <version>0.0.0</version>
+ <description>The bno055_driver package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/bno055_driver</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>bno055</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>eigen_conversions</build_depend>
+ <build_export_depend>bno055</build_export_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>geometry_msgs</build_export_depend>
+ <build_export_depend>sensor_msgs</build_export_depend>
+ <build_export_depend>eigen_conversions</build_export_depend>
+ <exec_depend>bno055</exec_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>sensor_msgs</exec_depend>
+ <exec_depend>eigen_conversions</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /RobotModel1
+ - /RobotModel1/Links1
+ - /RobotModel1/Links1/base_link1
+ Splitter Ratio: 0.3928571343421936
+ Tree Height: 558
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Preferences:
+ PromptSaveOnExit: true
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: true
+ Show Trail: false
+ Value: true
+ origin_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Alpha: 1
+ Class: rviz_plugin_tutorials/Imu
+ Color: 204; 51; 204
+ Enabled: true
+ History Length: 1
+ Name: Imu
+ Topic: /imu
+ Unreliable: false
+ Value: true
+ - Class: rviz/Axes
+ Enabled: true
+ Length: 0.10000000149011612
+ Name: Axes
+ Radius: 0.009999999776482582
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: false
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: temperature
+ Class: rviz/Temperature
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: true
+ Max Color: 255; 255; 255
+ Max Intensity: 100
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: Temperature
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.10000000149011612
+ Style: Flat Squares
+ Topic: /temperature
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ Enabled: true
+ Global Options:
+ Background Color: 249; 249; 249
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 1.749889850616455
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: -0.19180631637573242
+ Y: -0.2178049087524414
+ Z: 0.2896389067173004
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.785398006439209
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 0.785398006439209
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001de000002b6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000002b6000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000002b60000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055c0000003efc0100000002fb0000000800540069006d006501000000000000055c0000024200fffffffb0000000800540069006d00650100000000000004500000000000000000000002630000020d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1372
+ X: 32
+ Y: 0
--- /dev/null
+/******************************************************************************
+ ROS BNO055 Driver
+ BNO055 IMU Publisher Node
+ Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <bno055_tty.h>
+//#include <bno055_i2c.h>
+#include <bno055_units.h>
+
+#include <ros/node_handle.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/Temperature.h>
+#include <sensor_msgs/MagneticField.h>
+#include <eigen_conversions/eigen_msg.h>
+
+#include <Eigen/Dense>
+using namespace Eigen;
+
+class Bno055Node
+{
+ public:
+ Bno055Node(ros::NodeHandle node);
+ ~Bno055Node(void);
+ void sample(void);
+ void publish(void) const;
+
+ private:
+ struct bno055_t bno055_;
+ ros::NodeHandle node_;
+ ros::Publisher imuPub_;
+ ros::Publisher temperaturePub_;
+ ros::Publisher magneticFieldPub_;
+ ros::Publisher accelPub_;
+ ros::Publisher gravityPub_;
+
+ bool uart_;
+
+ Vector3d accel_;
+ Vector3d mag_;
+ Vector3d gyro_;
+
+ Quaterniond quaternion_;
+ Vector3d linearAccel_;
+ Vector3d gravity_;
+
+ double temp_;
+};
+
+Bno055Node::Bno055Node(ros::NodeHandle node): uart_(false)
+{
+ node_=node;
+
+ ros::NodeHandle nh("~");
+ std::string device;
+ if(!nh.getParam("device",device))
+ {
+ ROS_ERROR_STREAM("Could not find parameter: device.");
+ exit(-1);
+ }
+
+ std::string addressStr;
+ if(!nh.getParam("address",addressStr)) uart_=true;
+
+ int mode;
+ nh.param("mode",mode,BNO055_OPERATION_MODE_NDOF);
+
+ if(uart_)
+ {
+ if(BNO055_tty_init(device.c_str(),&bno055_))
+ {
+ ROS_ERROR_STREAM("Error initializing TTY interface");
+ exit(-1);
+ }
+ }
+ else
+ {
+ u8 address=std::stoi(addressStr,NULL,0);
+// if(BNO055_i2c_init(device.c_str(),address,&bno055_))
+ {
+ perror("Error initializing I2C interface");
+ exit(-1);
+ }
+ }
+
+ if(bno055_init(&bno055_) != BNO055_SUCCESS)
+ ROS_ERROR_STREAM("Error initializing bno055.");
+
+ u8 stat_clk;
+ if(bno055_get_stat_main_clk(&stat_clk) != BNO055_SUCCESS)
+ ROS_ERROR_STREAM("Error reading clock status.");
+ else
+ {
+ if(stat_clk==0)
+ {
+ if(bno055_set_clk_src(1) != BNO055_SUCCESS)
+ ROS_ERROR_STREAM("Error setting clock source.");
+ }
+ else ROS_ERROR_STREAM("Clock source can't be changed.");
+ }
+
+ if(bno055_set_accel_unit(BNO055_ACCEL_UNIT_MSQ) != BNO055_SUCCESS)
+ ROS_ERROR_STREAM("Error setting acceleration unit to m/s^2.");
+
+ if(bno055_set_gyro_unit(BNO055_GYRO_UNIT_RPS) != BNO055_SUCCESS)
+ ROS_ERROR_STREAM("Error setting gyroscope unit to rad/s.");
+
+ if(bno055_set_temp_unit(BNO055_TEMP_UNIT_CELSIUS) != BNO055_SUCCESS)
+ ROS_ERROR_STREAM("Error setting temperature unit to Celsius.");
+
+ if(bno055_set_data_output_format(0x01) != BNO055_SUCCESS)
+ ROS_ERROR_STREAM("Error setting data output format.");
+
+ if(bno055_set_operation_mode(mode) != BNO055_SUCCESS)
+ ROS_ERROR_STREAM("Error setting operation mode.");
+
+ imuPub_=node_.advertise<sensor_msgs::Imu>("imu",100);
+ temperaturePub_=node_.advertise<sensor_msgs::Temperature>("temperature",100);
+ magneticFieldPub_=node_.advertise<sensor_msgs::MagneticField>("magnetic_field",100);
+ accelPub_=node_.advertise<geometry_msgs::Vector3Stamped>("acceleration",100);
+ gravityPub_=node_.advertise<geometry_msgs::Vector3Stamped>("gravity",100);
+}
+
+Bno055Node::~Bno055Node(void)
+{
+ imuPub_.shutdown();
+ temperaturePub_.shutdown();
+ magneticFieldPub_.shutdown();
+ accelPub_.shutdown();
+ gravityPub_.shutdown();
+
+ if(uart_) BNO055_tty_close(&bno055_);
+// else BNO055_i2c_close(&bno055_);
+}
+
+void Bno055Node::publish(void) const
+{
+ ros::Time time=ros::Time::now();
+
+ sensor_msgs::Imu imuMsg;
+ imuMsg.header.stamp=time;
+ imuMsg.header.frame_id="imu";
+
+ tf::quaternionEigenToMsg(quaternion_,imuMsg.orientation);
+ for (int i=0; i < 9; i++) imuMsg.orientation_covariance[i]=0.0;
+ imuMsg.orientation_covariance[0]=0.0;
+ imuMsg.orientation_covariance[3]=0.0;
+ imuMsg.orientation_covariance[6]=0.0;
+
+ tf::vectorEigenToMsg(gyro_,imuMsg.angular_velocity);
+ for (int i=0; i < 9; i++) imuMsg.angular_velocity_covariance[i]=0.0;
+ imuMsg.angular_velocity_covariance[0]=0.0;
+ imuMsg.angular_velocity_covariance[3]=0.0;
+ imuMsg.angular_velocity_covariance[6]=0.0;
+
+ tf::vectorEigenToMsg(linearAccel_,imuMsg.linear_acceleration);
+ for (int i=0; i < 9; i++) imuMsg.linear_acceleration_covariance[i]=0.0;
+ imuMsg.linear_acceleration_covariance[0]=0.0;
+ imuMsg.linear_acceleration_covariance[3]=0.0;
+ imuMsg.linear_acceleration_covariance[6]=0.0;
+
+ imuPub_.publish(imuMsg);
+
+ sensor_msgs::Temperature temperatureMsg;
+ temperatureMsg.header.stamp=time;
+ temperatureMsg.header.frame_id="imu";
+
+ temperatureMsg.temperature=temp_;
+ temperatureMsg.variance=0.0;
+
+ temperaturePub_.publish(temperatureMsg);
+
+ sensor_msgs::MagneticField magneticFieldMsg;
+ magneticFieldMsg.header.stamp=time;
+ magneticFieldMsg.header.frame_id="imu";
+
+ tf::vectorEigenToMsg(mag_,magneticFieldMsg.magnetic_field);
+ for (int i=0; i < 9; i++) magneticFieldMsg.magnetic_field_covariance[i]=0.0;
+ magneticFieldMsg.magnetic_field_covariance[0]=0.0;
+ magneticFieldMsg.magnetic_field_covariance[3]=0.0;
+ magneticFieldMsg.magnetic_field_covariance[6]=0.0;
+
+ magneticFieldPub_.publish(magneticFieldMsg);
+
+ geometry_msgs::Vector3Stamped accelMsg;
+ accelMsg.header.stamp=time;
+ accelMsg.header.frame_id="imu";
+
+ tf::vectorEigenToMsg(accel_,accelMsg.vector);
+
+ accelPub_.publish(accelMsg);
+
+ geometry_msgs::Vector3Stamped gravityMsg;
+ gravityMsg.header.stamp=time;
+ gravityMsg.header.frame_id="imu";
+
+ tf::vectorEigenToMsg(accel_,gravityMsg.vector);
+
+ gravityPub_.publish(gravityMsg);
+
+}
+
+void Bno055Node::sample(void)
+{
+ struct bno055_accel_t accel;
+ if(bno055_read_accel_xyz(&accel) == BNO055_SUCCESS)
+ accel_ << accel.x/BNO055_ACCEL_DIV_MSQ,
+ accel.y/BNO055_ACCEL_DIV_MSQ,
+ accel.z/BNO055_ACCEL_DIV_MSQ;
+ else ROS_ERROR_STREAM("Error reading acceleration.");
+
+ struct bno055_mag_t mag;
+ if(bno055_read_mag_xyz(&mag) == BNO055_SUCCESS)
+ mag_ << mag.x/BNO055_MAG_DIV_UT*1e-6,
+ mag.y/BNO055_MAG_DIV_UT*1e-6,
+ mag.z/BNO055_MAG_DIV_UT*1e-6;
+ else ROS_ERROR_STREAM("Error reading magnetometer.");
+
+ struct bno055_gyro_t gyro;
+ if(bno055_read_gyro_xyz(&gyro) == BNO055_SUCCESS)
+ gyro_ << gyro.x/BNO055_GYRO_DIV_RPS,
+ gyro.y/BNO055_GYRO_DIV_RPS,
+ gyro.z/BNO055_GYRO_DIV_RPS;
+ else ROS_ERROR_STREAM("Error reading gyroscope.");
+
+ s8 temp;
+ if(bno055_read_temp_data(&temp) == BNO055_SUCCESS)
+ temp_=temp/BNO055_TEMP_DIV_CELSIUS;
+ else ROS_ERROR_STREAM("Error reading temperature.");
+
+ struct bno055_quaternion_t quaternion;
+ if(bno055_read_quaternion_wxyz(&quaternion) == BNO055_SUCCESS)
+ {
+ quaternion_.w()=quaternion.w/BNO055_QUATERNION_DIV;
+ quaternion_.x()=quaternion.x/BNO055_QUATERNION_DIV;
+ quaternion_.y()=quaternion.y/BNO055_QUATERNION_DIV;
+ quaternion_.z()=quaternion.z/BNO055_QUATERNION_DIV;
+ }
+ else ROS_ERROR_STREAM("Error reading quaternion.");
+
+ struct bno055_linear_accel_t linearAccel;
+ if(bno055_read_linear_accel_xyz(&linearAccel) == BNO055_SUCCESS)
+ linearAccel_ << linearAccel.x/BNO055_LINEAR_ACCEL_DIV_MSQ,
+ linearAccel.y/BNO055_LINEAR_ACCEL_DIV_MSQ,
+ linearAccel.z/BNO055_LINEAR_ACCEL_DIV_MSQ;
+ else ROS_ERROR_STREAM("Error reading linear acceleration.");
+
+ struct bno055_gravity_t gravity;
+ if(bno055_read_gravity_xyz(&gravity) == BNO055_SUCCESS)
+ gravity_ << gravity.x/BNO055_GRAVITY_DIV_MSQ,
+ gravity.y/BNO055_GRAVITY_DIV_MSQ,
+ gravity.z/BNO055_GRAVITY_DIV_MSQ;
+ else ROS_ERROR_STREAM("Error reading gravity.");
+}
+
+int main(int argc,char* argv[])
+{
+ ros::init(argc,argv,"bno055_node");
+ ros::NodeHandle node;
+
+ Bno055Node bno055Node(node);
+
+ ros::Rate loop(100);
+ while(ros::ok())
+ {
+ bno055Node.sample();
+ bno055Node.publish();
+
+ ros::spinOnce();
+ loop.sleep();
+ }
+ return 0;
+}
projects / bno055_driver.git / commitdiff