Initial commit

2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
@@ -0,0 +1,126 @@
+#!/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