completely separate BNO055/I2C logic and ROS logic to allow ROS2 node…

… to be created with minimal repetition
dheera · Aug 28, 2021 · 4c80f36 · 4c80f36
1 parent dceeae1
commit 4c80f36
Show file tree

Hide file tree

Showing 6 changed files with 311 additions and 349 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,15 +1,15 @@
 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)
 
 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 → include/imu_bno055/bno055_i2c_driver.h b/include/imu_bno055/bno055_i2c_activity.h → 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 <ros/ros.h>
 #include <cstdlib>
 #include <cerrno>
 #include <cstring>
+#include <stdexcept>
 #include <sys/ioctl.h>
 #include <fcntl.h>
-
-#include <sensor_msgs/Imu.h>
-#include <sensor_msgs/MagneticField.h>
-#include <sensor_msgs/Temperature.h>
-#include <std_srvs/Trigger.h>
-#include <std_msgs/UInt8.h>
-#include <diagnostic_msgs/DiagnosticStatus.h>
-#include <diagnostic_msgs/DiagnosticArray.h>
-#include <diagnostic_msgs/KeyValue.h>
+#include <iostream>
+#include <chrono>
+#include <thread>
 
 #include <linux/i2c-dev.h>
 #include <smbus_functions.h>
@@ -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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>imu_bno055</name>
-  <version>1.0.0</version>
+  <version>2.0.0</version>
   <description>I2C driver for a BNO055 IMU. Tested on Jetson TX2.</description>
   <maintainer email="dheera@dheera.net">dheera</maintainer>
   <license>MIT</license>

diff --git a/src/bno055_i2c_activity.cpp b/src/bno055_i2c_activity.cpp