Initial commit

sensors/rc_receiver/CMakeLists.txt

@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(rc_receiver)
+
+# 查找必要的catkin组件
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  serial
+  std_msgs
+  sensor_msgs
+  message_generation
+)
+
+# 声明消息文件（位于msg/rc.msg）
+add_message_files(
+  FILES
+  rc.msg
+)
+
+# 生成消息文件，依赖std_msgs
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+# 配置catkin包
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES rc_receiver
+  CATKIN_DEPENDS roscpp serial std_msgs message_runtime sensor_msgs
+  DEPENDS system_lib
+)
+
+# 包含头文件路径
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+# 编译可执行文件
+add_executable(rc_receiver_node
+  src/rc_receiver_node.cpp
+)
+
+# 添加消息生成依赖，确保先生成消息再编译可执行文件
+add_dependencies(rc_receiver_node
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+# 链接依赖库
+target_link_libraries(rc_receiver_node
+  ${catkin_LIBRARIES}
+)
+
+# 安装可执行文件
+install(TARGETS rc_receiver_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# 安装头文件
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)

sensors/rc_receiver/include/rc_receiver.h

@@ -0,0 +1,48 @@
+#ifndef RC_RECEIVER_H
+#define RC_RECEIVER_H
+
+#include <ros/ros.h>
+#include <serial/serial.h>
+#include <rc_receiver/rc.h>
+#include <vector>
+#include "sensor_msgs/BatteryState.h"
+
+class RCReceiver {
+public:
+    RCReceiver();
+    void run();
+
+    
+private:
+    ros::Subscriber battery_state_sub_;
+    std::string input_battery_state_topic_;
+    void readSerialData();
+    void parseFrameData();
+    bool parseModbusProtocol(const std::vector<uint8_t> &frame);
+    void battery_status_callback(const sensor_msgs::BatteryState::ConstPtr &msg);
+    void checkTimeout(const ros::TimerEvent& event);
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle private_nh_;
+    serial::Serial ser_;
+    ros::Publisher control_pub_;
+    ros::Timer timeoutTimer_;
+
+    std::vector<uint8_t> serial_buffer_;
+    ros::Time lastReceivedTime_;
+
+    ros::Time lastReconnectAttempt_;  // 记录上一次尝试重连的时间
+
+    const uint8_t AUTO_DRIVING;
+    const uint8_t REMOTE_CONTROL;
+    
+    size_t frames_received_;
+    size_t frames_discarded_;
+    bool debug_;
+
+    // 新增：串口配置参数（用于重连）
+    std::string port_;          // 串口设备路径
+    int baudrate_;              // 波特率
+};
+
+#endif // RC_RECEIVER_H

sensors/rc_receiver/launch/rc_receiver.launch

@@ -0,0 +1,10 @@
+<launch>
+    <node name="rc_receiver_node" pkg="rc_receiver" type="rc_receiver_node" output="screen">
+        <!-- 直接设置参数，无需通过arg中转 -->
+        <param name="port" value="/dev/stm32_rc"/>
+        <param name="baudrate" value="115200"/>
+        <param name="output_remote_ctrl_topic_" value="/remote_ctrl"/>
+        <param name="input_battery_state_topic_" value="/sensor/battery_state"/>
+        <param name="debug" value="false"/>
+    </node>
+</launch>

sensors/rc_receiver/msg/rc.msg

@@ -0,0 +1,12 @@
+# 线性速度（单位：m/s）
+float32 linear_x
+float32 linear_y
+float32 linear_z
+
+# 转向速度（角速度，单位：rad/s）
+float32 angular_x
+float32 angular_y
+float32 angular_z
+
+# 控制模式（0: auto_driving，1: remote_control）
+int8 control_mode

sensors/rc_receiver/package.xml

@@ -0,0 +1,72 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rc_receiver</name>
+  <version>0.0.0</version>
+  <description>The rc_receiver package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="dreamdeck@todo.todo">dreamdeck</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rc_receiver</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>serial</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>serial</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>serial</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

sensors/rc_receiver/src/rc_receiver_node.cpp

@@ -0,0 +1,481 @@
+#include "rc_receiver.h"
+#include <sstream>
+#include <vector>
+#include <algorithm>
+#include <string>
+#include <cmath>
+#include <iomanip>
+
+using namespace std;
+
+RCReceiver::RCReceiver()
+    : AUTO_DRIVING(0),
+      REMOTE_CONTROL(1),
+      frames_received_(0),
+      frames_discarded_(0),
+      debug_(false),
+      private_nh_("~"),
+      nh_("~"),
+      lastReceivedTime_(ros::Time::now())
+{
+
+    // 从参数服务器获取串口配置（默认设为/dev/stm32_rc）
+    private_nh_.param<std::string>("port", port_, "/dev/stm32_rc"); // 默认设备路径
+    private_nh_.param<int>("baudrate", baudrate_, 115200);
+
+    // debug参数可以有默认值（可选参数）
+    nh_.param<bool>("debug", debug_, false);
+    private_nh_.param<std::string>("input_battery_state_topic_", input_battery_state_topic_, "");
+
+    ROS_INFO("Configuration: port=%s, baudrate=%d, debug=%s",
+             port_.c_str(), baudrate_, debug_ ? "true" : "false");
+
+    lastReconnectAttempt_ = ros::Time(0); // 初始化为0时刻（远早于当前时间）
+
+    // 设置日志级别
+    if (debug_)
+    {
+        if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
+                                           ros::console::levels::Debug))
+        {
+            ros::console::notifyLoggerLevelsChanged();
+        }
+    }
+
+    // 初始化串口（首次连接）
+    try
+    {
+        ser_.setPort(port_);
+        ser_.setBaudrate(baudrate_);
+
+        // 创建超时对象
+        serial::Timeout timeout = serial::Timeout::simpleTimeout(100);
+        ser_.setTimeout(timeout);
+
+        ser_.open();
+
+        // 清空启动时的垃圾数据
+        ser_.flushInput();
+        ros::Duration(0.1).sleep();
+
+        ROS_INFO("Serial port %s opened successfully at %d baud",
+                 port_.c_str(), baudrate_);
+        ROS_INFO("Serial settings: timeout=%dms",
+                 timeout.read_timeout_constant);
+    }
+    catch (const serial::IOException &e)
+    {
+        ROS_ERROR("Initial failed to open serial port %s: %s",
+                  port_.c_str(), e.what());
+        // 不立即退出，留待后续重连
+    }
+
+    // 初始化发布者
+    string topic_name;
+    if (!nh_.getParam("output_remote_ctrl_topic_", topic_name))
+    {
+        ROS_FATAL("Missing required parameter: output_remote_ctrl_topic_");
+        ros::shutdown();
+        return;
+    }
+
+    control_pub_ = nh_.advertise<rc_receiver::rc>(topic_name, 10);
+    ROS_INFO("Publisher created for topic: %s", topic_name.c_str());
+
+    // 初始化超时定时器（每0.5秒检查一次连接状态）
+    timeoutTimer_ = nh_.createTimer(ros::Duration(0.5), &RCReceiver::checkTimeout, this);
+}
+
+void RCReceiver::run()
+{
+    ros::Rate rate(20);
+    ROS_INFO("Starting main loop (20Hz)");
+    battery_state_sub_ = nh_.subscribe<sensor_msgs::BatteryState>(
+        input_battery_state_topic_, 1, &RCReceiver::battery_status_callback, this);
+
+    short loopCount = 0;
+    while (ros::ok())
+    {
+        loopCount++;
+        ROS_DEBUG("Loop %d start", loopCount);
+
+        readSerialData(); // 这里会处理重连逻辑
+        ros::spinOnce();
+
+        // 每10秒报告一次状态
+        if (loopCount % 200 == 0)
+        {
+            ROS_INFO("Still running. Serial buffer: %zu bytes, Frames: %zu good, %zu bad",
+                     serial_buffer_.size(), frames_received_, frames_discarded_);
+            loopCount = 0;
+        }
+
+        rate.sleep();
+    }
+
+    // 退出时关闭串口
+    if (ser_.isOpen())
+    {
+        ser_.close();
+        ROS_INFO("Serial port %s closed", port_.c_str());
+    }
+}
+
+// 电池状态回调函数（保持不变）
+uint8_t battery_info[6] = {0x5F, 0x00, 0x00, 0x00, 0x00, 0X9B};
+void RCReceiver::battery_status_callback(const sensor_msgs::BatteryState::ConstPtr &msg)
+{
+    if (!ser_.isOpen())
+        return; // 串口未打开时不发送数据
+
+    uint16_t soc_ = msg->percentage * 10;
+    battery_info[1] = (msg->power_supply_status == 1) ? 1 : 0;
+    battery_info[2] = (soc_ & 0xFFFF) >> 8;
+    battery_info[3] = soc_ & 0xFF;
+    uint8_t checksum = 0;
+    for (size_t i = 0; i < 4; ++i)
+    {
+        checksum ^= battery_info[i];
+    }
+    battery_info[4] = checksum;
+
+    try
+    {
+        ser_.write(battery_info, sizeof(battery_info));
+    }
+    catch (const serial::IOException &e)
+    {
+        ROS_ERROR("Failed to write battery info: %s", e.what());
+        ser_.close(); // 写失败时主动关闭，触发重连
+    }
+}
+
+void RCReceiver::readSerialData()
+{
+    if (!ser_.isOpen())
+    {
+        // 计算当前时间与上次重连尝试的时间差（秒）
+        double timeSinceLastAttempt = (ros::Time::now() - lastReconnectAttempt_).toSec();
+
+        // 仅当间隔≥2秒时，才尝试重连
+        if (timeSinceLastAttempt >= 2.0)
+        {
+            ROS_WARN("Serial port %s is closed, attempting to reopen...", port_.c_str()); // 无需THROTTLE，因为本身已间隔2秒
+            try
+            {
+                // 重新配置串口参数（重连核心逻辑）
+                ser_.setPort(port_);
+                ser_.setBaudrate(baudrate_);
+                serial::Timeout timeout = serial::Timeout::simpleTimeout(100);
+                ser_.setTimeout(timeout);
+
+                ser_.open();
+                ser_.flushInput();                    // 清空输入缓冲区
+                serial_buffer_.clear();               // 清除历史缓冲数据
+                lastReceivedTime_ = ros::Time::now(); // 重置接收时间
+
+                ROS_INFO("Serial port %s reopened successfully at %d baud",
+                         port_.c_str(), baudrate_);
+            }
+            catch (const serial::IOException &e)
+            {
+                ROS_ERROR("Failed to reopen serial port %s: %s",
+                          port_.c_str(), e.what());
+            }
+            catch (const serial::PortNotOpenedException &e)
+            {
+                ROS_ERROR("Port %s not open: %s",
+                          port_.c_str(), e.what());
+            }
+
+            // 更新上次重连尝试时间（无论成功与否，都间隔2秒再试）
+            lastReconnectAttempt_ = ros::Time::now();
+        }
+        // 若间隔不足2秒，则不执行任何操作，等待下次主循环检查
+    }
+
+    // 读取串口数据（仅在串口打开状态下执行）
+    try
+    {
+        const size_t available = ser_.available();
+        ROS_DEBUG("Serial available: %zu bytes", available);
+
+        if (available > 0)
+        {
+            vector<uint8_t> buffer(available);
+            size_t bytes_read = ser_.read(buffer.data(), available);
+            ROS_DEBUG("Read %zu bytes", bytes_read);
+
+            serial_buffer_.insert(serial_buffer_.end(), buffer.begin(), buffer.end());
+            lastReceivedTime_ = ros::Time::now();
+
+            if (debug_)
+            {
+                ROS_DEBUG("Buffer now: %zu bytes", serial_buffer_.size());
+                stringstream ss;
+                for (size_t i = 0; i < min(bytes_read, size_t(20)); ++i)
+                {
+                    ss << "0x" << hex << setw(2) << setfill('0')
+                       << static_cast<int>(buffer[i]) << " ";
+                }
+                ROS_DEBUG("Raw data (first 20): %s", ss.str().c_str());
+            }
+        }
+        else
+        {
+            ROS_DEBUG_THROTTLE(5, "No data available on %s", port_.c_str());
+        }
+    }
+    catch (const serial::IOException &e)
+    {
+        ROS_ERROR("Read error on %s: %s", port_.c_str(), e.what());
+        ser_.close(); // 读失败时主动关闭，触发下次重连
+    }
+    catch (const serial::PortNotOpenedException &e)
+    {
+        ROS_ERROR("Port %s not open during read: %s", port_.c_str(), e.what());
+    }
+
+    parseFrameData();
+}
+
+void RCReceiver::parseFrameData()
+{
+    const uint8_t FRAME_HEADER = 0x5F;
+    const uint8_t FRAME_TAIL = 0x9B;
+    const size_t FRAME_LENGTH = 10;
+    const size_t MAX_BUFFER_SIZE = 1000;
+
+    ROS_DEBUG("Enter parseFrameData(). Buffer size: %zu", serial_buffer_.size());
+
+    // 安全：限制缓冲区大小
+    if (serial_buffer_.size() > MAX_BUFFER_SIZE)
+    {
+        ROS_WARN("Buffer overflow (%zu > %zu), clearing buffer",
+                 serial_buffer_.size(), MAX_BUFFER_SIZE);
+        serial_buffer_.clear();
+        frames_discarded_++;
+        return;
+    }
+
+    // 当缓冲区长度不足时直接返回
+    if (serial_buffer_.size() < FRAME_LENGTH)
+    {
+        ROS_DEBUG("Buffer too small (%zu < %zu)", serial_buffer_.size(), FRAME_LENGTH);
+        return;
+    }
+
+    // 帧定位主循环
+    size_t validFramePos = -1;
+    for (size_t pos = 0; pos <= serial_buffer_.size() - FRAME_LENGTH; pos++)
+    {
+        ROS_DEBUG("Checking position %zu", pos);
+
+        // 检查帧头
+        uint8_t header = static_cast<uint8_t>(serial_buffer_[pos]);
+        if (header != FRAME_HEADER)
+        {
+            ROS_DEBUG("No header at %zu (0x%02X)", pos, header);
+            continue;
+        }
+
+        // 检查帧尾
+        size_t tail_pos = pos + FRAME_LENGTH - 1;
+        uint8_t tail = static_cast<uint8_t>(serial_buffer_[tail_pos]);
+
+        if (tail != FRAME_TAIL)
+        {
+            ROS_DEBUG("Bad frame tail at %zu: expected 0x%02X, got 0x%02X",
+                      tail_pos, FRAME_TAIL, tail);
+            frames_discarded_++;
+            continue;
+        }
+
+        ROS_DEBUG("Valid frame found at position %zu", pos);
+        validFramePos = pos;
+        break;
+    }
+
+    if (validFramePos == (size_t)-1)
+    {
+        ROS_DEBUG("No valid frame found in buffer");
+
+        // 如果找不到有效帧，但缓冲区足够大，清除非帧头数据
+        if (!serial_buffer_.empty())
+        {
+            auto firstHeader = find(serial_buffer_.begin(), serial_buffer_.end(), FRAME_HEADER);
+            if (firstHeader != serial_buffer_.begin())
+            {
+                size_t discardSize = distance(serial_buffer_.begin(), firstHeader);
+                ROS_DEBUG("Discarding %zu bytes before first header", discardSize);
+                serial_buffer_.erase(serial_buffer_.begin(), firstHeader);
+            }
+            else
+            {
+                // 如果没有找到帧头，清除所有数据
+                ROS_DEBUG("No header found, clearing entire buffer");
+                serial_buffer_.clear();
+            }
+        }
+        return;
+    }
+
+    // 提取有效帧
+    vector<uint8_t> frame(
+        serial_buffer_.begin() + validFramePos,
+        serial_buffer_.begin() + validFramePos + FRAME_LENGTH);
+
+    ROS_DEBUG("Processing frame at pos %zu", validFramePos);
+    bool parsed = parseModbusProtocol(frame);
+
+    if (parsed)
+    {
+        frames_received_++;
+        ROS_DEBUG("Frame parsed successfully (#%zu)", frames_received_);
+    }
+    else
+    {
+        frames_discarded_++;
+        ROS_DEBUG("Frame parsing failed (#%zu)", frames_discarded_);
+    }
+
+    // 移除已处理帧
+    size_t newStart = validFramePos + FRAME_LENGTH;
+    serial_buffer_.erase(serial_buffer_.begin(), serial_buffer_.begin() + newStart);
+    ROS_DEBUG("Buffer after erase: %zu bytes", serial_buffer_.size());
+}
+
+bool RCReceiver::parseModbusProtocol(const vector<uint8_t> &frame)
+{
+    // 基本完整性检查
+    if (frame.size() != 10)
+    {
+        ROS_WARN_THROTTLE(1, "Invalid frame size: expected 10, got %zu", frame.size());
+        return false;
+    }
+
+    rc_receiver::rc msg;
+
+    // 帧头帧尾校验
+    if (frame[0] != 0x5F || frame[9] != 0x9B)
+    {
+        ROS_WARN_THROTTLE(1, "Invalid frame: header=0x%02X, tail=0x%02X",
+                          static_cast<uint8_t>(frame[0]),
+                          static_cast<uint8_t>(frame[9]));
+        return false;
+    }
+
+    // 校验和计算（前8字节异或和）
+    uint8_t checksum = 0;
+    for (size_t i = 0; i < 8; ++i)
+    {
+        checksum ^= frame[i];
+    }
+
+    if (checksum != static_cast<uint8_t>(frame[8]))
+    {
+        ROS_WARN_THROTTLE(1, "Checksum error: calculated 0x%02X vs received 0x%02X",
+                          checksum, static_cast<uint8_t>(frame[8]));
+        return false;
+    }
+
+    // 解析控制模式
+    msg.control_mode = static_cast<uint8_t>(frame[1]);
+    if (msg.control_mode != AUTO_DRIVING && msg.control_mode != REMOTE_CONTROL)
+    {
+        ROS_WARN_THROTTLE(1, "Invalid control mode: %d", msg.control_mode);
+        return false;
+    }
+
+    // 解析速度数据（使用安全类型转换）
+    int16_t linear_raw = static_cast<int16_t>((frame[2] << 8) | frame[3]);
+    int16_t angular_raw = static_cast<int16_t>((frame[6] << 8) | frame[7]);
+
+    // 设置合理范围限制（±2m/s和±2rad/s）
+    const float MAX_SPEED = 2.0f;
+    msg.linear_x = linear_raw * 0.01f;
+    msg.angular_z = angular_raw * 0.01f;
+
+    if (fabs(msg.linear_x) > MAX_SPEED)
+    {
+        ROS_WARN_THROTTLE(1, "Unreasonable linear speed: %.2f m/s", msg.linear_x);
+        msg.linear_x = 0.0f;
+    }
+
+    if (fabs(msg.angular_z) > MAX_SPEED)
+    {
+        ROS_WARN_THROTTLE(1, "Unreasonable angular speed: %.2f rad/s", msg.angular_z);
+        msg.angular_z = 0.0f;
+    }
+
+    // 发布消息
+    control_pub_.publish(msg);
+
+    // ROS_INFO("Received frame #%zu: mode=%s, linear=%.2f m/s, angular=%.2f rad/s",
+    //             frames_received_,
+    //             msg.control_mode == AUTO_DRIVING ? "AUTO" : "REMOTE",
+    //             msg.linear_x, msg.angular_z);
+
+    return true;
+}
+
+// 超时检查：更安全的发布逻辑
+void RCReceiver::checkTimeout(const ros::TimerEvent &event)
+{
+    ros::Duration delta = ros::Time::now() - lastReceivedTime_;
+
+    if (delta >= ros::Duration(1.0))
+    {
+        // 安全获取节点句柄状态
+        if (!ros::ok())
+            return;
+
+        // 在发布前检查订阅者是否存在
+        if (control_pub_.getNumSubscribers() > 0)
+        {
+            rc_receiver::rc msg;
+            msg.linear_x = 0.0f;
+            msg.angular_z = 0.0f;
+
+            try
+            {
+                control_pub_.publish(msg);
+                ROS_WARN_THROTTLE(2, "No data for %.1fs, speed zeroed", delta.toSec());
+            }
+            catch (const ros::Exception &e)
+            {
+                ROS_ERROR("Failed to publish timeout message: %s", e.what());
+            }
+        }
+
+        // // 定期打印统计信息
+        // ROS_INFO_THROTTLE(30, "Received stats: good=%zu, bad=%zu",
+        //                  frames_received_, frames_discarded_);
+    }
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "rc_receiver_node");
+    ROS_INFO("RC Receiver node starting...");
+
+    try
+    {
+        RCReceiver rcReceiver;
+        ROS_INFO("Initialization completed, starting run loop");
+        rcReceiver.run();
+    }
+    catch (const std::exception &e)
+    {
+        ROS_FATAL("Unhandled exception: %s", e.what());
+        return 1;
+    }
+    catch (...)
+    {
+        ROS_FATAL("Unknown exception occurred");
+        return 1;
+    }
+
+    ROS_INFO("RC Receiver node stopped");
+    return 0;
+}