chiyu1468
/
AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Upload files to 'src/fc_network_adapter/fc_network_adapter'

Browse Source
chiyu
ken910606 10 months ago
parent 6bbf2c450f
commit 55b77f5920
1 changed files with 56 additions and 48 deletions
Show Stats Download Patch File Download Diff File

104
src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py
Unescape Escape View File

@ -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 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑
Write Preview
Loading…
Cancel
Save