Merge branch 'ken' into chiyu

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -22,6 +22,27 @@ from pymavlink import mavutil
 # ROS2 的 import
 from rclpy.node import Node
 
+# 自定義的 import
+from mavlinkDevice import mavlink_device
+from mavlinkPublish import mavlink_publisher
+from theLogger import setup_logger
+
+
+# ====================== 分割線 =====================
+
+logger = setup_logger("mavlinkObject.py")
+
+# 基礎功能的 import
+import threading
+import queue
+import time
+
+# mavlink 的 import
+from pymavlink import mavutil
+
+# ROS2 的 import
+from rclpy.node import Node
+
 # 自定義的 import
 from mavlinkDevice import mavlink_device, mavlink_systems
 from mavlinkPublish import mavlink_publisher
@@ -131,8 +152,20 @@ class mavlink_bridge(Node, mavlink_publisher):
                 # print("get mode :", mavutil.mode_string_v10(msg)) # debug
                 # print("record mode :", this_component.emitParams['flightMode_mode']) # debug
 
-            elif msg.get_msgId() == 147: # BATTERY_STATUS 處理
-                this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()]
+            elif msg_id == 30:  # ATTITUDE 處理
+                this_component.emitParams['attitude'] = msg
+
+            elif msg_id == 32:  # LOCAL_POSITION_NED 處理
+                this_component.emitParams['local_position'] = msg
+
+            elif msg_id == 33:  # GLOBAL_POSITION_INT 處理
+                this_component.emitParams['global_position'] = msg
+
+            elif msg_id == 74:  # VFR_HUD 處理
+                this_component.emitParams['vfr_hud'] = msg
+
+            elif msg_id == 147:  # BATTERY_STATUS 處理
+                this_component.emitParams['battery'] = msg
 
             # ↑↑↑↑↑↑↑↑↑↑↑↑  處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↑↑↑↑↑↑↑↑↑↑↑↑
 
@@ -273,6 +306,7 @@ class mavlink_object():
         self.running = False
 
     def _run_thread(self):
+        logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id))
         logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id))
         start_time = time.time()
         while self.running:
@@ -282,6 +316,7 @@ class mavlink_object():
             try:
                 mavlinkMsg = self.mavlink_socket.recv_msg()
             except Exception as e:
+                logger.critical(f"Receiving data not mavlink format. Object Delete.")
                 logger.critical(f"Receiving data not mavlink format. Object Delete.")
                 print(f"[mavlinkObject.py] Error receiving mavlink data: {e}")
                 print(mavlinkMsg)

src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py

@@ -10,8 +10,13 @@ publisher topic name 命名規則為 <前綴詞>/s<sysid>/<具體 topic>
 '''
 
 import std_msgs.msg
+import sensor_msgs.msg
+import geometry_msgs.msg
+import mavros_msgs.msg
 
 from theLogger import setup_logger
+import math
+
 
 logger = setup_logger("mavlinkPublish.py")
 
@@ -46,7 +51,158 @@ class mavlink_publisher():
         pass
 
     # ↓↓↓↓↓↓↓↓↓↓↓↓  處理不同 ros2 topic 訊息 請放在這裡  ↓↓↓↓↓↓↓↓↓↓↓↓ 
+    def euler_to_quaternion(cls,roll, pitch, yaw):
+        qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
+        qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
+        qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
+        qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
+        return [qx, qy, qz, qw]
+    
+    def create_attitude(self, sysid, component_obj):
+        target_topic = 'attitude'
 
+        try:
+            _ = component_obj.emitParams['attitude']
+        except KeyError:
+            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
+            return False
+        
+        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
+        publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1)
+        component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude]
 
+    def packEmit_attitude(self, emitParams, publisher):
+        mav_msg = emitParams['attitude']
+        msg = sensor_msgs.msg.Imu()
+        x, y, z, w = self.euler_to_quaternion(mav_msg.roll, mav_msg.pitch, mav_msg.yaw)
+        msg.orientation.x = x
+        msg.orientation.y = y
+        msg.orientation.z = z
+        msg.orientation.w = w
+        msg.angular_velocity.x = mav_msg.rollspeed
+        msg.angular_velocity.y = mav_msg.pitchspeed
+        msg.angular_velocity.z = mav_msg.yawspeed
+        publisher.publish(msg)
+        pass
+    
+    def create_local_position_pose(self, sysid, component_obj):
+        target_topic = 'local_position/pose'
+        try:
+            _ = component_obj.emitParams['local_position']
+        except KeyError:
+            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
+            return False
+        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
+        publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1)
+        component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose]
 
+    def packEmit_local_pose(cls, emitParams, publisher):
+        mav_msg = emitParams['local_position']
+        msg = geometry_msgs.msg.Point()
+        msg.x = mav_msg.x
+        msg.y = mav_msg.y
+        msg.z = mav_msg.z
+        publisher.publish(msg)
+        pass
+
+    def create_local_position_velocity(self, sysid, component_obj):
+        target_topic = 'local_position/velocity'
+        try:
+            _ = component_obj.emitParams['local_position']
+        except KeyError:
+            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
+            return False
+        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
+        publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1)
+        component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel]
+
+    def packEmit_local_vel(cls, emitParams, publisher):
+        mav_msg = emitParams['local_position']
+        msg = geometry_msgs.msg.Vector3()
+        msg.x = mav_msg.vx
+        msg.y = mav_msg.vy
+        msg.z = mav_msg.vz
+        publisher.publish(msg)
+        pass
+
+    def create_global_global(self, sysid, component_obj):
+        target_topic = 'global_position/global'
+        try:
+            _ = component_obj.emitParams['global_position']
+        except KeyError:
+            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
+            return False
+        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
+        publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1)
+        component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global]
+
+    def packEmit_global_global(cls, emitParams, publisher):
+        mav_msg = emitParams['global_position']
+        msg = sensor_msgs.msg.NavSatFix()
+        msg.latitude = mav_msg.lat/1e7
+        msg.longitude = mav_msg.lon/1e7
+        msg.altitude = mav_msg.alt/1e3
+        publisher.publish(msg)
+        pass
+        
+    def create_global_rel(self, sysid, component_obj):
+        target_topic = 'global_position/rel_alt'
+        try:
+            _ = component_obj.emitParams['global_position']
+        except KeyError:
+            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
+            return False
+        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
+        publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1)
+        component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel]
+
+    def packEmit_global_rel(cls, emitParams, publisher):
+        mav_msg = emitParams['global_position']
+        msg = std_msgs.msg.Float64()
+        msg.data = float(mav_msg.relative_alt/1e3)
+        publisher.publish(msg)
+        pass
+
+    def create_vfr_hud(self, sysid, component_obj):
+        target_topic = 'vfr_hud'
+        try:
+            _ = component_obj.emitParams['vfr_hud']
+        except KeyError:
+            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
+            return False
+        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
+        publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1)
+        component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud]
+
+    def packEmit_vfr_hud(cls, emitParams, publisher):
+        mav_msg = emitParams['vfr_hud']
+        msg = mavros_msgs.msg.VfrHud()
+        msg.airspeed = mav_msg.airspeed
+        msg.groundspeed = mav_msg.groundspeed
+        msg.heading = mav_msg.heading
+        msg.throttle = float(mav_msg.throttle)
+        msg.altitude = mav_msg.alt
+        msg.climb = mav_msg.climb
+        publisher.publish(msg)
+        pass
+
+    def create_battery(self, sysid, component_obj):
+        target_topic = 'battery'
+        try:
+            _ = component_obj.emitParams['battery']
+        except KeyError:
+            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
+            return False
+        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
+        publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1)
+        component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery]
+
+    def packEmit_battery(cls, emitParams, publisher):
+        mav_msg = emitParams['battery']
+        msg = sensor_msgs.msg.BatteryState()
+        msg.voltage = mav_msg.voltages[0]/1e3
+        publisher.publish(msg)
+        pass
+
+        
     # ↑↑↑↑↑↑↑↑↑↑↑↑  處理不同 ros2 topic 訊息 請放在這裡  ↑↑↑↑↑↑↑↑↑↑↑↑