Race Condition fix

colcon.meta

@@ -3,7 +3,7 @@
         "rmw_microxrcedds": {
             "cmake-args": [
                 "-DRMW_UXRCE_MAX_NODES=1",
-                "-DRMW_UXRCE_MAX_PUBLISHERS=7",
+                "-DRMW_UXRCE_MAX_PUBLISHERS=8",
                 "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=9",
                 "-DRMW_UXRCE_MAX_SERVICES=10",
                 "-DRMW_UXRCE_MAX_CLIENTS=0",

include/configuration.hpp

@@ -18,7 +18,7 @@ constexpr const char* ROS_NODE_NAME = "firmware";
 constexpr const char* ROS_NAMESPACE = "";
 
 // Number of encoder readings to remember when estimating the wheel velocity
-constexpr uint32_t ENCODER_BUFFER_SIZE = 10;
+constexpr uint32_t VELOCITY_ROLLING_WINDOW_SIZE = 10;
 
 // The period (in number of calls to the update() function) at which the battery
 // voltage is probed
@@ -38,7 +38,7 @@ constexpr uint16_t UPDATE_PERIOD = 10;
 constexpr uint8_t BATTERY_PUB_PERIOD = 10;
 constexpr uint8_t JOINTS_PUB_PERIOD = 5;
 constexpr uint8_t ODOM_PUB_PERIOD = 5;
-// constexpr uint8_t IMU_PUB_PERIOD = 1;
+constexpr uint8_t IMU_PUB_PERIOD = 1;
 constexpr uint8_t PARAM_TRIGGER_PUB_PERIOD = 100;
 
 // The time after which the firmware will boot with default parameter values
@@ -78,25 +78,21 @@ constexpr diff_drive_lib::RobotConfiguration ROBOT_CONFIG = {
     .wheel_FL_conf =
         {
             .motor = MotC,
-            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
-            .velocity_rolling_window_size = ENCODER_BUFFER_SIZE,
+            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY
         },
     .wheel_RL_conf =
         {
             .motor = MotD,
-            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
-            .velocity_rolling_window_size = ENCODER_BUFFER_SIZE,
+            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY
         },
     .wheel_FR_conf =
         {
             .motor = MotA,
-            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
-            .velocity_rolling_window_size = ENCODER_BUFFER_SIZE,
+            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY
         },
     .wheel_RR_conf =
         {
             .motor = MotB,
-            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
-            .velocity_rolling_window_size = ENCODER_BUFFER_SIZE,
+            .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY
         },
 };

include/hal_compat.hpp

@@ -24,3 +24,7 @@ inline void gpio_toggle(mbed::DigitalOut& gpio) {
 inline void reset() {
   NVIC_SystemReset();
 }
+
+inline void delay(uint32_t delay_ms) {
+  ThisThread::sleep_for(delay_ms);
+}

include/imu_receiver.hpp

@@ -0,0 +1,40 @@
+#pragma once
+
+#include <mbed.h>
+
+#include <sensors/MPU9250/MPU9250.h>
+
+class ImuReceiver {
+ public:
+  explicit ImuReceiver(mbed::I2C &i2c) : mpu_(i2c) {
+    mpu_.begin(100000);
+  }
+
+  void start() {
+    thread_running_ = true;
+    thread_.start(mbed::callback(this, &ImuReceiver::loop));
+  }
+
+  void stop() {
+    thread_running_ = false;
+    thread_.join();
+  }
+
+  bool is_initialized() {
+    return initialized_;
+  }
+
+  float temp;        // temperature
+  float ax, ay, az;  // accelerometer data
+  float gx, gy, gz;  // gyroscope data
+
+ private:
+  MPU9250 mpu_;
+  float ares_, gres_;
+  rtos::Thread thread_;
+  bool initialized_, thread_running_;
+  void loop();
+  bool init();
+  void update();
+  bool check_status();
+};

platformio.ini

@@ -15,7 +15,7 @@ board_microros_distro = humble-fictionlab
 board_microros_transport = custom
 board_microros_user_meta = colcon.meta
 lib_deps =
-    https://github.com/fictionlab/diff_drive_lib.git#1.6
+    https://github.com/fictionlab/diff_drive_lib.git#2.0
     https://github.com/fictionlab/micro_ros_platformio#fictionlab-v2
     https://github.com/byq77/encoder-mbed.git#dfac32c
     https://github.com/byq77/drv88xx-driver-mbed.git#61854e8

src/imu_receiver.cpp

@@ -0,0 +1,64 @@
+#include "imu_receiver.hpp"
+#include <sensors/MPU9250/RegisterMap.h>
+
+static constexpr float PI = 3.141592653F;
+static constexpr float GRAVITATIONAL_ACCELERATION = 9.80665F;
+static constexpr float DEGREE_TO_RADIAN = 2.0F * PI / 360.0F;
+static constexpr float TEMP_RESOLUTION = 1.0F / 333.87F;
+static constexpr float TEMP_OFFSET = 21.0F;
+static constexpr uint16_t CHECK_PERIOD = 500;
+static uint16_t cnt = 0;
+
+bool ImuReceiver::init() {
+  if (!check_status()) return false;
+
+  mpu_.initMPU9250(AFS_2G, GFS_250DPS, 0x02);
+
+  ares_ = mpu_.getAres(AFS_2G);
+  gres_ = mpu_.getGres(GFS_250DPS);
+
+  temp = 0.0;
+  ax = ay = az = 0.0;
+  gx = gy = gz = 0.0;
+
+  cnt = 0;
+
+  return true;
+}
+
+void ImuReceiver::update() {
+  int16_t MPU9250Data[7];
+  mpu_.readMPU9250Data(MPU9250Data);
+
+  temp = (static_cast<float>(MPU9250Data[3]) - TEMP_OFFSET) * TEMP_RESOLUTION +
+         TEMP_OFFSET;
+  ax = static_cast<float>(MPU9250Data[0]) * ares_ * GRAVITATIONAL_ACCELERATION;
+  ay = static_cast<float>(MPU9250Data[1]) * ares_ * GRAVITATIONAL_ACCELERATION;
+  az = static_cast<float>(MPU9250Data[2]) * ares_ * GRAVITATIONAL_ACCELERATION;
+  gx = static_cast<float>(MPU9250Data[4]) * gres_ * DEGREE_TO_RADIAN;
+  gy = static_cast<float>(MPU9250Data[5]) * gres_ * DEGREE_TO_RADIAN;
+  gz = static_cast<float>(MPU9250Data[6]) * gres_ * DEGREE_TO_RADIAN;
+}
+
+bool ImuReceiver::check_status() {
+  uint8_t c = mpu_.getMPU9250ID();
+
+  return c == 0x71;
+}
+
+void ImuReceiver::loop() {
+  while (thread_running_) {
+    if (is_initialized()) {
+      update();
+    } else {
+      initialized_ = init();
+    }
+
+    cnt++;
+    if(!cnt % CHECK_PERIOD) {
+      initialized_ = check_status();
+    }
+
+    ThisThread::sleep_for(10);
+  }
+}