houyongdi/dreamDeckAutopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/e_stop_relay/CMakeLists.txt
+68
View File
@@ -0,0 +1,68 @@
cmake_minimum_required(VERSION 3.0.2)
project(e_stop_relay)
# 查找必要的catkin组件
find_package(catkin REQUIRED COMPONENTS
roscpp
serial
std_msgs
message_generation
)
# 声明消息文件
add_message_files(
FILES
)
# 声明服务文件
add_service_files(
FILES
EStopService.srv
)
# 生成消息文件
generate_messages(
DEPENDENCIES
std_msgs
)
# 配置catkin包
catkin_package(
INCLUDE_DIRS include
LIBRARIES e_stop_relay
CATKIN_DEPENDS roscpp serial std_msgs message_runtime
)
# 包含头文件路径
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# 编译可执行文件
add_executable(e_stop_relay_node
src/e_stop_relay_node.cpp
)
# 添加消息生成依赖
add_dependencies(e_stop_relay_node
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
# 链接依赖库
target_link_libraries(e_stop_relay_node
${catkin_LIBRARIES}
)
# 安装可执行文件
install(TARGETS e_stop_relay_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/e_stop_relay/include/e_stop_relay.h
+47
View File
@@ -0,0 +1,47 @@
#ifndef E_STOP_RELAY_H
#define E_STOP_RELAY_H
#include <ros/ros.h>
#include <serial/serial.h>
#include "e_stop_relay/EStopService.h"
class EStopRelay {
public:
EStopRelay();
void e_stop_readSerialData();
void relay_readSerialData();
void e_stop_parseModbusFrame(const std::vector<uint8_t> &frame);
void relay_parseModbusFrame(const std::vector<uint8_t> &frame);
void run();
bool handleEStopService(e_stop_relay::EStopService::Request &req, e_stop_relay::EStopService::Response &res);
bool setLightStatus(bool status);
private:
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
serial::Serial ser_;
ros::Publisher e_stop_pub_;
ros::ServiceServer e_stop_service_;
std::string port_name_;
int baudrate_;
std::string topic_name_;
std::vector<uint8_t> serial_buffer_;
ros::Time lastReceivedTime_;
std::string output_e_stop_topic_;
bool relay_status;
// 新增：重连控制参数（保留原有注释结构）
ros::Duration reconnect_interval_; // 重连间隔时间（秒）
ros::Time last_reconnect_attempt_; // 上次重连尝试的时间戳
// Modbus指令定义（原有注释保留）
const unsigned char e_stop_pressed_data[7] = {0x01, 0x04, 0x02 ,0x00, 0x01, 0x78, 0xF0};
const unsigned char e_stop_released_data[7] = {0x01 ,0x04 ,0x02 ,0x00 ,0x00 ,0xB9 ,0x30};
const unsigned char relay_turnon_cmd[13] = {0x01 ,0x10 ,0x00 ,0x01 ,0x00 ,0x02 ,0x04 ,0x00 ,0x01 ,0x00 ,0x01 ,0xA2 ,0x63};
const unsigned char relay_turnoff_cmd[13] = {0x01 ,0x10 ,0x00 ,0x01 ,0x00 ,0x02 ,0x04 ,0x00 ,0x00 ,0x00 ,0x00 ,0x32 ,0x63};
// 串口初始化函数（原有声明保留）
bool initSerial();
};
#endif // E_STOP_RELAY_H
sensors/e_stop_relay/launch/e_stop_relay.launch
+11
View File
@@ -0,0 +1,11 @@
<launch>
<arg name="e_stop_relay_com_" default="/dev/ttyS0"/>
<arg name="baudrate_" default="115200"/>
<arg name="output_e_stop_topic_" default="/e_stop_states"/>
<node name="e_stop_relay_node" pkg="e_stop_relay" type="e_stop_relay_node" output="screen">
<param name="e_stop_relay_com_" value="$(arg e_stop_relay_com_)" type="string"/>
<param name="baudrate_" value="$(arg baudrate_)" type="int"/>
<param name="output_e_stop_topic_" value="$(arg output_e_stop_topic_)" type="string" />
</node>
</launch>
sensors/e_stop_relay/msg/wheel_motor_states.msg
+2
View File
@@ -0,0 +1,2 @@
# 急停按钮状态（0: Released，1: Pressed）
uint8 emergencyStopButton_status
sensors/e_stop_relay/package.xml
+64
View File
@@ -0,0 +1,64 @@
<?xml version="1.0"?>
<package format="2">
<name>e_stop_relay</name>
<version>0.0.0</version>
<description>The e_stop_relay 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/e_stop_relay</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_export_depend>roscpp</build_export_depend>
<build_export_depend>serial</build_export_depend>
<build_export_depend>std_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/e_stop_relay/src/e_stop_relay_node.cpp
+271
View File
@@ -0,0 +1,271 @@
#include "e_stop_relay.h"
#include <std_msgs/Bool.h>
#include <ros/console.h>
#include <boost/algorithm/string.hpp>
// 构造函数
EStopRelay::EStopRelay() : private_nh_("~"), lastReceivedTime_(ros::Time::now())
{
// 从参数服务器获取配置参数
private_nh_.param<std::string>("e_stop_relay_com_", port_name_, "/dev/usb485_1");
private_nh_.param<int>("baudrate_", baudrate_, 115200);
private_nh_.param<std::string>("output_e_stop_topic_", output_e_stop_topic_, "/e_stop_states");
// 创建急停状态发布者
e_stop_pub_ = nh_.advertise<std_msgs::Bool>(output_e_stop_topic_, 10);
relay_status = false;
// 初始化重连参数（5秒重连一次）
reconnect_interval_ = ros::Duration(5.0);
last_reconnect_attempt_ = ros::Time::now() - reconnect_interval_; // 允许立即首次尝试
// 初始化串口（失败不退出，后续重连）
if (!initSerial()) {
ROS_ERROR("Failed to initialize serial port on startup! Will attempt reconnection...");
} else {
ROS_INFO_STREAM("Serial port initialized: " << port_name_ << " @ " << baudrate_);
}
// 创建急停服务服务器
e_stop_service_ = nh_.advertiseService("e_stop_service", &EStopRelay::handleEStopService, this);
ROS_INFO("EStop service is ready.");
}
// 串口初始化（保留原有注释，新增异常处理）
bool EStopRelay::initSerial()
{
try {
// 先关闭已打开的串口（避免重复占用）
if (ser_.isOpen()) {
ser_.close();
}
// 配置串口参数
ser_.setPort(port_name_);
ser_.setBaudrate(baudrate_);
serial::Timeout to = serial::Timeout::simpleTimeout(1000); // 超时时间1秒
ser_.setTimeout(to);
ser_.open();
} catch (serial::IOException &e) {
ROS_ERROR_STREAM("Unable to open serial port: " << e.what());
return false;
} catch (std::exception &e) {
ROS_ERROR_STREAM("Serial port error: " << e.what());
return false;
}
return ser_.isOpen();
}
// 读取急停串口数据（保留原有注释，新增重连逻辑）
void EStopRelay::e_stop_readSerialData()
{
// 新增：检查串口状态，未打开则尝试重连
if (!ser_.isOpen()) {
ros::Time now = ros::Time::now();
if (now - last_reconnect_attempt_ > reconnect_interval_) {
ROS_INFO_STREAM("Attempting to reconnect to " << port_name_ << "...");
if (initSerial()) {
ROS_INFO_STREAM("Successfully reconnected to " << port_name_);
serial_buffer_.clear(); // 清空缓冲区，避免旧数据干扰
} else {
ROS_ERROR_STREAM("Reconnection to " << port_name_ << " failed. Retrying...");
}
last_reconnect_attempt_ = now;
}
return; // 串口未就绪，退出本次读取
}
// 读取
try {
const size_t available = ser_.available();
if (available > 0) {
std::vector<uint8_t> buffer(available);
ser_.read(buffer.data(), available);
serial_buffer_.insert(serial_buffer_.end(), buffer.begin(), buffer.end());
lastReceivedTime_ = ros::Time::now(); // 更新最后接收时间
}
} catch (serial::IOException &e) {
// 新增：捕获读取异常（物理断开时触发）
ROS_ERROR_STREAM("Serial read error (e-stop): " << e.what() << " - forcing reconnection");
ser_.close(); // 标记为关闭，触发重连
return;
}
// 解析Modbus帧
if (serial_buffer_.size() == 7) {
std::vector<uint8_t> frame(serial_buffer_.begin(), serial_buffer_.begin() + 7);
e_stop_parseModbusFrame(frame);
serial_buffer_.erase(serial_buffer_.begin(), serial_buffer_.begin() + 7);
} else {
serial_buffer_.clear(); // 帧不完整时清空，避免堆积
}
}
// 读取继电器串口数据（保留原有注释，新增重连逻辑）
void EStopRelay::relay_readSerialData()
{
// 检查串口状态，未打开则尝试重连
if (!ser_.isOpen()) {
ros::Time now = ros::Time::now();
if (now - last_reconnect_attempt_ > reconnect_interval_) {
ROS_INFO_STREAM("Attempting to reconnect to " << port_name_ << "...");
if (initSerial()) {
ROS_INFO_STREAM("Successfully reconnected to " << port_name_);
serial_buffer_.clear(); // 清空缓冲区
} else {
ROS_ERROR_STREAM("Reconnection to " << port_name_ << " failed. Retrying...");
}
last_reconnect_attempt_ = now;
}
return; // 串口未就绪，退出本次读取
}
// 读取
try {
const size_t available = ser_.available();
if (available > 0) {
std::vector<uint8_t> buffer(available);
ser_.read(buffer.data(), available);
serial_buffer_.insert(serial_buffer_.end(), buffer.begin(), buffer.end());
lastReceivedTime_ = ros::Time::now(); // 更新最后接收时间
}
} catch (serial::IOException &e) {
// 捕获读取异常（物理断开时触发）
ROS_ERROR_STREAM("Serial read error (relay): " << e.what() << " - forcing reconnection");
ser_.close(); // 标记为关闭，触发重连
return;
}
// 解析Modbus帧
if (serial_buffer_.size() == 13) {
std::vector<uint8_t> frame(serial_buffer_.begin(), serial_buffer_.begin() + 13);
relay_parseModbusFrame(frame);
serial_buffer_.erase(serial_buffer_.begin(), serial_buffer_.begin() + 13);
} else {
serial_buffer_.clear(); // 帧不完整时清空
}
}
// 解析急停Modbus帧
void EStopRelay::e_stop_parseModbusFrame(const std::vector<uint8_t> &frame)
{
std_msgs::Bool e_stop_msg;
// 匹配急停按下指令
if (frame[0] == e_stop_pressed_data[0] && frame[1] == e_stop_pressed_data[1] && frame[2] == e_stop_pressed_data[2]) {
static uint8_t last_stat = 3; // 初始状态无效值
if (frame[4] == e_stop_pressed_data[4]) {
if (last_stat != 1) { // 状态变化时打印日志
ROS_INFO("Emergency stop button is pressed");
}
e_stop_msg.data = true;
e_stop_pub_.publish(e_stop_msg);
last_stat = 1;
} else if (frame[4] == e_stop_released_data[4]) {
if (last_stat != 0) { // 状态变化时打印日志
ROS_INFO("Emergency stop button is released");
}
e_stop_msg.data = false;
e_stop_pub_.publish(e_stop_msg);
last_stat = 0;
}
}
}
// 解析继电器Modbus帧
void EStopRelay::relay_parseModbusFrame(const std::vector<uint8_t> &frame)
{
// 匹配继电器指令帧头
if (frame[0] == 0x01 && frame[1] == 0x10) {
// 继电器打开指令响应
if (frame[8] == 0x01 && frame[10] == 0x01 && frame[11] == 0xA2 && frame[12] == 0x63) {
relay_status = true;
ROS_INFO("Relay turned on");
}
// 继电器关闭指令响应
else if (frame[8] == 0x00 && frame[10] == 0x00 && frame[11] == 0x32 && frame[12] == 0x63) {
relay_status = false;
ROS_INFO("Relay turned off");
}
}
}
// 主循环（超时检测）
void EStopRelay::run()
{
ros::Rate rate(10); // 10Hz循环频率
ros::Duration timeout(5.0); // 新增：5秒无数据判定为超时
while (ros::ok()) {
e_stop_readSerialData();
relay_readSerialData();
// 超时检测（长时间无数据强制重连）
if (ser_.isOpen() && (ros::Time::now() - lastReceivedTime_ > timeout)) {
ROS_WARN("No data received for %fs, forcing reconnection...", timeout.toSec());
ser_.close(); // 关闭串口触发重连
}
ros::spinOnce();
rate.sleep();
}
// 退出时关闭串口
if (ser_.isOpen()) {
ser_.close();
ROS_INFO("Serial port closed on exit");
}
}
// 急停服务处理
bool EStopRelay::handleEStopService(e_stop_relay::EStopService::Request &req, e_stop_relay::EStopService::Response &res)
{
std::string command = req.request_message;
boost::to_lower(command); // 指令转为小写，兼容大小写输入
if (command == "turn on") {
res.success = setLightStatus(true);
res.response_message = res.success ? "Relay turned on" : "Failed to turn on relay";
} else if (command == "turn off") {
res.success = setLightStatus(false);
res.response_message = res.success ? "Relay turned off" : "Failed to turn off relay";
} else {
res.success = false;
res.response_message = "Unknown command: " + req.request_message;
ROS_WARN_STREAM("Received unknown command: " << req.request_message);
}
return true; // 服务处理成功
}
// 控制灯光状态（异常处理）
bool EStopRelay::setLightStatus(bool status)
{
if (!ser_.isOpen()) {
ROS_ERROR("Cannot send command - serial port is not open");
return false;
}
try {
size_t written;
if (status) {
written = ser_.write(relay_turnon_cmd, 13); // 发送打开指令
} else {
written = ser_.write(relay_turnoff_cmd, 13); // 发送关闭指令
}
return written == 13; // 确认指令完整发送
} catch (serial::IOException &e) {
// 新增：发送异常时触发重连
ROS_ERROR_STREAM("Failed to write to serial port: " << e.what() << " - forcing reconnection");
ser_.close();
return false;
}
}
// 主函数
int main(int argc, char **argv)
{
ros::init(argc, argv, "e_stop_relay_node");
EStopRelay node;
node.run();
return 0;
}
sensors/e_stop_relay/srv/EStopService.srv
+6
View File
@@ -0,0 +1,6 @@
# 请求部分
string request_message
---
# 响应部分
bool success
string response_message