Merge branch 'main' into main

Author: Alex-Beh

CMakeLists.txt

@@ -11,5 +11,6 @@ if (NOT CMAKE_BUILD_TYPE)
 endif ()
 
 add_subdirectory(infinite_sense_core)
+
 add_subdirectory(tools)
 add_subdirectory(example)

example/CMakeLists.txt

@@ -2,4 +2,5 @@ cmake_minimum_required(VERSION 3.10)
 project(example)
 
 add_subdirectory(NetCam)
-add_subdirectory(CustomCam)
+add_subdirectory(CustomCam)
+add_subdirectory(ros2_example)

example/gprmc_streaming/data.log

@@ -0,0 +1,47 @@
+sprintf(gmrc_buffer, 
+    "GPRMC,
+    %02d%02d%02d,               <---- hhmmss
+    A,
+    %s,                         <---- 25.04776
+    N,
+    %s,                         <---- 121.53185
+    E,
+    022.4,
+    084.4,
+    %02d%02d%02d,               <---- ddmmyy
+    ,
+    A",                        <---- A XX
+
+    hh, mm, ss, "25.04776", "121.53185", day, month, year);
+
+$GPRMC,hhmmss,A,25.04776,N,121.53185,E,022.4,084.4,ddmmyy,,A XX
+
+void SendHex(uart_inst_t uart, uint8_t crc) {
+    char high = (crc >> 4) & 0x0F;
+    char low = crc & 0x0F;
+    high = (high < 10) ? ('0' + high) : ('A' + high - 10);
+    low = (low < 10) ? ('0' + low) : ('A' + low - 10);
+    uart_putc(uart, high);
+    uart_putc(uart, low);
+    uart_puts(uart, "\n");
+}
+
+主函数：
+sprintf(gmrc_buffer, "GPRMC,%02d%02d%02d,A,%s,N,%s,E,022.4,084.4,%02d%02d%02d,,A", hh, mm, ss, "25.04776", "121.53185", day, month, year);
+uart_puts(uart, "$");
+uart_puts(uart, gmrc_buffer);
+uart_puts(uart, "");
+byte crc = 0;
+for (byte x = 0; x < strlen(gmrc_buffer); x++) {
+    crc = crc ^ gmrc_buffer[x];
+}
+SendHex(uart, crc);
+
+65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 32 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 30 0a
+65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 33 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 31 0a
+65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 34 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 36 0a
+65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 35 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 37 0a
+65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 36 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 34 0a
+65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 37 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 35 0a
+65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 38 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 41 0a
+65 bytes: 24 47 50 52 4d 43 2c 30 30 30 30 31 39 2c 41 2c 32 35 2e 30 34 37 37 36 2c 4e 2c 31 32 31 2e 35 33 31 38 35 2c 45 2c 30 32 32 2e 34 2c 30 38 34 2e 34 2c 30 31 30 31 2d 33 30 2c 2c 41 2a 37 42 0a

example/gprmc_streaming/gprmc_streaming.py

@@ -0,0 +1,48 @@
+import serial
+from time import sleep
+
+ser = serial.Serial('/dev/ttyUSB0',      # or /dev/ttyUSB0
+                    # baudrate=9600,       # <-- set to the same value you used
+                    baudrate=115200,       # <-- set to the same value you used
+                    bytesize=serial.EIGHTBITS,
+                    parity=serial.PARITY_NONE,
+                    stopbits=serial.STOPBITS_ONE,
+                    timeout=1)
+
+def read_vendor_nmea(ser):
+    raw = ser.readline()                      # blocks until LF
+    # if raw:
+    #     print("RAW :", raw.hex(" "))
+
+    if not raw or raw[:1] != b'$' or len(raw) < 6:
+        return None, False
+
+    raw = raw.rstrip(b'\r\n')                 # drop LF/CR
+
+    star = raw.rfind(b'*')                    # <─ find the asterisk
+    if star == -1 or len(raw) - star < 3:     # need at least '*XX'
+        return None, False
+
+    payload = raw[1:star]                     # bytes between $ and *
+    cs_hex  = raw[star+1:star+3]              # the two ASCII hex digits
+
+    try:
+        cs_rx = int(cs_hex, 16)
+    except ValueError:
+        return None, False
+
+    cs_calc = 0
+    for b in payload:
+        cs_calc ^= b
+
+    nmea = b'$' + payload + b'*' + cs_hex + b'\r\n'
+    return nmea.decode('ascii', errors='replace'), cs_calc == cs_rx
+
+
+while True:
+    nmea, ok = read_vendor_nmea(ser)
+    if nmea:
+        print("✓" if ok else "✗", nmea)
+    else:
+        print("waiting …", end='\r')
+        sleep(0.1)

example/gprmc_streaming/loopback_test.py

@@ -0,0 +1,25 @@
+# Loopback Test
+
+import serial
+import time
+
+ser = serial.Serial('/dev/ttyUSB0',      # or /dev/ttyUSB0
+                    # baudrate=9600,       # <-- set to the same value you used
+                    baudrate=115200,       # <-- set to the same value you used
+                    bytesize=serial.EIGHTBITS,
+                    parity=serial.PARITY_NONE,
+                    stopbits=serial.STOPBITS_ONE,
+                    timeout=1)
+print(f"Listening on {ser.port} @ {ser.baudrate} baud – Ctrl‑C to quit")
+
+# --- Read forever -----------------------------------------------------
+try:
+    while True:
+        ser.write(b'>')        # send the character '>'
+        time.sleep(1.00)
+        print("echo:", ser.read(1))
+except KeyboardInterrupt:
+    pass
+finally:
+    ser.close()
+    print("\nSerial port closed – bye.")

example/ros2_example/CMakeLists.txt

@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.16)
+
+project(ros2_example VERSION 1.0)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# === InfiniteSense SDK ===
+set(INFINITE_SENSE_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/infinite_sense_core/include")
+set(INFINITE_SENSE_LIB_NAME infinite_sense_core)
+
+# === Component (shared library) ===
+add_library(synchronizer_component SHARED
+  synchronizer_component.cpp
+)
+
+target_include_directories(synchronizer_component 
+  PUBLIC
+    $<BUILD_INTERFACE:${INFINITE_SENSE_INCLUDE_DIR}>
+    $<INSTALL_INTERFACE:include>
+)
+
+target_link_libraries(synchronizer_component
+  ${INFINITE_SENSE_LIB_NAME}
+)
+
+ament_target_dependencies(synchronizer_component
+  rclcpp
+  rclcpp_components
+  std_msgs
+)
+
+rclcpp_components_register_nodes(synchronizer_component SynchronizerComponent)
+
+## === ROS2 GPRMC Publisher Node ===
+add_executable(gprmc_publisher_node gprmc_publisher_node.cpp)
+ament_target_dependencies(gprmc_publisher_node rclcpp std_msgs sensor_msgs)
+
+# === Standalone ROS 2 Node (executable) ===
+add_executable(synchronizer_node
+  synchronizer_node.cpp
+)
+target_include_directories(synchronizer_node 
+  PUBLIC
+    ${INFINITE_SENSE_INCLUDE_DIR}
+)
+target_link_libraries(synchronizer_node
+  ${INFINITE_SENSE_LIB_NAME}
+)
+ament_target_dependencies(synchronizer_node
+  rclcpp
+  std_msgs
+)
+
+# === Installation ===
+install(TARGETS
+    synchronizer_component
+    synchronizer_node
+    gprmc_publisher_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+# === Export ===
+ament_export_targets(synchronizer_component HAS_LIBRARY_TARGET)
+ament_export_dependencies(rclcpp rclcpp_components std_msgs)

example/ros2_example/gprmc_publisher_node.cpp

@@ -0,0 +1,186 @@
+#include <fstream>
+#include <string>
+#include <sstream>
+#include <iomanip>
+#include <termios.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <sensor_msgs/msg/time_reference.hpp>
+
+class GprmcPublisher : public rclcpp::Node {
+ public:
+  GprmcPublisher() : Node("gprmc_publisher") {
+    this->declare_parameter<std::string>("port", "/dev/ttyUSB0");
+    this->declare_parameter<int>("baud", 115200);
+
+    std::string port;
+    int baud;
+    this->get_parameter("port", port);
+    this->get_parameter("baud", baud);
+
+    publisher_ = this->create_publisher<std_msgs::msg::String>("/gps/gprmc", 10);
+    time_pub_ = this->create_publisher<sensor_msgs::msg::TimeReference>("/gps/time_reference", 10);
+
+    fd_ = open(port.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
+    if (fd_ < 0) {
+      RCLCPP_FATAL(this->get_logger(), "Failed to open port: %s", port.c_str());
+      rclcpp::shutdown();
+      return;
+    }
+
+    configure_serial(fd_, baud);
+    timer_ =
+        this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&GprmcPublisher::read_and_publish, this));
+
+    RCLCPP_INFO(this->get_logger(), "Opened %s @ %d baud", port.c_str(), baud);
+  }
+
+  ~GprmcPublisher() {
+    if (fd_ >= 0) close(fd_);
+  }
+
+ private:
+  void configure_serial(int fd, int baudrate) {
+    struct termios tty;
+    tcgetattr(fd, &tty);
+    cfsetospeed(&tty, baudrate_to_constant(baudrate));
+    cfsetispeed(&tty, baudrate_to_constant(baudrate));
+    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;
+    tty.c_iflag = IGNBRK;
+    tty.c_lflag = 0;
+    tty.c_oflag = 0;
+    tty.c_cc[VMIN] = 0;
+    tty.c_cc[VTIME] = 10;
+    tty.c_cflag |= (CLOCAL | CREAD);
+    tty.c_cflag &= ~(PARENB | PARODD);
+    tty.c_cflag &= ~CSTOPB;
+    tty.c_cflag &= ~CRTSCTS;
+    tcsetattr(fd, TCSANOW, &tty);
+  }
+
+  speed_t baudrate_to_constant(int baud) {
+    switch (baud) {
+      case 9600:
+        return B9600;
+      case 19200:
+        return B19200;
+      case 38400:
+        return B38400;
+      case 57600:
+        return B57600;
+      case 115200:
+        return B115200;
+      default:
+        RCLCPP_FATAL(this->get_logger(), "Unsupported baudrate: %d", baud);
+        rclcpp::shutdown();
+        return B9600;
+    }
+  }
+
+  void read_and_publish() {
+    char buf[256];
+    ssize_t n = read(fd_, buf, sizeof(buf));
+    if (n <= 0) return;
+
+    for (ssize_t i = 0; i < n; ++i) {
+      if (buf[i] == '$') {
+        line_.clear();
+      }
+      line_ += buf[i];
+      if (buf[i] == '\n') {
+        if (is_valid_checksum(line_)) {
+          // ---> Publish GPRMC as String
+          std_msgs::msg::String msg;
+          msg.data = line_;
+          std::cout << "Data: " << line_ << std::endl;
+          publisher_->publish(msg);
+
+          // ---> Publish GPRMC as TimeReference
+          auto maybe_time = parse_gprmc_utc(line_);
+          if (maybe_time.has_value()) {
+            sensor_msgs::msg::TimeReference tref;
+            tref.header.stamp = this->now();
+            tref.header.frame_id = "infinite_sense";
+            tref.source = "infinite_sense";
+            tref.time_ref = maybe_time.value();
+            time_pub_->publish(tref);
+          }
+        }
+        line_.clear();
+      }
+    }
+  }
+
+  bool is_valid_checksum(const std::string &line) {
+    auto asterisk = line.find('*');
+    if (asterisk == std::string::npos || asterisk + 2 >= line.size()) return false;
+
+    uint8_t checksum = 0;
+    for (size_t i = 1; i < asterisk; ++i) {
+      checksum ^= line[i];
+    }
+
+    std::istringstream iss(line.substr(asterisk + 1, 2));
+    int sent_checksum;
+    iss >> std::hex >> sent_checksum;
+    return checksum == sent_checksum;
+  }
+
+  std::optional<builtin_interfaces::msg::Time> parse_gprmc_utc(const std::string &nmea_line) {
+    std::vector<std::string> fields;
+    std::istringstream ss(nmea_line);
+    std::string token;
+    while (std::getline(ss, token, ',')) {
+      fields.push_back(token);
+    }
+
+    if (fields.size() > 9 && fields[1].length() >= 6 && fields[9].length() >= 6) {
+      try {
+        std::string hh = fields[1].substr(0, 2);
+        std::string mm = fields[1].substr(2, 2);
+        std::string ss = fields[1].substr(4, 2);
+
+        std::string dd = fields[9].substr(0, 2);
+        std::string mo = fields[9].substr(2, 2);
+        std::string yy = fields[9].substr(4, 2);
+
+        std::tm timeinfo = {};
+        timeinfo.tm_year = std::stoi(yy) + 100;
+        timeinfo.tm_mon = std::stoi(mo) - 1;
+        timeinfo.tm_mday = std::stoi(dd);
+        timeinfo.tm_hour = std::stoi(hh);
+        timeinfo.tm_min = std::stoi(mm);
+        timeinfo.tm_sec = std::stoi(ss);
+        timeinfo.tm_isdst = 0;
+
+        time_t utc_sec = timegm(&timeinfo);
+        if (utc_sec == -1) return std::nullopt;
+
+        builtin_interfaces::msg::Time t;
+        t.sec = static_cast<int32_t>(utc_sec);
+        t.nanosec = 0;
+        return t;
+      } catch (...) {
+        return std::nullopt;
+      }
+    }
+
+    return std::nullopt;
+  }
+
+  int fd_;
+  std::string line_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::TimeReference>::SharedPtr time_pub_;
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<GprmcPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}