Skip to content

Instantly share code, notes, and snippets.

@litch

litch/serial_bridge_node.cpp

Created July 15, 2025 11:35
Show Gist options
  • Save litch/fb656b0bd15d9ba729535aabcac5dc50 to your computer and use it in GitHub Desktop.
Save litch/fb656b0bd15d9ba729535aabcac5dc50 to your computer and use it in GitHub Desktop.
Download ZIP
Two nodes (the python and the cpp version) that read lines off the serial port and publish the appropriate ROS messages.
Raw
serial_bridge_node.cpp
#include "robot_bridge_cpp/serial_bridge_node.hpp"
#include <fstream>
#include <sstream>
#include <chrono>
#include <cmath>
#include <algorithm>
using namespace std::chrono_literals;
namespace robot_bridge_cpp
{
SerialBridgeNode::SerialBridgeNode()
: Node("serial_bridge_node"),
serial_fd_(-1),
running_(false),
log_next_odom_(true),
log_next_imu_(true),
log_next_telemetry_(true)
{
line_buffer_.reserve(256); // Pre-allocate buffer
// Declare parameters
this->declare_parameter("port", "/dev/ttySCOUT_esp");
this->declare_parameter("baudrate", 460800);
this->declare_parameter("simulate", false);
this->declare_parameter("namespace", "/scout");
}
SerialBridgeNode::~SerialBridgeNode()
{
// Ensure threads are stopped and joined
running_ = false;
if (serial_read_thread_.joinable()) {
serial_read_thread_.join();
}
if (message_processing_thread_.joinable()) {
queue_cv_.notify_all();
message_processing_thread_.join();
}
// Close serial port if open
if (serial_fd_ >= 0) {
close(serial_fd_);
serial_fd_ = -1;
}
// Close file if open
if (simulated_data_file_.is_open()) {
simulated_data_file_.close();
}
}
bool SerialBridgeNode::initialize()
{
// Get parameters
port_name_ = this->get_parameter("port").as_string();
baudrate_ = this->get_parameter("baudrate").as_int();
simulate_ = this->get_parameter("simulate").as_bool();
namespace_ = this->get_parameter("namespace").as_string();
// Declare and get debug log interval parameter (default to 2 seconds)
this->declare_parameter("debug_log_interval", 20.0);
debug_log_interval_ = this->get_parameter("debug_log_interval").as_double();
// Ensure namespace starts with a slash
if (namespace_[0] != '/') {
namespace_ = "/" + namespace_;
RCLCPP_WARN(get_logger(), "Namespace %s does not start with a slash, adding one", namespace_.c_str());
}
RCLCPP_INFO(get_logger(), "Using namespace: %s", namespace_.c_str());
try {
if (simulate_) {
RCLCPP_INFO(get_logger(), "Running in simulation mode (reading from file)");
simulated_data_file_.open("/tmp/fake_serial.txt");
if (!simulated_data_file_.is_open()) {
RCLCPP_ERROR(get_logger(), "Failed to open simulation data file");
return false;
}
} else {
// Try to connect to serial port with retries
const int max_retries = 20;
const auto retry_delay = 500ms;
for (int attempt = 0; attempt < max_retries; ++attempt) {
// Open serial port
serial_fd_ = open(port_name_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if (serial_fd_ < 0) {
RCLCPP_WARN(get_logger(), "Attempt %d/%d failed to connect to %s: %s",
attempt + 1, max_retries, port_name_.c_str(), strerror(errno));
if (attempt == max_retries - 1) {
RCLCPP_ERROR(get_logger(), "Failed to connect to serial port %s after %d attempts.",
port_name_.c_str(), max_retries);
return false;
}
std::this_thread::sleep_for(retry_delay);
continue;
}
// Configure serial port
struct termios tty;
memset(&tty, 0, sizeof(tty));
// Get current attributes
if (tcgetattr(serial_fd_, &tty) != 0) {
RCLCPP_ERROR(get_logger(), "Error from tcgetattr: %s", strerror(errno));
close(serial_fd_);
serial_fd_ = -1;
std::this_thread::sleep_for(retry_delay);
continue;
}
// Set baud rate
speed_t baud;
switch (baudrate_) {
case 9600: baud = B9600; break;
case 19200: baud = B19200; break;
case 38400: baud = B38400; break;
case 57600: baud = B57600; break;
case 115200: baud = B115200; break;
case 230400: baud = B230400; break;
case 460800: baud = B460800; break;
case 500000: baud = B500000; break;
case 921600: baud = B921600; break;
case 1000000: baud = B1000000; break;
default:
RCLCPP_ERROR(get_logger(), "Unsupported baud rate: %d", baudrate_);
close(serial_fd_);
serial_fd_ = -1;
return false;
}
cfsetispeed(&tty, baud);
cfsetospeed(&tty, baud);
// 8N1 mode, no flow control
tty.c_cflag &= ~PARENB; // No parity
tty.c_cflag &= ~CSTOPB; // 1 stop bit
tty.c_cflag &= ~CSIZE;
tty.c_cflag |= CS8; // 8 bits per byte
tty.c_cflag &= ~CRTSCTS; // No hardware flow control
tty.c_cflag |= CREAD | CLOCAL; // Enable receiver, ignore modem control lines
// Raw input
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// Raw output
tty.c_oflag &= ~OPOST;
// Set timeout for non-blocking read
tty.c_cc[VTIME] = 1; // 0.1 seconds timeout
tty.c_cc[VMIN] = 0; // No minimum characters
// Apply attributes
if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
RCLCPP_ERROR(get_logger(), "Error from tcsetattr: %s", strerror(errno));
close(serial_fd_);
serial_fd_ = -1;
std::this_thread::sleep_for(retry_delay);
continue;
}
// Success
RCLCPP_INFO(get_logger(), "Serial connected on %s at %d baud", port_name_.c_str(), baudrate_);
break;
}
// Check if we successfully opened the port
if (serial_fd_ < 0) {
RCLCPP_ERROR(get_logger(), "Failed to open serial port %s", port_name_.c_str());
return false;
}
}
} catch (const std::exception& e) {
RCLCPP_ERROR(get_logger(), "Final failure: %s", e.what());
return false;
}
// Create publishers
odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>(namespace_ + "/odom", 10);
imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>(namespace_ + "/imu", 10);
telemetry_publisher_ = this->create_publisher<std_msgs::msg::String>(namespace_ + "/telemetry", 10);
// Create debug log timer
debug_log_timer_ = this->create_wall_timer(
std::chrono::duration<double>(debug_log_interval_),
std::bind(&SerialBridgeNode::debugLogTimerCallback, this));
// Create subscription
cmd_vel_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
namespace_ + "/cmd_vel",
10,
std::bind(&SerialBridgeNode::cmdVelCallback, this, std::placeholders::_1)
);
blade_subscription_ = this->create_subscription<std_msgs::msg::String>(
namespace_ + "/blade",
10,
std::bind(&SerialBridgeNode::bladeCallback, this, std::placeholders::_1)
);
return true;
}
bool SerialBridgeNode::start()
{
// Start threads for serial reading and message processing
running_ = true;
// Start the message processing thread
message_processing_thread_ = std::thread(&SerialBridgeNode::processMessagesThread, this);
// Start the serial reading thread
serial_read_thread_ = std::thread(&SerialBridgeNode::readSerialThread, this);
RCLCPP_INFO(get_logger(), "SerialBridgeNode started");
return true;
}
bool SerialBridgeNode::stop()
{
// Stop threads
running_ = false;
// Notify processing thread to wake up and check running_ flag
queue_cv_.notify_all();
// Join threads
if (serial_read_thread_.joinable()) {
serial_read_thread_.join();
}
if (message_processing_thread_.joinable()) {
message_processing_thread_.join();
}
RCLCPP_INFO(get_logger(), "SerialBridgeNode stopped");
return true;
}
// Cleanup resources is now handled in the destructor
void SerialBridgeNode::readSerialThread()
{
while (running_) {
try {
if (serial_fd_ >= 0) {
// Read from serial port with timeout
ssize_t bytes_read = read(serial_fd_, read_buffer_, READ_BUFFER_SIZE - 1);
if (bytes_read > 0) {
// Null-terminate the buffer
read_buffer_[bytes_read] = '\0';
// Process each character
for (ssize_t i = 0; i < bytes_read; ++i) {
char c = read_buffer_[i];
if (c == '\n' || c == '\r') {
// End of line, process if not empty
if (!line_buffer_.empty()) {
// Add to processing queue
{
std::lock_guard<std::mutex> lock(queue_mutex_);
message_queue_.push(line_buffer_);
}
queue_cv_.notify_one();
// Clear buffer for next line
line_buffer_.clear();
}
} else {
// Add character to line buffer
line_buffer_ += c;
}
}
} else if (bytes_read < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
// Error reading from serial port
RCLCPP_ERROR(get_logger(), "Error reading from serial port: %s", strerror(errno));
std::this_thread::sleep_for(100ms);
} else {
// No data available or would block, sleep briefly
std::this_thread::sleep_for(1ms);
}
} else if (simulate_ && simulated_data_file_.is_open()) {
// Read from simulation file
std::string line;
if (std::getline(simulated_data_file_, line)) {
if (!line.empty()) {
// Add to processing queue
{
std::lock_guard<std::mutex> lock(queue_mutex_);
message_queue_.push(line);
}
queue_cv_.notify_one();
// Add a small delay to simulate realistic timing
std::this_thread::sleep_for(10ms);
}
} else {
// Reached end of file, rewind
RCLCPP_INFO(get_logger(), "Reached end of simulated data file, rewinding");
simulated_data_file_.clear();
simulated_data_file_.seekg(0);
std::this_thread::sleep_for(3s);
}
} else {
// Neither serial port nor simulation file available
std::this_thread::sleep_for(100ms);
}
} catch (const std::exception& e) {
RCLCPP_ERROR(get_logger(), "Error in serial read thread: %s", e.what());
std::this_thread::sleep_for(100ms);
}
}
}
void SerialBridgeNode::processMessagesThread()
{
while (running_) {
std::string line;
{
std::unique_lock<std::mutex> lock(queue_mutex_);
queue_cv_.wait(lock, [this]() { return !running_ || !message_queue_.empty(); });
if (!running_ && message_queue_.empty()) {
break;
}
if (!message_queue_.empty()) {
line = message_queue_.front();
message_queue_.pop();
}
}
if (!line.empty()) {
dispatchMessage(line);
}
}
}
void SerialBridgeNode::dispatchMessage(const std::string &line)
{
if (line.rfind("ODOM:", 0) == 0) {
handleOdom(line);
} else if (line.rfind("IMU:", 0) == 0) {
handleImu(line);
} else if (line.rfind("TELEMETRY:", 0) == 0) {
handleTelemetry(line);
} else {
RCLCPP_WARN(get_logger(), "Unknown message type: %s", line.c_str());
}
}
std::vector<float> SerialBridgeNode::parseFloatValues(const std::string &line, size_t start_pos)
{
std::vector<float> values;
std::istringstream ss(line.substr(start_pos));
std::string value;
while (std::getline(ss, value, ',')) {
try {
values.push_back(std::stof(value));
} catch (const std::exception& e) {
RCLCPP_WARN(get_logger(), "Failed to parse float value: %s", value.c_str());
}
}
return values;
}
void SerialBridgeNode::handleOdom(const std::string &line)
{
try {
// Increment counter and log raw data every 1000 iterations
if (log_next_odom_) {
RCLCPP_INFO(get_logger(), "Raw ODOM data: %s", line.c_str());
log_next_odom_ = false;
}
// Parse values from line
std::vector<float> values = parseFloatValues(line, 5); // Skip "ODOM:"
if (values.size() < 9) {
RCLCPP_WARN(get_logger(), "ODOM message too short: %s", line.c_str());
return;
}
// sprintf(buffer, "ODOM:%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%ld,%ld",
// pos_x, pos_y, theta, cmd_linear_x, cmd_angular_z, measured_linear_x, measured_angular_z,
// leftEncoderCount, rightEncoderCount);
// Extract values
float pos_x = values[0];
float pos_y = values[1];
float theta = values[2];
float measured_linear_x = values[5];
float measured_angular_z = values[6];
// Create and fill odometry message
auto odom_msg = std::make_unique<nav_msgs::msg::Odometry>();
odom_msg->header.stamp = this->now();
odom_msg->header.frame_id = "odom";
odom_msg->child_frame_id = "base_link";
// Set position
odom_msg->pose.pose.position.x = pos_x;
odom_msg->pose.pose.position.y = pos_y;
odom_msg->pose.pose.position.z = 0.0;
// Set orientation using tf2 quaternion
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, theta);
odom_msg->pose.pose.orientation = tf2::toMsg(quat);
odom_msg->pose.covariance = {
999999999.0, 0, 0, 0, 0, 0,
0, 999999999.0, 0, 0, 0, 0,
0, 0, 999999999.0, 0, 0, 0,
0, 0, 0, 999999999.0, 0, 0,
0, 0, 0, 0, 999999999.0, 0,
0, 0, 0, 0, 0, 999999999.0
};
odom_msg->twist.twist.linear.x = measured_linear_x;
odom_msg->twist.twist.linear.y = 0.0;
odom_msg->twist.twist.angular.z = measured_angular_z;
odom_msg->twist.covariance = {
0.1, 0, 0, 0, 0, 0,
0, 0.1, 0, 0, 0, 0,
0, 0, 0.1, 0, 0, 0,
0, 0, 0, 0.1, 0, 0,
0, 0, 0, 0, 0.1, 0,
0, 0, 0, 0, 0, 0.2
};
// Publish
odom_publisher_->publish(std::move(odom_msg));
} catch (const std::exception& e) {
RCLCPP_WARN(get_logger(), "Failed to parse ODOM message: %s", e.what());
}
}
void SerialBridgeNode::handleImu(const std::string &line)
{
try {
// Increment counter and log raw data every 1000 iterations
if (log_next_imu_) {
RCLCPP_INFO(get_logger(), "Raw IMU data: %s", line.c_str());
log_next_imu_ = false;
}
// Parse values from line
std::vector<float> values = parseFloatValues(line, 4); // Skip "IMU:"
if (values.size() < 16) {
RCLCPP_WARN(get_logger(), "IMU message too short: %s", line.c_str());
return;
}
// Extract values
// float _millis = values[0];
float qx = values[1];
float qy = values[2];
float qz = values[3];
float qw = values[4];
// float _yaw = values[5];
// float _pitch = values[6];
// float _roll = values[7];
float gx = values[8];
float gy = values[9];
float gz = values[10];
float ax = values[11];
float ay = values[12];
float az = values[13];
// float _cal_rot = values[14];
// float _cal_geomag = values[15];
// Create and fill IMU message
auto imu_msg = std::make_unique<sensor_msgs::msg::Imu>();
imu_msg->header.stamp = this->now();
imu_msg->header.frame_id = "imu_link";
// Set orientation
imu_msg->orientation.x = qx;
imu_msg->orientation.y = qy;
imu_msg->orientation.z = qz;
imu_msg->orientation.w = qw;
// Set angular velocity
imu_msg->angular_velocity.x = gx;
imu_msg->angular_velocity.y = gy;
imu_msg->angular_velocity.z = gz;
// Set linear acceleration
imu_msg->linear_acceleration.x = ax;
imu_msg->linear_acceleration.y = ay;
imu_msg->linear_acceleration.z = az;
// Fill in covariance matrices - required for robot_localization
std::array<double, 9> orientation_cov;
orientation_cov.fill(0.0);
orientation_cov[0] = orientation_cov[4] = orientation_cov[8] = 0.01;
imu_msg->orientation_covariance = orientation_cov;
std::array<double, 9> angular_vel_cov;
angular_vel_cov.fill(0.0);
angular_vel_cov[0] = angular_vel_cov[4] = angular_vel_cov[8] = 0.01;
imu_msg->angular_velocity_covariance = angular_vel_cov;
std::array<double, 9> linear_acc_cov;
linear_acc_cov.fill(0.0);
linear_acc_cov[0] = linear_acc_cov[4] = linear_acc_cov[8] = 0.01;
imu_msg->linear_acceleration_covariance = linear_acc_cov;
// Publish
imu_publisher_->publish(std::move(imu_msg));
} catch (const std::exception& e) {
RCLCPP_ERROR(get_logger(), "Failed to parse IMU message: %s", e.what());
}
}
void SerialBridgeNode::handleTelemetry(const std::string &line) {
try {
// Increment counter and log raw data every 1000 iterations
if (log_next_telemetry_) {
RCLCPP_INFO(get_logger(), "Raw telemetry data: %s", line.c_str());
log_next_telemetry_ = false;
}
// Parse values from line
std::vector<float> values = parseFloatValues(line, 11); // Skip "TELEMETRY:"
if (values.size() < 2) {
RCLCPP_WARN(get_logger(), "Telemetry message too short: %s", line.c_str());
return;
}
// Extract values
float timestamp = values[0];
float hz = values[1];
// Create and fill telemetry message
auto telemetry_msg = std::make_unique<std_msgs::msg::String>();
// telemetry_msg->header.stamp = this->now();
// Set values
telemetry_msg->data = std::to_string(timestamp) + "," + std::to_string(hz);
// Publish
telemetry_publisher_->publish(std::move(telemetry_msg));
} catch (const std::exception& e) {
RCLCPP_ERROR(get_logger(), "Failed to parse telemetry message: %s", e.what());
}
}
void SerialBridgeNode::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
try {
// Extract linear and angular velocities
float linear_velocity = static_cast<float>(msg->linear.x);
float angular_velocity = static_cast<float>(msg->angular.z);
// Format the command string
std::ostringstream cmd_stream;
cmd_stream << "CMD:" << linear_velocity << "," << angular_velocity << "\n";
std::string cmd_str = cmd_stream.str();
// Send the command to the serial port if available
if (serial_fd_ >= 0) {
ssize_t bytes_written = write(serial_fd_, cmd_str.c_str(), cmd_str.length());
if (bytes_written < 0) {
RCLCPP_ERROR(get_logger(), "Error writing to serial port: %s", strerror(errno));
} else if (bytes_written < static_cast<ssize_t>(cmd_str.length())) {
RCLCPP_WARN(get_logger(), "Incomplete write to serial port");
} else {
RCLCPP_DEBUG(get_logger(), "Sent command: %s", cmd_str.c_str());
}
} else if (simulate_) {
RCLCPP_DEBUG(get_logger(), "Would send command (simulation mode): %s", cmd_str.c_str());
}
} catch (const std::exception& e) {
RCLCPP_ERROR(get_logger(), "Error sending cmd_vel command: %s", e.what());
}
}
void SerialBridgeNode::bladeCallback(const std_msgs::msg::String::SharedPtr msg)
{
try {
// Extract the throttle value from the string
float throttle = std::stof(msg->data);
// Format the command string
std::ostringstream cmd_stream;
cmd_stream << "BLADE:" << throttle << "\n";
std::string cmd_str = cmd_stream.str();
// Send the command to the serial port if available
if (serial_fd_ >= 0) {
ssize_t bytes_written = write(serial_fd_, cmd_str.c_str(), cmd_str.length());
if (bytes_written < 0) {
RCLCPP_ERROR(get_logger(), "Error writing to serial port: %s", strerror(errno));
} else if (bytes_written < static_cast<ssize_t>(cmd_str.length())) {
RCLCPP_WARN(get_logger(), "Incomplete write to serial port");
} else {
RCLCPP_DEBUG(get_logger(), "Sent command: %s", cmd_str.c_str());
}
} else if (simulate_) {
RCLCPP_DEBUG(get_logger(), "Would send command (simulation mode): %s", cmd_str.c_str());
}
} catch (const std::exception& e) {
RCLCPP_ERROR(get_logger(), "Error sending blade command: %s", e.what());
}
}
void SerialBridgeNode::debugLogTimerCallback()
{
// Set the flags to true to trigger logging on the next message
log_next_odom_ = true;
log_next_imu_ = true;
log_next_telemetry_ = true;
RCLCPP_DEBUG(get_logger(), "Debug logging flags set for next odom and IMU messages");
}
} // namespace robot_bridge_cpp
// Main function
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
// Create node
auto node = std::make_shared<robot_bridge_cpp::SerialBridgeNode>();
// Initialize the node
if (!node->initialize()) {
RCLCPP_ERROR(node->get_logger(), "Failed to initialize node");
return 1;
}
// Start processing
if (!node->start()) {
RCLCPP_ERROR(node->get_logger(), "Failed to start node");
return 1;
}
// Use a multithreaded executor for better performance
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
// Spin
executor.spin();
// Stop processing
node->stop();
// Cleanup
rclcpp::shutdown();
return 0;
}
Raw
serial_bridge_node.hpp
#ifndef ROBOT_BRIDGE_CPP_SERIAL_BRIDGE_NODE_HPP
#define ROBOT_BRIDGE_CPP_SERIAL_BRIDGE_NODE_HPP
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include <mutex>
#include <atomic>
#include <queue>
#include <condition_variable>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <fstream>
namespace robot_bridge_cpp
{
class SerialBridgeNode : public rclcpp::Node
{
public:
SerialBridgeNode();
~SerialBridgeNode();
// Initialize the node (called after construction)
bool initialize();
// Start processing (equivalent to on_activate)
bool start();
// Stop processing (equivalent to on_deactivate)
bool stop();
private:
// Serial port handling
int serial_fd_;
std::string port_name_;
int baudrate_;
bool simulate_;
std::string namespace_;
std::ifstream simulated_data_file_;
// Buffer for serial reading
static constexpr size_t READ_BUFFER_SIZE = 1024;
char read_buffer_[READ_BUFFER_SIZE];
std::string line_buffer_;
// Thread for reading from serial port
std::thread serial_read_thread_;
std::atomic<bool> running_;
// Message processing queue and thread
std::queue<std::string> message_queue_;
std::mutex queue_mutex_;
std::condition_variable queue_cv_;
std::thread message_processing_thread_;
// Publishers
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr telemetry_publisher_;
// Subscription
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr blade_subscription_;
// Counters for logging
std::atomic<int> odom_counter_;
std::atomic<int> imu_counter_;
// Debug logging flags
std::atomic<bool> log_next_odom_;
std::atomic<bool> log_next_imu_;
std::atomic<bool> log_next_telemetry_;
// Timer for periodic logging
rclcpp::TimerBase::SharedPtr debug_log_timer_;
double debug_log_interval_;
// Serial reading function (runs in separate thread)
void readSerialThread();
// Message processing function (runs in separate thread)
void processMessagesThread();
// Message handlers
void dispatchMessage(const std::string &line);
void handleOdom(const std::string &line);
void handleImu(const std::string &line);
void handleTelemetry(const std::string &line);
// Command velocity callback
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
// Blade callback
void bladeCallback(const std_msgs::msg::String::SharedPtr msg);
// Helper functions
std::vector<float> parseFloatValues(const std::string &line, size_t start_pos);
// Debug logging timer callback
void debugLogTimerCallback();
};
} // namespace robot_bridge_cpp
#endif // ROBOT_BRIDGE_CPP_SERIAL_BRIDGE_NODE_HPP
Raw
serial_bridge_node.py
import time
import rclpy
import serial
from lifecycle_msgs.msg import State
from rclpy.lifecycle import LifecycleNode
from std_msgs.msg import String
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from scipy.spatial.transform import Rotation as R
class SerialBridgeNode(LifecycleNode):
def __init__(self):
super().__init__('serial_bridge_node')
self.ser = None
self.timer = None
self.odom_counter = 0
self.imu_counter = 0
def on_configure(self, state):
# Parameters
self.declare_parameter('port', '/dev/ttySCOUT_esp')
self.declare_parameter('baudrate', 460800)
self.declare_parameter('simulate', False)
self.declare_parameter('namespace', '/scout')
namespace = self.get_parameter('namespace').get_parameter_value().string_value
if namespace[0] != '/':
namespace = '/' + namespace
self.get_logger().warn(f'Namespace {namespace} does not start with a slash, adding one')
self.get_logger().info(f'Using namespace: {namespace}')
port = self.get_parameter('port').get_parameter_value().string_value
baud = self.get_parameter('baudrate').get_parameter_value().integer_value
simulate = self.get_parameter('simulate').get_parameter_value().bool_value
try:
if simulate:
self.get_logger().info("Running in simulation mode (reading from file)")
self.simulated_data = open('/tmp/fake_serial.txt', 'r')
self.ser = None
else:
max_retries = 20
retry_delay = 0.5 # seconds
for attempt in range(max_retries):
try:
self.ser = serial.Serial(port, baud, timeout=1)
self.get_logger().info(f'Serial connected on {port} at {baud} baud')
break
except serial.SerialException as e:
self.get_logger().warn(f'Attempt {attempt + 1}/{max_retries} failed to connect to {port}: {e}')
time.sleep(retry_delay)
else:
self.get_logger().error(f'Failed to connect to serial port {port} after {max_retries} attempts.')
raise serial.SerialException(f'Unable to open {port} after retries.')
except serial.SerialException as e:
self.get_logger().error(f'Final failure: {e}')
raise e
self.odom_msg_pub = self.create_publisher(Odometry, namespace + '/odom', 10)
self.imu_msg_pub = self.create_publisher(Imu, namespace + '/imu', 10)
self.cmd_vel_sub = self.create_subscription(Twist, namespace + '/cmd_vel', self.cmd_vel_callback, 10)
self.telemetry_msg_pub = self.create_publisher(String, namespace + '/telemetry', 10)
return rclpy.lifecycle.TransitionCallbackReturn.SUCCESS
def on_activate(self, state):
self.timer = self.create_timer(0.01, self.read_serial)
return rclpy.lifecycle.TransitionCallbackReturn.SUCCESS
def on_deactivate(self, state):
self.timer.cancel()
return rclpy.lifecycle.TransitionCallbackReturn.SUCCESS
def on_cleanup(self, state):
if self.ser:
self.ser.close()
return rclpy.lifecycle.TransitionCallbackReturn.SUCCESS
def read_serial(self):
try:
if self.ser:
line = self.ser.readline().decode('utf-8').strip()
else:
line = self.simulated_data.readline().strip()
# If we've reached the end of the file, rewind to the beginning
if not line:
self.get_logger().info('Reached end of simulated data file, rewinding')
time.sleep(3)
self.simulated_data.seek(0)
line = self.simulated_data.readline().strip()
if line:
self.dispatch_message(line)
self.get_logger().debug(f'Processed line: {line}')
except Exception as e:
self.get_logger().warn(f'Error reading serial: {e}')
def dispatch_message(self, line):
if line.startswith('ODOM:'):
self.handle_odom(line)
elif line.startswith('IMU:'):
self.handle_imu(line)
elif line.startswith('TELEMETRY:'):
self.handle_telemetry(line)
else:
self.get_logger().warn(f'Unknown message type: {line}')
def handle_telemetry(self, line):
try:
data = line[11:].split(',')
if len(data) < 2:
self.get_logger().warn(f"TELEMETRY message too short: {line}")
return
# Telemetry message is the timestamp and the hz
self.telemetry_msg_pub.publish(String(data[0]), String(data[1]))
except Exception as e:
self.get_logger().warn(f"Failed to parse TELEM message: {e}")
def handle_odom(self, line):
try:
# Increment counter and log raw data every 1000 iterations
self.odom_counter += 1
if self.odom_counter >= 100000:
self.get_logger().info(f"Raw ODOM data (iteration {self.odom_counter}): {line}")
self.odom_counter = 0
data = line[5:].split(',')
if len(data) < 9:
self.get_logger().warn(f"ODOM message too short: {line}")
return
# Parse all floats
(
pos_x, pos_y, theta,
vel_x, vel_y, measured_linear_x, measured_angular_z,
leftEncoderCount, rightEncoderCount
) = [float(x) for x in data]
odom_msg = Odometry()
odom_msg.header.stamp = self.get_clock().now().to_msg()
odom_msg.header.frame_id = 'odom'
odom_msg.child_frame_id = 'base_link'
odom_msg.pose.pose.position.x = pos_x
odom_msg.pose.pose.position.y = pos_y
odom_msg.pose.pose.position.z = 0.0
odom_msg.twist.twist.linear.x = vel_x
odom_msg.twist.twist.linear.y = vel_y
odom_msg.twist.twist.angular.z = measured_angular_z
quat = R.from_euler('z', theta).as_quat()
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
# Add covariance matrices - required for robot_localization
# Use moderate uncertainty values for pose
pose_cov = [0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.1, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
odom_msg.pose.covariance = pose_cov
# Use moderate uncertainty values for twist
twist_cov = [0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.1, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
odom_msg.twist.covariance = twist_cov
self.odom_msg_pub.publish(odom_msg)
except Exception as e:
self.get_logger().warn(f"Failed to parse ODOM message: {e}")
def handle_imu(self, line):
try:
# Increment counter and log raw data every 1000 iterations
self.imu_counter += 1
if self.imu_counter >= 100000:
self.get_logger().info(f"Raw IMU data (iteration {self.imu_counter}): {line}")
self.imu_counter = 0
data = line[4:].split(',')
if len(data) < 16:
self.get_logger().warn(f"IMU message too short: {line}")
return
# Parse all floats
(
millis,
qx, qy, qz, qw,
yaw, pitch, roll,
gx, gy, gz,
ax, ay, az,
cal_rot, cal_geomag
) = [float(x) for x in data]
imu_msg = Imu()
now = self.get_clock().now().to_msg()
imu_msg.header.stamp = now
imu_msg.header.frame_id = 'imu_link'
imu_msg.orientation.x = qx
imu_msg.orientation.y = qy
imu_msg.orientation.z = qz
imu_msg.orientation.w = qw
imu_msg.angular_velocity.x = gx
imu_msg.angular_velocity.y = gy
imu_msg.angular_velocity.z = gz
imu_msg.linear_acceleration.x = ax
imu_msg.linear_acceleration.y = ay
imu_msg.linear_acceleration.z = az
# Fill in covariance matrices - required for robot_localization
# Orientation covariance
orientation_cov = [0.01, 0.0, 0.0,
0.0, 0.01, 0.0,
0.0, 0.0, 0.01]
imu_msg.orientation_covariance = orientation_cov
# Angular velocity covariance
angular_vel_cov = [0.01, 0.0, 0.0,
0.0, 0.01, 0.0,
0.0, 0.0, 0.01]
imu_msg.angular_velocity_covariance = angular_vel_cov
# Linear acceleration covariance
linear_acc_cov = [0.01, 0.0, 0.0,
0.0, 0.01, 0.0,
0.0, 0.0, 0.01]
imu_msg.linear_acceleration_covariance = linear_acc_cov
self.imu_msg_pub.publish(imu_msg)
except Exception as e:
self.get_logger().error(f"Failed to parse IMU message: {e}")
def cmd_vel_callback(self, msg):
"""
Callback for cmd_vel subscription. Formats velocity commands and sends them to the serial port.
Args:
msg (Twist): The velocity command message
"""
try:
# Extract linear and angular velocities
linear_velocity = msg.linear.x
angular_velocity = msg.angular.z
# Format the command string
cmd_str = f"CMD:{linear_velocity},{angular_velocity}\n"
# Send the command to the serial port if available
if self.ser:
self.ser.write(cmd_str.encode('utf-8'))
self.get_logger().debug(f"Sent command: {cmd_str.strip()}")
else:
self.get_logger().debug(f"Would send command (simulation mode): {cmd_str.strip()}")
except Exception as e:
self.get_logger().error(f"Error sending cmd_vel command: {e}")
def main(args=None):
rclpy.init(args=args)
node = SerialBridgeNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment