127 lines
4.8 KiB
Python
127 lines
4.8 KiB
Python
#!/usr/bin/env python3
|
|
import rospy
|
|
import paho.mqtt.client as mqtt
|
|
from std_msgs.msg import String
|
|
import pkg_resources
|
|
import json
|
|
|
|
# 获取 paho-mqtt 版本
|
|
try:
|
|
PAHO_VERSION = pkg_resources.get_distribution("paho-mqtt").version
|
|
PAHO_MAJOR_VERSION = int(PAHO_VERSION.split('.')[0])
|
|
except Exception:
|
|
PAHO_VERSION = "unknown"
|
|
PAHO_MAJOR_VERSION = 1
|
|
|
|
# MQTT 配置
|
|
MQTT_BROKER = "211.154.252.186" # MQTT 代理地址
|
|
MQTT_PORT = 1883 # MQTT 代理端口
|
|
MQTT_CLIENT_ID = "ros_mqtt_bridge"
|
|
MQTT_TOPIC_CONTROL = "head_servo/control/1" # 接收控制指令的 MQTT 主题
|
|
# MQTT_TOPIC_CONTROL = "robot/yuntai_control/+"
|
|
MQTT_TOPIC_STATUS = "head_servo/status" # 发布状态的 MQTT 主题
|
|
|
|
# ROS 配置
|
|
ROS_TOPIC_CONTROL = "/head_servo/control_cmd" # 发布控制指令的 ROS 话题
|
|
ROS_TOPIC_STATUS = "/head_servo/status" # 接收状态的 ROS 话题
|
|
|
|
class MQTTROSBridge:
|
|
def __init__(self):
|
|
# 初始化 ROS 节点
|
|
rospy.init_node("mqtt_ros_bridge", anonymous=True)
|
|
rospy.loginfo(f"MQTT-ROS 桥接器启动中 (paho-mqtt 版本: {PAHO_VERSION})")
|
|
|
|
# 根据版本选择初始化方式
|
|
if PAHO_MAJOR_VERSION >= 2:
|
|
# 版本 2.0+ 使用新 API
|
|
self.mqtt_client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2, clean_session=True)
|
|
else:
|
|
# 版本 1.x 使用旧 API
|
|
self.mqtt_client = mqtt.Client(client_id=MQTT_CLIENT_ID)
|
|
|
|
self.mqtt_client.on_connect = self.on_connect
|
|
self.mqtt_client.on_message = self.on_message
|
|
|
|
# 连接 MQTT 代理
|
|
try:
|
|
self.mqtt_client.connect(MQTT_BROKER, MQTT_PORT, 60)
|
|
self.mqtt_client.loop_start() # 启动 MQTT 网络循环（非阻塞）
|
|
rospy.loginfo(f"已连接到 MQTT 代理: {MQTT_BROKER}:{MQTT_PORT}")
|
|
except Exception as e:
|
|
rospy.logerr(f"MQTT 连接失败: {e}")
|
|
exit(1)
|
|
|
|
# 创建 ROS 发布者和订阅者
|
|
self.control_pub = rospy.Publisher(ROS_TOPIC_CONTROL, String, queue_size=10)
|
|
rospy.Subscriber(ROS_TOPIC_STATUS, String, self.status_callback)
|
|
|
|
rospy.loginfo("MQTT-ROS 桥接器已就绪")
|
|
|
|
def on_connect(self, client, userdata, flags, rc, properties):
|
|
"""MQTT 连接成功回调"""
|
|
rospy.loginfo(f"MQTT 连接成功，返回代码: {rc}")
|
|
# 订阅 MQTT 控制主题
|
|
client.subscribe(MQTT_TOPIC_CONTROL)
|
|
rospy.loginfo(f"已订阅 MQTT 主题: {MQTT_TOPIC_CONTROL}")
|
|
|
|
# def on_message(self, client, userdata, msg):
|
|
# """MQTT 消息到达回调"""
|
|
# try:
|
|
# # 解析 MQTT 消息
|
|
# payload = msg.payload.decode()
|
|
# rospy.loginfo(f"收到 MQTT 消息: [{msg.topic}] {payload}")
|
|
# # 将消息发布到 ROS 话题
|
|
# ros_msg = String()
|
|
# ros_msg.data = payload
|
|
# self.control_pub.publish(ros_msg)
|
|
|
|
# except Exception as e:
|
|
# rospy.logerr(f"处理 MQTT 消息时出错: {e}")
|
|
|
|
# 修改 MQTT-ROS 桥接脚本中的消息处理部分
|
|
def on_message(self, client, userdata, msg):
|
|
try:
|
|
payload = msg.payload.decode()
|
|
rospy.loginfo(f"收到 MQTT 消息: [{msg.topic}] {payload}")
|
|
|
|
# 解析 JSON 命令
|
|
try:
|
|
cmd_data = json.loads(payload)
|
|
ros_msg = String()
|
|
ros_msg.data = json.dumps(cmd_data) # 保持 JSON 格式
|
|
self.control_pub.publish(ros_msg)
|
|
except json.JSONDecodeError:
|
|
# 如果不是 JSON 格式，作为普通字符串处理
|
|
ros_msg = String()
|
|
ros_msg.data = payload
|
|
self.control_pub.publish(ros_msg)
|
|
|
|
except Exception as e:
|
|
rospy.logerr(f"处理 MQTT 消息时出错: {e}")
|
|
|
|
|
|
|
|
def status_callback(self, msg):
|
|
"""ROS 状态消息回调"""
|
|
try:
|
|
# 将 ROS 状态消息发布到 MQTT 主题
|
|
self.mqtt_client.publish(MQTT_TOPIC_STATUS, msg.data)
|
|
rospy.loginfo(f"已发布到 MQTT 主题 [{MQTT_TOPIC_STATUS}]: {msg.data}")
|
|
except Exception as e:
|
|
rospy.logerr(f"发布到 MQTT 时出错: {e}")
|
|
|
|
def run(self):
|
|
"""运行桥接器"""
|
|
rospy.spin() # 进入 ROS 循环
|
|
# 关闭 MQTT 连接
|
|
self.mqtt_client.loop_stop()
|
|
self.mqtt_client.disconnect()
|
|
rospy.loginfo("桥接器已关闭")
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
bridge = MQTTROSBridge()
|
|
bridge.run()
|
|
except rospy.ROSInterruptException:
|
|
pass
|