add imu data (#9)

* add pulisher and subscriber for imu data

* add dummy imu node

* code format

* code format

Author: gezp

ssct_common/CMakeLists.txt

@@ -41,11 +41,14 @@ add_library(${PROJECT_NAME} SHARED
   # publisher
   src/publisher/cloud_publisher.cpp
   src/publisher/image_publisher.cpp
+  src/publisher/imu_publisher.cpp
   # subscriber
   src/subscriber/image_subscriber.cpp
+  src/subscriber/imu_subscriber.cpp
   # dummy_sensor
   src/dummy_sensor/dummy_camera_node.cpp
   src/dummy_sensor/dummy_lidar_node.cpp
+  src/dummy_sensor/dummy_imu_node.cpp
   # utils
   src/calibration_params.cpp
   src/msg_utils.cpp
@@ -68,6 +71,11 @@ add_executable(dummy_camera_node
   src/dummy_sensor/dummy_camera_main.cpp)
 target_link_libraries(dummy_camera_node ${PROJECT_NAME})
 
+# dummy imu node
+add_executable(dummy_imu_node
+  src/dummy_sensor/dummy_imu_main.cpp)
+target_link_libraries(dummy_imu_node ${PROJECT_NAME})
+
 # install include directories
 install(DIRECTORY include/
   DESTINATION include
@@ -86,6 +94,7 @@ install(TARGETS ${PROJECT_NAME}
 install(TARGETS
     dummy_lidar_node
     dummy_camera_node
+    dummy_imu_node
   DESTINATION lib/${PROJECT_NAME}
 )
 

ssct_common/include/ssct_common/dummy_sensor/dummy_imu_node.hpp

@@ -0,0 +1,53 @@
+// Copyright 2024 Gezp (https://github.com/gezp).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "ssct_common/publisher/imu_publisher.hpp"
+
+namespace ssct_common
+{
+
+class DummyImuNode
+{
+public:
+  explicit DummyImuNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface()
+  {
+    return node_->get_node_base_interface();
+  }
+
+private:
+  bool read_data();
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::shared_ptr<ImuPublisher> imu_pub_;
+  // param
+  std::string frame_id_{"imu"};
+  std::string data_file_;
+  double rate_{100};
+  double timestamp_{0};
+  bool loop_{true};
+  // data
+  std::vector<ImuData> imus_;
+  size_t current_idx_{0};
+};
+}  // namespace ssct_common

ssct_common/include/ssct_common/dummy_sensor/dummy_lidar_node.hpp

@@ -1,4 +1,4 @@
-// Copyright 2024 Gezp (https://github.com/gezp).
+// Copyright 2025 Gezp (https://github.com/gezp).
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

ssct_common/include/ssct_common/publisher/imu_publisher.hpp

@@ -0,0 +1,42 @@
+// Copyright 2025 Gezp (https://github.com/gezp).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+#include "ssct_common/sensor_data/imu_data.hpp"
+
+namespace ssct_common
+{
+class ImuPublisher
+{
+public:
+  ImuPublisher(
+    rclcpp::Node::SharedPtr node, std::string topic_name, size_t buffer_size, std::string frame_id);
+
+  void publish(const ImuData & imu_data);
+  bool has_subscribers();
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
+  std::string frame_id_;
+
+  sensor_msgs::msg::Imu imu_;
+};
+}  // namespace ssct_common

ssct_common/include/ssct_common/sensor_data/imu_data.hpp

@@ -0,0 +1,28 @@
+// Copyright 2025 Gezp (https://github.com/gezp).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <Eigen/Dense>
+
+namespace ssct_common
+{
+struct ImuData
+{
+  double time = 0.0;
+  Eigen::Vector3d angular_velocity;
+  Eigen::Vector3d linear_acceleration;
+};
+
+}  // namespace ssct_common

ssct_common/include/ssct_common/subscriber/imu_subscriber.hpp

@@ -0,0 +1,44 @@
+// Copyright 2025 Gezp (https://github.com/gezp).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+#include <deque>
+#include <mutex>
+#include <string>
+
+#include "ssct_common/sensor_data/imu_data.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+namespace ssct_common
+{
+class ImuSubscriber
+{
+public:
+  ImuSubscriber(rclcpp::Node::SharedPtr node, std::string topic_name, size_t buff_size);
+  ImuSubscriber() = default;
+  void read(std::deque<ImuData> & output);
+  void clear();
+
+private:
+  void msg_callback(const sensor_msgs::msg::Imu::SharedPtr imu_msg_ptr);
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscriber_;
+
+  std::deque<ImuData> buffer_;
+  std::mutex buffer_mutex_;
+};
+}  // namespace ssct_common

ssct_common/launch/dummy_sensors.launch.py

@@ -19,11 +19,10 @@
 
 
 def generate_launch_description():
-    data_dir = os.path.join(
-        os.environ["HOME"], "calibration_data", "SensorsCalibration"
-    )
-    lidar_data_dir = os.path.join(data_dir, "lidar2camera", "lidar")
-    camera_data_dir = os.path.join(data_dir, "camera_intrinsic")
+    data_dir = os.path.join(os.environ["HOME"], "calibration_data")
+    lidar_data_dir = os.path.join(data_dir, "SensorsCalibration", "lidar2camera", "lidar")
+    camera_data_dir = os.path.join(data_dir, "SensorsCalibration", "camera_intrinsic")
+    imu_data_file = os.path.join(data_dir, "imu_data", "uncalibrated_imu_data.csv")
     dummy_lidar_node = Node(
         name="dummy_lidar_node",
         package="ssct_common",
@@ -44,7 +43,21 @@ def generate_launch_description():
         ],
         output="screen",
     )
+    dummy_imu_node = Node(
+        name="dummy_imu_node",
+        package="ssct_common",
+        executable="dummy_imu_node",
+        parameters=[
+            {
+                "data_file": imu_data_file,
+                "frame_id": "center_imu",
+                "rate": 100.0,
+            }
+        ],
+        output="screen",
+    )
     ld = LaunchDescription()
     ld.add_action(dummy_lidar_node)
     ld.add_action(dummy_camera_node)
+    ld.add_action(dummy_imu_node)
     return ld

ssct_common/src/dummy_sensor/dummy_imu_main.cpp

@@ -0,0 +1,25 @@
+// Copyright 2025 Gezp (https://github.com/gezp).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ssct_common/dummy_sensor/dummy_imu_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<ssct_common::DummyImuNode>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+  return 0;
+}

ssct_common/src/dummy_sensor/dummy_imu_node.cpp

@@ -0,0 +1,105 @@
+// Copyright 2025 Gezp (https://github.com/gezp).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ssct_common/dummy_sensor/dummy_imu_node.hpp"
+
+#include <fstream>
+#include <sstream>
+
+namespace ssct_common
+{
+
+DummyImuNode::DummyImuNode(const rclcpp::NodeOptions & options)
+{
+  node_ = std::make_shared<rclcpp::Node>("dummy_imu_node", options);
+  node_->declare_parameter("frame_id", frame_id_);
+  node_->declare_parameter("data_file", data_file_);
+  node_->declare_parameter("rate", rate_);
+  node_->declare_parameter("timestamp", timestamp_);
+  node_->declare_parameter("loop", loop_);
+
+  node_->get_parameter("frame_id", frame_id_);
+  node_->get_parameter("data_file", data_file_);
+  node_->get_parameter("rate", rate_);
+  node_->get_parameter("timestamp", timestamp_);
+  node_->get_parameter("loop", loop_);
+  // read data
+  if (data_file_.empty()) {
+    RCLCPP_FATAL(node_->get_logger(), "data_file is empty.");
+    return;
+  }
+  if (!read_data()) {
+    RCLCPP_FATAL(node_->get_logger(), "no imus.");
+    return;
+  }
+  // publisher
+  std::string topic_name = "/sensor/" + frame_id_ + "/imu";
+  imu_pub_ = std::make_shared<ImuPublisher>(node_, topic_name, 100, frame_id_);
+  // create timer
+  auto period_ms = std::chrono::milliseconds(static_cast<int64_t>(1000.0 / rate_));
+  auto timer_callback = [this]() {
+      if (loop_ && current_idx_ == imus_.size()) {
+        current_idx_ = 0;
+      }
+      if (current_idx_ < imus_.size()) {
+        imu_pub_->publish(imus_[current_idx_]);
+        current_idx_++;
+      }
+    };
+  timer_ = node_->create_wall_timer(period_ms, timer_callback);
+}
+
+bool DummyImuNode::read_data()
+{
+  std::ifstream ifs(data_file_);
+  if (!ifs.is_open()) {
+    RCLCPP_FATAL(node_->get_logger(), "failed to open file %s.", data_file_.c_str());
+    return false;
+  }
+  std::string line;
+  while (std::getline(ifs, line)) {
+    if (line.empty() || !std::isdigit(line[0])) {
+      // skip head line and empty line
+      continue;
+    }
+    std::istringstream ss(line);
+    std::string token;
+    ImuData imu;
+    try {
+      std::getline(ss, token, ',');
+      imu.time = timestamp_ + std::stod(token);
+      std::getline(ss, token, ',');
+      imu.angular_velocity.x() = std::stod(token);
+      std::getline(ss, token, ',');
+      imu.angular_velocity.y() = std::stod(token);
+      std::getline(ss, token, ',');
+      imu.angular_velocity.z() = std::stod(token);
+      std::getline(ss, token, ',');
+      imu.linear_acceleration.x() = std::stod(token);
+      std::getline(ss, token, ',');
+      imu.linear_acceleration.y() = std::stod(token);
+      std::getline(ss, token, ',');
+      imu.linear_acceleration.z() = std::stod(token);
+      imus_.push_back(imu);
+    } catch (const std::exception & e) {
+      RCLCPP_FATAL(
+        node_->get_logger(), "failed parse line: [%s], error: %s", line.c_str(), e.what());
+      return false;
+    }
+  }
+  ifs.close();
+  return !imus_.empty();
+}
+
+}  // namespace ssct_common