diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index b328f81..a0ce087 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -19,26 +19,43 @@ import mavros_msgs.msg from theLogger import setup_logger import math - logger = setup_logger("mavlinkPublish.py") class mavlink_publisher(): prefix_path = 'MavLinkBus' + TOPIC_CREATORS = ( + "create_state", + "create_attitude", + "create_local_position_pose", + "create_local_position_velocity", + "create_global_global", + "create_global_rel", + "create_vfr_hud", + "create_battery", + "create_ping" + ) + + def ensure_all_publishers(self, sysid: int, component): + """若還沒替這個 component 建立過 publisher，則一次補齊。""" + if getattr(component, "_pub_ready", False): + return # 已做過就跳過 + for fn in self.TOPIC_CREATORS: + getattr(self, fn)(sysid, component) + # 發布丟包率 + if not hasattr(component, "loss_rate_publisher") or component.loss_rate_publisher is None: + target_topic = 'packet_loss_rate' + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + component.loss_rate_publisher = self.create_publisher(std_msgs.msg.Float64, topic_name, 10) + component.loss_rate_topic_name = topic_name + + component._pub_ready = True # 打個旗標以免重複 + logger.info("✔︎ sysid=%d compid=%d → 所有 topic 已註冊", sysid, component.compid) + def create_state(self, sysid, component_obj): # target topic name # 請跟這個 method 的名稱保持一致 target_topic = 'state' - - # 這邊要檢查 flight_mode 是否存在 - try: - _ = component_obj.emitParams['heartbeat'] - except KeyError: - # 這個 component id 還不存在 - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - - # 若存在則 建立 publisher object 並回傳 true topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(mavros_msgs.msg.State, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_state] @@ -46,6 +63,8 @@ class mavlink_publisher(): return True def packEmit_state(cls, emitParams, publisher): + if 'heartbeat' not in emitParams: # ← 沒資料就直接返回 + return mav_msg = emitParams['heartbeat'] msg = mavros_msgs.msg.State() msg.mode = mavutil.mode_string_v10(mav_msg) @@ -63,18 +82,13 @@ class mavlink_publisher(): 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): + if 'attitude' not in emitParams: # ← 沒資料就直接返回 + return 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) @@ -90,16 +104,13 @@ class mavlink_publisher(): 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): + if 'local_position' not in emitParams: + return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Point() msg.x = mav_msg.x @@ -110,16 +121,13 @@ class mavlink_publisher(): 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): + if 'local_position' not in emitParams: + return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Vector3() msg.x = mav_msg.vx @@ -130,16 +138,13 @@ class mavlink_publisher(): 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): + if 'global_position' not in emitParams: + return mav_msg = emitParams['global_position'] msg = sensor_msgs.msg.NavSatFix() msg.latitude = mav_msg.lat/1e7 @@ -150,16 +155,13 @@ class mavlink_publisher(): 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): + if 'global_position' not in emitParams: + return mav_msg = emitParams['global_position'] msg = std_msgs.msg.Float64() msg.data = float(mav_msg.relative_alt/1e3) @@ -168,16 +170,13 @@ class mavlink_publisher(): 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): + if 'vfr_hud' not in emitParams: + return mav_msg = emitParams['vfr_hud'] msg = mavros_msgs.msg.VfrHud() msg.airspeed = mav_msg.airspeed @@ -191,21 +190,30 @@ class mavlink_publisher(): 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): + if 'battery' not in emitParams: + return mav_msg = emitParams['battery'] msg = sensor_msgs.msg.BatteryState() msg.voltage = mav_msg.voltages[0]/1e3 publisher.publish(msg) pass + + def create_ping(self, sysid, component_obj): + target_topic = 'ping' + 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_ping] + def packEmit_ping(cls, emitParams, publisher): + if 'ping' not in emitParams: + return + mav_msg = emitParams['ping'] + publisher.publish(mav_msg) + pass # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file