Bosh BNO055 IMU implementation

diffbot_base/config/base.yaml

@@ -8,5 +8,5 @@ diffbot:
   motor_constant: 1.0 #27.0
   pwm_limit: 1.0
   debug:
-    hardware_interface: false
+    hardware_interface: true
     base_controller: true

diffbot_base/include/diffbot_base/diffbot_hw_interface.h

@@ -9,6 +9,8 @@
 #include <diffbot_msgs/WheelsCmdStamped.h>
 #include <diffbot_msgs/AngularVelocitiesStamped.h>
 #include <sensor_msgs/JointState.h>
+#include <sensor_msgs/Imu.h>
+#include <diffbot_msgs/IMU.h>
 
 // ROS Controls
 #include <hardware_interface/robot_hw.h>
@@ -123,6 +125,11 @@ namespace diffbot_base
 
         /** \brief Callback to receive the encoder ticks from Teensy MCU */
         void encoderTicksCallback(const diffbot_msgs::EncodersStamped::ConstPtr& msg_encoders);
+
+        /** \brief Callback to receive the IMU from MCU */
+        void IMUDataCallback(const diffbot_msgs::IMU::ConstPtr& msg_imu);
+
+        void PublishIMUtoFilter();
 
         /** \brief Callback to receive the measured joint states from Teensy MCU */
         void measuredJointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg_joint_states);
@@ -166,7 +173,8 @@ namespace diffbot_base
         std::vector<std::string> joint_names_;
         std::size_t num_joints_;
         urdf::Model *urdf_model_;
-
+        // IMU message package
+        sensor_msgs::Imu imu;
         double wheel_radius_;
         double wheel_diameter_;
         double max_velocity_;
@@ -201,17 +209,25 @@ namespace diffbot_base
         double joint_velocities_[NUM_JOINTS];
         double joint_efforts_[NUM_JOINTS];
 
+        // Data that handles IMU data from diffbot_msgs format to sensor_msgs/Imu.h
+        double orientation[4];
+        double angular_velocity[3];
+        double linear_acceleration[3];
+        int IMU_data_[10];
+
         ros::ServiceServer srv_start_;
         ros::ServiceServer srv_stop_;
 
         // Declare publishers for the motor driver
         ros::Publisher pub_left_motor_value_;
         ros::Publisher pub_right_motor_value_;
 
-
         // Declare publishers for angular wheel joint velocities
         ros::Publisher pub_wheel_cmd_velocities_;
 
+        // Declare publisher for the IMU
+        ros::Publisher imu_pub_;
+
         // Declare publisher to reset the wheel encoders
         // used during first launch of hardware interface to avoid large difference in encoder ticks from a previous run
         ros::Publisher pub_reset_encoders_;
@@ -222,6 +238,9 @@ namespace diffbot_base
         // This subscriber receives the measured angular wheel joint velocity in the custom diffbot_msgs/AngularVelocitiesStamped message
         ros::Subscriber sub_measured_joint_states_;
 
+        // This subscriber receives the IMU data from the Microcontroller
+        ros::Subscriber sub_imu_data_;
+
         // Array to store the received encoder tick values from the \ref sub_encoder_ticks_ subscriber
         int encoder_ticks_[NUM_JOINTS];
         JointState measured_joint_states_[NUM_JOINTS];

diffbot_base/launch/diffbot.launch

@@ -25,4 +25,7 @@
     <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
         output="screen" ns="diffbot" args="joint_state_controller
                                             mobile_base_controller"/>
+
+    <!-- Load EKF Filter to fuse IMU with Odometry -->
+    <include file="$(find diffbot_control)/launch/diffbot_filter.launch"/>
 </launch>

diffbot_base/scripts/encoders/encoders/encoders.ino

@@ -97,4 +97,4 @@ void loop() {
     positionLeft = newLeft;
     positionRight = newRight;
   }
-}
+}

diffbot_base/scripts/encoders/encoders_imu/encoders_imu.ino

@@ -0,0 +1,157 @@
+/* Encoder Library - TwoKnobs Example
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+
+#include <ros.h>
+//#include <std_msgs/Int32.h>
+#include <diffbot_msgs/Encoders.h>
+#include <diffbot_msgs/IMU.h>
+#include <std_msgs/Empty.h>
+
+#include <Encoder.h>
+
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+
+
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
+
+double orientation[4];
+double angular_velocity[3];
+double linear_acceleration[3];
+
+// Encoder setup
+// Change these pin numbers to the pins connected to your encoder.
+//   Best Performance: both pins have interrupt capability
+//   Good Performance: only the first pin has interrupt capability
+//   Low Performance:  neither pin has interrupt capability
+Encoder encoderLeft(5, 6);  // Default pins 5, 6
+Encoder encoderRight(7, 8); // Default pins 7, 8
+//   avoid using pins with LEDs attached
+
+ros::NodeHandle nh;
+
+// ROS Subscriber setup to reset both encoders to zero
+void resetCallback( const std_msgs::Empty& reset)
+{
+  //digitalWrite(13, HIGH-digitalRead(13));   // blink the led
+  // reset both back to zero.
+  encoderLeft.write(0);
+  encoderRight.write(0);
+  nh.loginfo("Reset both wheel encoders to zero");
+}
+
+ros::Subscriber<std_msgs::Empty> sub_reset("reset", resetCallback);
+
+// ROS Publisher setup to publish left and right encoder ticks
+// This uses the custom encoder ticks message that defines an array of two integers
+diffbot_msgs::Encoders encoder_ticks;
+ros::Publisher pub_encoders("encoder_ticks", &encoder_ticks);
+
+// This uses the custom IMU imu_msg message that defines an array of 10 integers
+// The first 4 integers for Orientation (4th is Z angular velocity, becuse <sensor_msgs/Imu.h> structure)
+// Next 3 integers for Acceleration
+// Next 3 integers for Angular Velocity
+diffbot_msgs::IMU imu_data;
+ros::Publisher pub_imu("imu", &imu_data);
+
+void setup()
+{
+
+  // Encoder
+  nh.initNode();
+  nh.advertise(pub_encoders);
+  nh.subscribe(sub_reset);
+
+  while (!nh.connected())
+  {
+    nh.spinOnce();
+  }
+  nh.loginfo("Initialize DiffBot Wheel Encoders");
+  std_msgs::Empty reset;
+  resetCallback(reset);
+  delay(1);
+
+  // IMU
+  nh.advertise(pub_imu);
+  nh.loginfo("Initialize Diffbot Axis Data");
+  delay(1);
+  bno.begin();
+  //if (!bno.begin())
+  //{
+  //  /* There was a problem detecting the BNO055 ... check your connections */
+  //  Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+  //  while (1)
+  //    ;
+  //}
+}
+
+long positionLeft  = -999;
+long positionRight = -999;
+
+void loop() {
+  long newLeft, newRight;
+  newLeft = encoderLeft.read();
+  newRight = encoderRight.read();
+
+  encoder_ticks.ticks[0] = newLeft;
+  encoder_ticks.ticks[1] = newRight;
+  // Use at least a delay of 3 ms on the work pc and 5 ms on the Raspberry Pi
+  // Too low delay causes errors in rosserial similar to the following:
+  // [INFO]: wrong checksum for topic id and msg
+  // [INFO]: Wrong checksum for msg length, length 4, dropping message.
+  // [ERROR]: Mismatched protocol version in packet (b'\x00'): lost sync or rosserial_python is from different ros release than the rosserial client
+  // [INFO]: Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+)
+  // [INFO]: wrong checksum for topic id and msg
+  // [WARN]: Last read step: message length
+  // [WARN]: Run loop error: Serial Port read failure: Returned short (expected 3 bytes, received 2 instead).
+  // [INFO]: Requesting topics...
+  // [ERROR]: Lost sync with device, restarting...
+  // [INFO]: Requesting topics...
+
+  // Note that https://www.arduino.cc/reference/en/language/functions/time/delay/:
+  // Certain things do go on while the delay() function is controlling the Atmega chip,
+  // however, because the delay function does not disable interrupts.
+  // Serial communication that appears at the RX pin is recorded, PWM (analogWrite) values and pin states are maintained,
+  // and interrupts will work as they should.
+
+  // IMU CODE SECTION
+
+  // Get a new IMU sensor event //
+  sensors_event_t orien, accel, gyro;
+  bno.getEvent(&orien, Adafruit_BNO055::VECTOR_EULER);
+  bno.getEvent(&accel), Adafruit_BNO055::VECTOR_LINEARACCEL;
+  bno.getEvent(&gyro, Adafruit_BNO055::VECTOR_GYROSCOPE);
+
+  // Bulding IMU message into array that will get publish to ROS
+  imu_data.imu_msg[0] = orien.orientation.x;
+  imu_data.imu_msg[1] = orien.orientation.y;
+  imu_data.imu_msg[2] = orien.orientation.z;
+  imu_data.imu_msg[3] = accel.acceleration.x;
+  imu_data.imu_msg[4] = accel.acceleration.y;
+  imu_data.imu_msg[5] = accel.acceleration.z;
+  imu_data.imu_msg[6] = gyro.gyro.x;
+  imu_data.imu_msg[7] = gyro.gyro.y;
+  imu_data.imu_msg[8] = gyro.gyro.z;
+  imu_data.imu_msg[9] = bno.getTemp();
+
+  // Publishing Nodes
+  pub_imu.publish(&imu_data);
+  pub_encoders.publish(&encoder_ticks);
+  nh.spinOnce();
+  delay(5);
+
+  if (newLeft != positionLeft || newRight != positionRight)
+  {
+    //String str = "Left = " + String(newLeft) + ", Right = " + String(newRight);
+    //nh.loginfo(str);
+    positionLeft = newLeft;
+    positionRight = newRight;
+  }
+
+}

diffbot_base/src/diffbot_hw_interface.cpp

@@ -62,12 +62,18 @@ namespace diffbot_base
         //Setup publisher for angular wheel joint velocity commands
         pub_wheel_cmd_velocities_ = nh_.advertise<diffbot_msgs::WheelsCmdStamped>("wheel_cmd_velocities", 10);
 
+        // Setup publisher for the IMU format transform
+        imu_pub_ = nh.advertise<sensor_msgs::Imu>("imu/processed", 50);
+
         // Setup publisher to reset wheel encoders (used during first launch of the hardware interface)
         pub_reset_encoders_ = nh_.advertise<std_msgs::Empty>("reset", 10);
         // Setup subscriber for the wheel encoders
         sub_encoder_ticks_ = nh_.subscribe("encoder_ticks", 10, &DiffBotHWInterface::encoderTicksCallback, this);
         sub_measured_joint_states_ = nh_.subscribe("measured_joint_states", 10, &DiffBotHWInterface::measuredJointStatesCallback, this);
 
+        //setup subscriber for the IMU
+        sub_imu_data_ = nh_.subscribe("imu", 10, &DiffBotHWInterface::IMUDataCallback, this);
+
         // Initialize the hardware interface
         init(nh_, nh_);
 
@@ -105,9 +111,10 @@ namespace diffbot_base
 
             joint_velocity_commands_[i] = 0.0;
 
-            // Initialize encoder_ticks_ to zero because receiving meaningful
+            // Initialize encoder_ticks_ & IMU to zero because receiving meaningful
             // tick values from the microcontroller might take some time
             encoder_ticks_[i] = 0.0;
+            IMU_data_[i] = 0.0;
             measured_joint_states_[i].angular_position_ = 0.0;
             measured_joint_states_[i].angular_velocity_ = 0.0;
 
@@ -158,6 +165,9 @@ namespace diffbot_base
             ROS_INFO_STREAM(std::endl << ss.str());
             //printState();
         }
+
+        //  Publish IMU values to handle information to EKF Filter
+        PublishIMUtoFilter();
     }
 
     void DiffBotHWInterface::write(const ros::Time& time, const ros::Duration& period)
@@ -364,6 +374,55 @@ namespace diffbot_base
         ROS_DEBUG_STREAM_THROTTLE(1, "Right encoder ticks: " << encoder_ticks_[1]);
     }
 
+    /// Process updates from imu
+    void DiffBotHWInterface::IMUDataCallback(const diffbot_msgs::IMU::ConstPtr& msg_imu)
+    {
+        /// update current data from IMU
+        orientation[0] = msg_imu->imu_msg[0];
+        orientation[1] = msg_imu->imu_msg[1];
+        orientation[2] = msg_imu->imu_msg[2];
+        orientation[3] = msg_imu->imu_msg[8];
+
+        angular_velocity[0] = msg_imu->imu_msg[6];
+        angular_velocity[1] = msg_imu->imu_msg[7];
+        angular_velocity[2] = msg_imu->imu_msg[8];
+
+        linear_acceleration[0] = msg_imu->imu_msg[3];
+        linear_acceleration[1] = msg_imu->imu_msg[4];
+        linear_acceleration[2] = msg_imu->imu_msg[5];
+
+        ROS_DEBUG_STREAM_THROTTLE(1, "x orientation: " << encoder_ticks_[0]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "y orientation: " << encoder_ticks_[1]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "z orientation: " << encoder_ticks_[2]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "w orientation: " << encoder_ticks_[3]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "x angular velocity: " << encoder_ticks_[0]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "y angular velocity: " << encoder_ticks_[1]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "z angular velocity: " << encoder_ticks_[2]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "x linear accelaration: " << encoder_ticks_[0]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "y linear accelaration: " << encoder_ticks_[1]);
+        ROS_DEBUG_STREAM_THROTTLE(1, "z linear accelaration: " << encoder_ticks_[2]);
+    }
+
+
+    void DiffBotHWInterface::PublishIMUtoFilter()
+    {
+        imu_pub_.publish(imu);
+        imu.header.stamp = ros::Time::now();
+        imu.header.frame_id = "imu";
+
+        imu.orientation.x = orientation[0];
+        imu.orientation.y = orientation[1];
+        imu.orientation.z = orientation[2];
+        imu.orientation.w = orientation[3];
+
+        imu.angular_velocity.x = angular_velocity[0];
+        imu.angular_velocity.y = angular_velocity[1];
+        imu.angular_velocity.z = angular_velocity[2];
+
+        imu.linear_acceleration.x = linear_acceleration[0];
+        imu.linear_acceleration.y = linear_acceleration[1];
+        imu.linear_acceleration.z = linear_acceleration[2];
+    }
     void DiffBotHWInterface::measuredJointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg_joint_states)
     {
         /// Update current encoder ticks in encoders array