temp commit for share code
This commit is contained in:
@@ -16,5 +16,11 @@
|
||||
|
||||
===
|
||||
Package 簡述
|
||||
1. unitdev 為各自協作者做開發時的測試區
|
||||
2. fc_network_adapter
|
||||
建立、維護與飛控韌體的連接
|
||||
構築 mavlink 封包
|
||||
同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息
|
||||
處理無線模組的通訊格式 (XBee)
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,97 @@
|
||||
import threading
|
||||
from pymavlink import mavutil
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
|
||||
import time
|
||||
|
||||
'''
|
||||
|
||||
# 不同的匯流排只有單一種通訊協定
|
||||
# 匯流排接到訊息後透過 queue stack 傳送到分析器
|
||||
# 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器
|
||||
# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue
|
||||
# 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去
|
||||
|
||||
# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排
|
||||
# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面
|
||||
# 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function
|
||||
|
||||
'''
|
||||
|
||||
class MavlinkObject():
|
||||
def __init__(self, mavlink_socket):
|
||||
self.mavlink_socket = mavlink_socket
|
||||
self.msg_count = {}
|
||||
# self.multiplexingDict = {}
|
||||
self.multiplexingToAnalysis = []
|
||||
self.multiplexingToReturn = []
|
||||
self.multiplexingToConvert = []
|
||||
def __del__(self):
|
||||
self.mavlink_socket.close()
|
||||
self.stop()
|
||||
|
||||
def run(self):
|
||||
self.thread = threading.Thread(target=self._run_thread)
|
||||
self.running = True
|
||||
self.thread.start()
|
||||
def stop(self):
|
||||
self.running = False
|
||||
|
||||
def _run_thread(self):
|
||||
start_time = time.time()
|
||||
while self.running:
|
||||
try:
|
||||
mavlinkMsg = self.mavlink_socket.recv_msg()
|
||||
if mavlinkMsg:
|
||||
self.count_mavlink_type(mavlinkMsg)
|
||||
# print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug
|
||||
# break # Debug
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error receiving mavlink data: {e}")
|
||||
print(mavlinkMsg)
|
||||
self.running = False
|
||||
break
|
||||
|
||||
# 每秒中 統計一次 收到的訊息總量
|
||||
elapsed_time = time.time() - start_time
|
||||
if elapsed_time > 1:
|
||||
print('Messages Type received (in about 1 sec) :')
|
||||
print(self.msg_count)
|
||||
start_time = time.time()
|
||||
self.msg_count = {}
|
||||
|
||||
def count_mavlink_type(self, mavlinkMsg):
|
||||
msg_type = mavlinkMsg.get_type()
|
||||
if msg_type in self.msg_count:
|
||||
self.msg_count[msg_type] += 1
|
||||
else:
|
||||
self.msg_count[msg_type] = 1
|
||||
|
||||
def publish_mavlink_data(self, mavlinkMsg):
|
||||
msg = String()
|
||||
msg.data = str(mavlinkMsg)
|
||||
self.publisher_.publish(msg)
|
||||
|
||||
|
||||
|
||||
def main(args=None):
|
||||
|
||||
connection_string="udp:127.0.0.1:14550"
|
||||
mavlink_socket = mavutil.mavlink_connection(connection_string)
|
||||
|
||||
mavlink_object = MavlinkObject(mavlink_socket)
|
||||
mavlink_object.run()
|
||||
|
||||
time.sleep(5)
|
||||
mavlink_object.running = False
|
||||
mavlink_object.thread.join()
|
||||
print('End of Program')
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,14 @@
|
||||
|
||||
'''
|
||||
|
||||
透過某個地方 得到 udp 或 uart 接口
|
||||
對於每個接口 視為一個獨立的物件
|
||||
|
||||
物件對於不同的接口 是為不同類型的物件
|
||||
|
||||
每個類型的物件 創建一個獨立的執行緒 來處理資料
|
||||
關於執行緒的實作 是寫在另一個模組
|
||||
|
||||
物件之間 也可以做資料轉換/轉拋
|
||||
|
||||
'''
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>fc_network_adapter</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="chiyu1468@hotmail.com">picars</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/fc_network_adapter
|
||||
[install]
|
||||
install_scripts=$base/lib/fc_network_adapter
|
||||
@@ -0,0 +1,25 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'fc_network_adapter'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='picars',
|
||||
maintainer_email='chiyu1468@hotmail.com',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
],
|
||||
},
|
||||
)
|
||||
File diff suppressed because one or more lines are too long
@@ -42,6 +42,11 @@ def mavlink_analyzer_simple(count = 500):
|
||||
print('No message yet, 1 second for data to fill')
|
||||
sleep(1)
|
||||
|
||||
print("markA ------")
|
||||
print(msg.get_type())
|
||||
print(msg.ordered_fieldnames)
|
||||
print("markA ------ END")
|
||||
|
||||
print('Messages Type received:')
|
||||
print(msg_count)
|
||||
|
||||
@@ -108,7 +113,8 @@ def fdm_switch_example_one():
|
||||
elapsed_time = current_time - start_time
|
||||
|
||||
if elapsed_time >= 1.0:
|
||||
print(f"Packets received in the last second: {packet_count}")
|
||||
# print(f"Packets received in the last second: {packet_count}")
|
||||
print(f'JSON Pack : {data_g2s}')
|
||||
packet_count = 0
|
||||
start_time = current_time
|
||||
fdm_parser_example(data_s2g)
|
||||
@@ -132,24 +138,24 @@ def fdm_switch_example_two():
|
||||
server_address = ('127.0.0.1', 19002)
|
||||
|
||||
data_queue = queue.Queue()
|
||||
servo_out = [0] * 16
|
||||
servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....]
|
||||
data_queue.put(servo_out)
|
||||
|
||||
def send_data_from_queue(sock, server_address, q, frame_rate_fdm=10, frame_count_fdm=1):
|
||||
def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
|
||||
interval = 1.0 / frame_rate_fdm
|
||||
while True:
|
||||
start_time = time.time()
|
||||
try:
|
||||
data = q.get(timeout=0.1)
|
||||
data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
|
||||
if data is None:
|
||||
break
|
||||
last_data = data
|
||||
except queue.Empty:
|
||||
data = last_data
|
||||
data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
|
||||
sock.sendto(data, server_address)
|
||||
frame_count_fdm += 1
|
||||
fdm_parser_example(data) # debug
|
||||
data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
|
||||
sock.sendto(data_fdm, server_address)
|
||||
frame_count_fdm += 3
|
||||
# fdm_parser_example(data_fdm) # debug
|
||||
|
||||
elapsed_time = time.time() - start_time
|
||||
sleep_time = interval - elapsed_time
|
||||
@@ -159,6 +165,7 @@ def fdm_switch_example_two():
|
||||
Running = True
|
||||
thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
|
||||
thread.start()
|
||||
print('Start sending data to Gazebo')
|
||||
|
||||
while Running:
|
||||
msgb1 = None
|
||||
@@ -184,7 +191,39 @@ def fdm_switch_example_two():
|
||||
# To stop the thread, you can put None into the queue
|
||||
# data_queue.put(None)
|
||||
|
||||
from pymavlink.dialects.v20 import common # 使用 MAVLink 2.0
|
||||
|
||||
def mavlink_btye_generator_test():
|
||||
|
||||
# 創建一個 MAVLink 對象
|
||||
mav = common.MAVLink(None) # 不透過 connection，直接使用 None
|
||||
|
||||
# 創建一個 HEARTBEAT 訊息
|
||||
msg = mav.heartbeat_encode(
|
||||
type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
|
||||
autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
|
||||
base_mode=0,
|
||||
custom_mode=0,
|
||||
system_status=mavutil.mavlink.MAV_STATE_ACTIVE
|
||||
)
|
||||
|
||||
# 取得 MAVLink 訊息的 bytes
|
||||
mavlink_bytes = msg.pack(mav)
|
||||
|
||||
# 回傳 bytes
|
||||
print(mavlink_bytes)
|
||||
print(type(mavlink_bytes))
|
||||
|
||||
mav2 = common.MAVLink(None)
|
||||
# my_msg = mav2.decode(mavlink_bytes)
|
||||
print("========")
|
||||
print(dir(mav2))
|
||||
# print(my_msg)
|
||||
|
||||
|
||||
|
||||
print('Start')
|
||||
# fdm_switch_example_two()
|
||||
# fdm_parser_example()
|
||||
# fdm_parser_example()
|
||||
# mavlink_analyzer_simple(8)
|
||||
mavlink_btye_generator_test()
|
||||
|
||||
Reference in New Issue
Block a user