(update)

1. mavlinkROS2Nodes.py 更新姿態發佈的 topic
2. 新增 interface AttitudeRaw 與對應的 package.xml & CMakeList

src/fc_interfaces/CMakeLists.txt

@@ -8,11 +8,14 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/AttitudeRaw.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
   "srv/MavPositionTargetGlobalInt.srv"
+  DEPENDENCIES std_msgs
   )
 
 ament_package()

src/fc_interfaces/msg/AttitudeRaw.msg

@@ -0,0 +1,9 @@
+uint32 seq
+builtin_interfaces/Time stamp
+string frame_id
+float32 roll
+float32 pitch
+float32 yaw
+float32 rollspeed
+float32 pitchspeed
+float32 yawspeed

src/fc_interfaces/package.xml

@@ -15,6 +15,9 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <build_depend>std_msgs</build_depend>
+  <exec_depend>std_msgs</exec_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -32,6 +32,7 @@ import mavros_msgs.msg
 
 # ROS2 Service imports
 import fc_interfaces.srv as fcsrv
+import fc_interfaces.msg as fcmsg
 from .ackEnum import serviceAckResult
 
 # 自定義 imports
@@ -231,7 +232,7 @@ class VehicleStatusPublisher(Node):
         publisher.publish(msg)
 
     def _publish_position_ned(self, sysid: int, status: mvv.ComponentStatus):
-        """發布 LOCAL_POSITION_NED（NED 座標，含位置與速度）"""
+        """發布 LOCAL_POSITION_NED (NED 座標，含位置與速度）"""
         if not self.rate_controller.should_publish(sysid, 'position_ned'):
             return
 
@@ -267,34 +268,28 @@ class VehicleStatusPublisher(Node):
         publisher.publish(msg)
     
     def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
-        """發布姿態（IMU）"""
+        """發布原始姿態（roll/pitch/yaw 與角速度）"""
         if not self.rate_controller.should_publish(sysid, 'attitude'):
             return
         
         att = status.attitude
-        if att.roll is None:
+        if att.roll is None or att.pitch is None or att.yaw is None:
             return
-        publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
+        publisher = self._get_or_create_publisher(
+            sysid, 'attitude', fcmsg.AttitudeRaw
+        )
         
         if publisher.get_subscription_count() == 0:
             return
         
-        msg = sensor_msgs.msg.Imu()
+        msg = fcmsg.AttitudeRaw()
-        
+        msg.stamp = self.get_clock().now().to_msg()
-        # 歐拉角轉四元數
+        msg.roll = float(att.roll)
-        qx, qy, qz, qw = self._euler_to_quaternion(
+        msg.pitch = float(att.pitch)
-            att.roll, att.pitch, att.yaw
+        msg.yaw = float(att.yaw)
-        )
+        msg.rollspeed = float(att.rollspeed) if att.rollspeed is not None else 0.0
-        msg.orientation.x = qx
+        msg.pitchspeed = float(att.pitchspeed) if att.pitchspeed is not None else 0.0
-        msg.orientation.y = qy
+        msg.yawspeed = float(att.yawspeed) if att.yawspeed is not None else 0.0
-        msg.orientation.z = qz
-        msg.orientation.w = qw
-        
-        # 角速度
-        if att.rollspeed is not None:
-            msg.angular_velocity.x = att.rollspeed
-            msg.angular_velocity.y = att.pitchspeed
-            msg.angular_velocity.z = att.yawspeed
         
         publisher.publish(msg)
     
@@ -1137,7 +1132,8 @@ ros2_manager = fc_ros_manager()
 2026.04.07
 1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
 2. 優化 response.ack_result 回傳值的資訊
-
+3. 新增 _publish_position_ned() 發布剛體座標的訊息
+4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息
 
 TODO 
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -38,6 +38,7 @@ class SerialMode(Enum):
     """連接類型"""
     STRAIGHT = auto()      # 原始數據直通
     XBEEAPI2AT = auto()    # XBee API 模式
+    XBEEAPI_POLL = auto()
     NOT_USE = auto()       # 不使用
 
 
@@ -176,10 +177,10 @@ class XBeeFrameProcessor(FrameProcessor):
         """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
         frame_type = frame[3]
 
-        if frame_type == self.FRAME_TYPE_RX_PACKET:
+        if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
             return self._decapsulate(frame)
 
-        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
+        if frame_type == self.FRAME_TYPE_AT_RESPONSE: # AT command
             if self.at_handler is not None:
                 self.at_handler.handle_frame(frame)
             return None
@@ -335,9 +336,10 @@ class ATCommandHandler:
     def _handle_rssi(self, data: bytes):
         """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
         pass
-        # if data:
+        if data:
-            # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
+            print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
             # logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
+            pass
         
     def _handle_serial_high(self, data: bytes):
         """處理 SH (Serial Number High)"""
@@ -370,6 +372,10 @@ class SerialHandler(asyncio.Protocol):
             at_handler = ATCommandHandler(self.serial_port_str)
             return XBeeFrameProcessor(at_handler=at_handler)
 
+        # if self.serial_mode == SerialMode.XBEEAPI_POLL:
+        #     at_handler = ATCommandHandler_new(self.serial_port_str)
+        #     return XBeeFrameProcessor(at_handler=at_handler)
+
         logger.warning(f"Unknown serial mode: {self.serial_mode}, using Raw")
         return RawFrameProcessor()
 

src/fc_network_adapter/package.xml

@@ -7,6 +7,13 @@
   <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
   <license>TODO: License declaration</license>
 
+  <exec_depend>fc_interfaces</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>mavros_msgs</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>