Feature/main/orange sensor tools

orange_ros2.rosinstall

@@ -42,3 +42,7 @@
     local-name: livox_laser_simulation_RO2
     uri: https://github.com/stm32f303ret6/livox_laser_simulation_RO2.git
     version: main
+- git:
+    local-name: serial
+    uri: https://github.com/RoverRobotics-forks/serial-ros2.git
+    version: master

orange_sensor_tools/CMakeLists.txt

@@ -5,19 +5,30 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+cmake_policy(SET CMP0074 NEW)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(laser_geometry REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(pcl_ros REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED)
+find_package(serial REQUIRED)
+
+# add include directories
+include_directories(
+  include
+)
 
 set(dependencies
   rclcpp
@@ -46,9 +57,29 @@ install(TARGETS laserscan_multi_merger
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
+add_executable(led_node src/led_node.cpp)
+ament_target_dependencies(led_node rclcpp std_msgs serial)
+
+install(TARGETS led_node
+  DESTINATION lib/${PROJECT_NAME})
+
+add_executable(wheel_imu_odom src/wheel_imu_odom.cpp)
+ament_target_dependencies(wheel_imu_odom
+  rclcpp
+  sensor_msgs
+  nav_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
+
+install(TARGETS wheel_imu_odom
+  DESTINATION lib/${PROJECT_NAME})
+
 # install
-install(DIRECTORY src launch config
+install(DIRECTORY config firmware include launch src
   DESTINATION share/${PROJECT_NAME}/
 )
 
-ament_package()
+ament_package()

orange_sensor_tools/firmware/LED.ino

@@ -0,0 +1,70 @@
+#include <Adafruit_NeoPixel.h>
+
+#define LED_PIN 6
+#define NUMPIXELS 30
+#define BUFFER_SIZE 10
+
+Adafruit_NeoPixel pixels(NUMPIXELS, LED_PIN, NEO_GRB + NEO_KHZ800);
+
+char receive_cmd[BUFFER_SIZE];
+uint8_t cmd_idx = 0;
+
+bool robot_state = false;
+bool led_state = false;
+unsigned long prev_millis = 0;
+const long interval = 500; // 500ms
+bool led_on = false;
+
+void setup() {
+  Serial.begin(115200);
+  pixels.begin();
+  pixels.clear();
+  pixels.show();
+}
+
+void loop() {
+  while (Serial.available()) {
+    char c = Serial.read();
+    if (c == '\n') {
+      receive_cmd[cmd_idx] = '\0';
+      processCommand(receive_cmd);
+      cmd_idx = 0;
+    } else if (cmd_idx < BUFFER_SIZE - 1) {
+      receive_cmd[cmd_idx++] = c;
+    }
+  }
+
+  unsigned long curr_millis = millis();
+  if (robot_state) { // ON
+    if (led_state) { // FLASH
+      if (curr_millis - prev_millis >= interval) {
+        prev_millis = curr_millis;
+        led_on = !led_on;
+        updateLED(led_on);
+      }
+    } else { // SOLID
+      updateLED(true);
+    }
+  } else { // OFF
+    updateLED(false);
+  }
+}
+
+void processCommand(const char* command) {
+  if (strcmp(command, "ON") == 0) {
+    robot_state = true;
+  } else if (strcmp(command, "OFF") == 0) {
+    robot_state = false;
+  } else if (strcmp(command, "FLASH") == 0) {
+    led_state = true;
+  } else if (strcmp(command, "SOLID") == 0) {
+    led_state = false;
+  }
+}
+
+void updateLED(bool state) {
+  for (int i = 0; i < NUMPIXELS; i++) {
+    pixels.setPixelColor(i, state ? pixels.Color(55, 0, 0) : pixels.Color(0, 0, 0));
+  }
+  pixels.show();
+}

orange_sensor_tools/include/led_node.hpp

@@ -0,0 +1,37 @@
+#ifndef LED_NODE_HPP
+#define LED_NODE_HPP
+
+#include <serial/serial.h>
+
+#include <chrono>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+class LedController : public rclcpp::Node
+{
+public:
+  LedController();
+  ~LedController();
+
+private:
+  void navCallback(const std_msgs::msg::Bool::SharedPtr msg);
+  void estopCallback(const std_msgs::msg::Bool::SharedPtr msg);
+  void sendSerialCommand();
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr nav_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
+  //rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr led_pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  serial::Serial serial_port_;
+
+  std::string port_name_;
+  int baud_rate_;
+  std::string nav_topic_;
+  std::string estop_topic_;
+  bool nav_state_;
+  bool estop_state_;
+  std::string led_state_;
+};
+
+#endif  // LED_NODE_HPP

orange_sensor_tools/include/wheel_imu_odom.hpp

@@ -0,0 +1,67 @@
+#ifndef WHEEL_IMU_ODOM_HPP
+#define WHEEL_IMU_ODOM_HPP
+
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+class OdomFusionNode : public rclcpp::Node
+{
+public:
+  OdomFusionNode();
+
+private:
+  void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
+  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
+  static void getYawFromQuaternion(
+    const geometry_msgs::msg::Quaternion & q, double & roll, double & pitch, double & yaw);
+
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr fused_odom_pub_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  std::string imu_topic_;
+  std::string odom_topic_;
+  std::string fused_odom_topic_;
+
+  std::string odom_header_frame_;
+  std::string odom_child_frame_;
+  std::string TF_header_frame_;
+  std::string TF_child_frame_;
+
+  double scale_factor_;
+  double pitch_diff_th_;
+  bool publish_odom_;
+  bool publish_TF_;
+  bool debug_;
+
+  rclcpp::Time prev_time_;
+  bool imu_received_;
+  bool baseline_initialized_;
+
+  double roll_;
+  double pitch_;
+  double yaw_;
+  double baseline_pitch_;
+
+  double position_x_;
+  double position_y_;
+  double position_z_;
+
+  double orientation_x_;
+  double orientation_y_;
+  double orientation_z_;
+  double orientation_w_;
+
+  double ang_vel_x_;
+  double ang_vel_y_;
+  double ang_vel_z_;
+};
+
+#endif  // WHEEL_IMU_ODOM_HPP

orange_sensor_tools/package.xml

@@ -7,20 +7,25 @@
   <maintainer email="shunki.shibuya.5v@stu.hosei.ac.jp">Shibuya</maintainer>
   <maintainer email="koki.kubota.8p@stu.hosei.ac.jp">Kubota</maintainer>
   <maintainer email="kimihiro.mori.4k@stu.hosei.ac.jp">Mori</maintainer>
+  <maintainer email="shun.saito.6t@stu.hosei.ac.jp">Saito</maintainer>
   <license>Apache-2.0</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>sensor_msgs</depend>
   <depend>libpcl-all-dev</depend>
   <depend>pcl_ros</depend>
   <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>laser_geometry</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>pointcloud_to_laserscan</depend>
   <depend>robot_localization</depend>
+  <depend>serial</depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <exec_depend>linefit_ground_segmentation_ros</exec_depend>
   <export>

orange_sensor_tools/src/led_node.cpp

@@ -0,0 +1,73 @@
+#include "led_node.hpp"
+
+using namespace std::chrono_literals;
+
+LedController::LedController() : Node("LED_node"), nav_state_(false), estop_state_(false)
+{
+  port_name_ = this->declare_parameter<std::string>("serial_port", "/dev/sensors/LED");
+  baud_rate_ = this->declare_parameter<int>("baud_rate", 115200);
+  nav_topic_ = this->declare_parameter<std::string>("nav_topic", "/nav_state");
+  estop_topic_ = this->declare_parameter<std::string>("estop_topic", "/estop");
+
+  try {
+    serial_port_.setPort(port_name_);
+    serial_port_.setBaudrate(baud_rate_);
+    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
+    serial_port_.setTimeout(to);
+    serial_port_.open();
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", e.what());
+  }
+
+  nav_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+    nav_topic_, 10, std::bind(&LedController::navCallback, this, std::placeholders::_1));
+  estop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+    estop_topic_, 10, std::bind(&LedController::estopCallback, this, std::placeholders::_1));
+  timer_ = this->create_wall_timer(500ms, std::bind(&LedController::sendSerialCommand, this));
+
+  if (serial_port_.isOpen()) {
+    serial_port_.write("ON\n");
+  }
+}
+
+LedController::~LedController()
+{
+  if (serial_port_.isOpen()) {
+    serial_port_.write("OFF\n");
+    serial_port_.close();
+  }
+}
+
+void LedController::navCallback(const std_msgs::msg::Bool::SharedPtr msg)
+{
+  nav_state_ = msg->data;
+}
+
+void LedController::estopCallback(const std_msgs::msg::Bool::SharedPtr msg)
+{
+  estop_state_ = msg->data;
+}
+
+void LedController::sendSerialCommand()
+{
+  led_state_ = (nav_state_ && !estop_state_) ? "FLASH" : "SOLID";
+
+  if (serial_port_.isOpen()) {
+    try {
+      serial_port_.write(led_state_ + "\n");
+      // RCLCPP_INFO(this->get_logger(), "Sent: %s", led_state_.c_str());
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to write to serial port: %s", e.what());
+    }
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "Serial port is NOT open!");
+  }
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<LedController>());
+  rclcpp::shutdown();
+  return 0;
+}