Merge pull request #1 from kazu-321/humble

feat: support high speed communication and add temperature

Author: Ar-Ray-code

README.md

@@ -1,4 +1,27 @@
 # cxd5602pwbimu_ros2_driver
 ROS 2 driver for Multi-IMU Add-on board for SONY SPRESENSE™
+High speed version up to 1.92 kHz
 
-[![ci_humble](https://github.com/NITKK-ROS-Team/cxd5602pwbimu_ros2_driver/actions/workflows/ci_humble.yml/badge.svg)](https://github.com/NITKK-ROS-Team/cxd5602pwbimu_ros2_driver/actions/workflows/ci_humble.yml)
+[![ci_humble](https://github.com/NITKK-ROS-Team/cxd5602pwbimu_ros2_driver/actions/workflows/ci_humble.yml/badge.svg)](https://github.com/NITKK-ROS-Team/cxd5602pwbimu_ros2_driver/actions/workflows/ci_humble.yml)
+
+## Flash
+### 1. Setup Arduino IDE for SPRESENSE
+Follow the instructions on the official SPRESENSE Arduino IDE setup guide:
+[https://developer.sony.com/spresense/development-guides/arduino_set_up_ja](https://developer.sony.com/spresense/development-guides/arduino_set_up_ja)
+
+### 2. Flash INO Sketch
+`firmware/cxd5602pwbimu/cxd5602pwbimu.ino` to SPRESENSE main board using Arduino IDE.
+
+## Build
+```bash
+git clone <this repo url>
+cd cxd5602pwbimu_ros2_driver
+git clone -b humble https://github.com/HarvestX/h6x_serial_interface.git
+colcon build --symlink-install
+```
+
+## Launch
+```bash
+source install/setup.bash
+ros2 launch cxd5602pwbimu_driver_bringup cxd5602pwbimu.launch.py
+```

cxd5602pwbimu_driver_bringup/launch/cxd5602pwbimu.launch.py

@@ -9,15 +9,15 @@ def generate_launch_description():
         executable='cxd5602pwbimu_driver_node_exec',
         name='cxd5602pwbimu_driver_node',
         output='screen',
-        parameters=[{'device': '/dev/ttyUSB0'}]
+        parameters=[{'device': '/dev/ttyUSB0', 'baudrate': 1000000}]
     )
 
     imu_filter_node = Node(
         package='imu_filter_madgwick',
         executable='imu_filter_madgwick_node',
         name='imu_filter_madgwick_node',
         output='screen',
-        parameters=[{'use_mag': False}]
+        parameters=[{'use_mag': False, 'remove_gravity_vector': True}]
     )
 
     rviz_node = Node(

cxd5602pwbimu_driver_node/include/cxd5602pwbimu_driver_node/cxd5602pwbimu_driver_node.hpp

@@ -8,9 +8,12 @@
 #define ____CXD5602PWBIMU_DRIVER_NODE_CXD5602PWBIMU_DRIVER_NODE_HPP__
 
 #include <functional>
+#include <thread>
+#include <atomic>
 #include <h6x_serial_interface/h6x_serial_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/temperature.hpp>
 
 #include "imu_class.hpp"
 
@@ -23,23 +26,26 @@ class Cxd5602pwbimuDriverNode : public rclcpp::Node
   using PortHandler = h6x_serial_interface::PortHandler;
   PortHandler port_handler_;
   uint32_t time_offset_;
-  const char delimiter_;
-
-  rclcpp::TimerBase::SharedPtr read_timer_;
+  const char delimiter_, start_byte_;
 
   std::unique_ptr<ImuClass> imu_;
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
-  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<sensor_msgs::msg::Temperature>::SharedPtr temp_publisher_;
+
+  std::thread recv_thread_;
+  std::atomic<bool> running_;
 
 public:
   Cxd5602pwbimuDriverNode() = delete;
   explicit Cxd5602pwbimuDriverNode(const rclcpp::NodeOptions &);
   ~Cxd5602pwbimuDriverNode();
 
 private:
-  void timerCallback();
-
+  void startSerialThread();
 
+  void processPacket(const uint8_t* data, size_t size);
 };
+
 }   // namespace cxd5602pwbimu_driver_node
+
 #endif  // ____CXD5602PWBIMU_DRIVER_NODE_CXD5602PWBIMU_DRIVER_NODE_HPP__

cxd5602pwbimu_driver_node/include/cxd5602pwbimu_driver_node/imu_class.hpp

@@ -26,11 +26,13 @@ class ImuClass
 
   bool set_data(const uint8_t *, size_t);
   void print_data() const;
-  std::tuple<std::array<float, 3>, std::array<float, 3>, uint32_t, uint32_t> get_data() const;
+  std::tuple<std::array<float, 3>, std::array<float, 3>, float, uint32_t, uint32_t> get_data() const;
 
 private:
   std::array<float, 3> linear_acceleration_;
   std::array<float, 3> angular_velocity_;
+  const char delimiter_, start_byte_;
+  float temperature_;
   uint32_t sec_;
   uint32_t msec_;
 };

cxd5602pwbimu_driver_node/src/cxd5602pwbimu_driver_node.cpp

@@ -13,69 +13,117 @@ Cxd5602pwbimuDriverNode::Cxd5602pwbimuDriverNode(const rclcpp::NodeOptions & opt
 : rclcpp::Node("imu_publisher", options),
   port_handler_(this->declare_parameter<std::string>("dev", "/dev/ttyUSB0")),
   time_offset_(0),
-  delimiter_(this->declare_parameter<char>("delimiter", '\n'))
+  delimiter_(this->declare_parameter<char>("delimiter", '\n')),
+  start_byte_(this->declare_parameter<char>("start_byte", 'X')),
+  running_(true)
 {
   const int baudrate = this->declare_parameter<int>("baudrate", 115200);
   const int timeout_ms = this->declare_parameter<int>("timeout_ms", 100);
-  const int spin_ms = this->declare_parameter<int>("spin_ms", 1);
 
   imu_ = std::make_unique<ImuClass>();
-  publisher_ = this->create_publisher<sensor_msgs::msg::Imu>(
-    "/imu/data_raw", rclcpp::SensorDataQoS());
+  publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu/data_raw", rclcpp::SensorDataQoS());
+  temp_publisher_ = this->create_publisher<sensor_msgs::msg::Temperature>("/imu/temperature", rclcpp::SensorDataQoS());
 
-  if (!this->port_handler_.configure(baudrate, timeout_ms)) {
-    RCLCPP_ERROR(this->get_logger(), "Failed to configure serial port");
+  if (!port_handler_.configure(baudrate, timeout_ms)) {
+    RCLCPP_ERROR(get_logger(), "Failed to configure serial port");
     exit(EXIT_FAILURE);
   }
 
-  if (!this->port_handler_.open()) {
-    RCLCPP_ERROR(this->get_logger(), "Failed to open serial port");
+  if (!port_handler_.open()) {
+    RCLCPP_ERROR(get_logger(), "Failed to open serial port");
     exit(EXIT_FAILURE);
   }
 
-  this->timer_ = this->create_wall_timer(
-    std::chrono::milliseconds(spin_ms), std::bind(&Cxd5602pwbimuDriverNode::timerCallback, this));
+  RCLCPP_INFO(get_logger(), "Serial port opened successfully");
+  startSerialThread();
 }
 
 Cxd5602pwbimuDriverNode::~Cxd5602pwbimuDriverNode()
 {
-  this->port_handler_.close();
+  running_ = false;
+  if (recv_thread_.joinable()) {
+    recv_thread_.join();
+  }
+  port_handler_.close();
 }
 
-void Cxd5602pwbimuDriverNode::timerCallback()
+void Cxd5602pwbimuDriverNode::startSerialThread()
 {
-  this->timer_->cancel();
+  recv_thread_ = std::thread([this]() {
+    std::string buf;
+    buf.reserve(39);
+    char c;
+
+    while (running_) {
+        int n = port_handler_.read(&c, 1);
+        if (n != 1) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            continue;
+        }
+
+        buf.push_back(c);
+
+        if (buf.front() != start_byte_) {
+            buf.erase(buf.begin());
+            continue;
+        }
+
+        if (buf.size() == 39) {
+            if (buf.back() == delimiter_) {
+                processPacket(reinterpret_cast<const uint8_t*>(buf.data()), buf.size());
+            } else {
+                char skip;
+                do {
+                    if (port_handler_.read(&skip, 1) != 1) break;
+                    RCLCPP_DEBUG(get_logger(), "Skipping byte: %c %d", skip, skip==delimiter_);
+                } while (skip != delimiter_);
+                RCLCPP_WARN_THROTTLE(get_logger(), *this->get_clock(), 5000, "Resynchronized at delimiter");
+            }
+            buf.clear();
+        }
+    }
 
-  std::stringstream ss;
-  this->port_handler_.readUntil(ss, delimiter_);
+  });
+}
 
-  std::string buffer = ss.str();
+void Cxd5602pwbimuDriverNode::processPacket(const uint8_t* data, size_t size)
+{
+  if (!imu_->set_data(data, size)) {
+    RCLCPP_WARN(get_logger(), "Failed to parse IMU data");
+    return;
+  }
 
-  if (imu_->set_data(reinterpret_cast<const uint8_t *>(buffer.c_str()), buffer.size())) {
-    auto [linear_acceleration, angular_velocity, sec, msec] = imu_->get_data();
+  auto [linear_acc, angular_vel, temperature, sec, msec] = imu_->get_data();
 
-    auto msg = std::make_unique<sensor_msgs::msg::Imu>();
+  auto msg = sensor_msgs::msg::Imu();
 
-    if (time_offset_ == 0) {
-      time_offset_ = static_cast<uint32_t>(std::time(nullptr));
-    }
+  if (time_offset_ == 0) {
+    time_offset_ = static_cast<uint32_t>(std::time(nullptr));
+  }
 
-    msg->header.frame_id = "imu";
-    msg->header.stamp.sec = sec + time_offset_;
-    msg->header.stamp.nanosec = msec * 1000000;
+  msg.header.frame_id = "imu";
+  msg.header.stamp.sec = sec + time_offset_;
+  msg.header.stamp.nanosec = msec * 1000000;
 
-    msg->linear_acceleration.x = linear_acceleration[0];
-    msg->linear_acceleration.y = linear_acceleration[1];
-    msg->linear_acceleration.z = linear_acceleration[2];
+  msg.linear_acceleration.x = -linear_acc[0];
+  msg.linear_acceleration.y = -linear_acc[1];
+  msg.linear_acceleration.z = linear_acc[2];
 
-    msg->angular_velocity.x = angular_velocity[0] * 0.5;
-    msg->angular_velocity.y = angular_velocity[1] * 0.5;
-    msg->angular_velocity.z = angular_velocity[2] * 0.5;
+  msg.angular_velocity.x = -angular_vel[0] * 0.5;
+  msg.angular_velocity.y = -angular_vel[1] * 0.5;
+  msg.angular_velocity.z = angular_vel[2] * 0.5;
 
-    publisher_->publish(std::move(msg));
-  }
+  msg.orientation_covariance[0] = -1;
+  msg.linear_acceleration_covariance[0] = -1;
+  msg.angular_velocity_covariance[0] = -1;
+
+  publisher_->publish(msg);
 
-  this->timer_->reset();
+  auto temp_msg = sensor_msgs::msg::Temperature();
+  temp_msg.header = msg.header;
+  temp_msg.temperature = temperature;
+  temp_msg.variance = -1;
+  temp_publisher_->publish(temp_msg);
 }
 
 }

cxd5602pwbimu_driver_node/src/imu_class.cpp

@@ -12,22 +12,35 @@ namespace cxd5602pwbimu_driver_node
 ImuClass::ImuClass()
 : linear_acceleration_({0.0, 0.0, 0.0}),
   angular_velocity_({0.0, 0.0, 0.0}),
+  temperature_(0.0),
   sec_(0),
-  msec_(0) {}
+  msec_(0),
+  delimiter_('\n'),
+  start_byte_('X') {}
 
 bool ImuClass::set_data(const uint8_t * data_bytes, size_t length)
 {
-  if (length != 36) {
+  const size_t expected_length = 39;
+  const size_t crc_index = expected_length - 2;
+  if (length != expected_length) {
+    std::cerr << "Invalid data length: " << length << std::endl;
     return false;
   }
-  if (data_bytes[0] != 'X') {
+  if (data_bytes[0] != start_byte_) {
+    std::cerr << "Invalid start byte: " << static_cast<int>(data_bytes[0]) << std::endl;
+    return false;
+  }
+  if (data_bytes[expected_length - 1] != delimiter_) {
+    std::cerr << "Invalid end byte: " << static_cast<int>(data_bytes[expected_length - 1]) << std::endl;
     return false;
   }
 
   CRC8 hash_obj(0x07);
-  hash_obj.add(data_bytes, 33);
+  hash_obj.add(data_bytes, crc_index);
 
-  if (hash_obj.calc() != data_bytes[33]) {
+  if (hash_obj.calc() != data_bytes[crc_index]) {
+    std::cerr << "CRC mismatch: calculated " << static_cast<int>(hash_obj.calc())
+              << ", received " << static_cast<int>(data_bytes[crc_index]) << std::endl;
     return false;
   }
 
@@ -49,6 +62,10 @@ bool ImuClass::set_data(const uint8_t * data_bytes, size_t length)
     offset += 4;
   }
 
+  float temp;
+  std::memcpy(&temp, &data_bytes[offset], sizeof(float));
+  temperature_ = temp;
+
   return true;
 }
 
@@ -63,12 +80,13 @@ void ImuClass::print_data() const
             << angular_velocity_[0] << ", "
             << angular_velocity_[1] << ", "
             << angular_velocity_[2] << "]" << std::endl;
+  std::cout << "Temperature: " << temperature_ << " C" << std::endl;
 }
 
-std::tuple<std::array<float, 3>, std::array<float, 3>, uint32_t,
+std::tuple<std::array<float, 3>, std::array<float, 3>, float, uint32_t,
   uint32_t> ImuClass::get_data() const
 {
-  return {linear_acceleration_, angular_velocity_, sec_, msec_};
+  return {linear_acceleration_, angular_velocity_, temperature_, sec_, msec_};
 }
 
 }  // namespace cxd5602pwbimu_driver_node

firmware/cxd5602pwbimu.ino firmware/cxd5602pwbimu/cxd5602pwbimu.inofirmware/cxd5602pwbimu.ino renamed to firmware/cxd5602pwbimu/cxd5602pwbimu.ino

@@ -65,36 +65,42 @@ static int drop_50msdata(int fd, int samprate)
 }
 
 void setup() {
+  Serial.begin(1000000);
+
   int devfd;
   board_cxd5602pwbimu_initialize(5);
 
   devfd = open(CXD5602PWBIMU_DRIVER_DEVPATH, O_RDONLY);
-  start_sensing(devfd, 960, 16, 4000, MAX_NFIFO);
-  drop_50msdata(devfd, 960);
+  start_sensing(devfd, 1920, 16, 4000, MAX_NFIFO);
+  drop_50msdata(devfd, 1920);
   delay(2000);
 
   int32_t sec = 0;
   int32_t msec = 0;
 
   float linear_acceleration[3] = {0};
   float angular_velocity[3] = {0};
+  float temperature = 0;
   while(1) {
     int ret = read(devfd, g_data, sizeof(g_data[0]) * MAX_NFIFO);
     if(ret == sizeof(g_data[0]) * MAX_NFIFO) {
       for(int i=0; i<MAX_NFIFO; i++) {
-        linear_acceleration[0] = -g_data[i].ax;
-        linear_acceleration[1] = -g_data[i].ay;
-        linear_acceleration[2] = -g_data[i].az;
-        angular_velocity[0] = -g_data[i].gx; 
-        angular_velocity[1] = -g_data[i].gy; 
+        linear_acceleration[0] = g_data[i].ax;
+        linear_acceleration[1] = g_data[i].ay;
+        linear_acceleration[2] = g_data[i].az;
+        angular_velocity[0] = g_data[i].gx; 
+        angular_velocity[1] = g_data[i].gy; 
         angular_velocity[2] = g_data[i].gz;
+        temperature = g_data[i].temp;
 
         float2byte f2b_linear_acceleration[3];
         float2byte f2b_angular_velocity[3];
+        float2byte f2b_temperature;
         for(int j=0; j<3; j++) {
           f2b_linear_acceleration[j].f = linear_acceleration[j];
           f2b_angular_velocity[j].f = angular_velocity[j];
         }
+        f2b_temperature.f = temperature;
 
         get_time(sec, msec);
 
@@ -104,6 +110,7 @@ void setup() {
         crc.add((uint8_t*)&msec, 4);
         crc.add((uint8_t*)&linear_acceleration, 12);
         crc.add((uint8_t*)&angular_velocity, 12);
+        crc.add((uint8_t*)&temperature, 4);
         uint8_t crc8 = crc.calc();
 
         printf("%c", SERIAL_HEADER);
@@ -123,6 +130,9 @@ void setup() {
             printf("%c", f2b.b[j]);
           }
         }
+        for(int i=0;i<4;i++) {
+          printf("%c", f2b_temperature.b[i]);
+        }
         printf("%c\n", crc8);
       }
     }