# RTK function update
--- 1. mavlinkROS2Node 新增 RtcmRelay Node 監聽 RTCM topic 並轉發到 mavlinkObject bus (GPS_RTCM_DATA) 2. 新增 fc_network_module ros2 package 作為協同附屬模組的放置空間 3. Create a new fc_network_module with NtripClientNode for connecting to NTRIP casters and publishing RTCM data to ROS2 topics. 4. (modify) mainOrchestrator adjust the publishing rates for GNSS topics
This commit is contained in:
@@ -0,0 +1,146 @@
|
||||
import socket
|
||||
import base64
|
||||
import threading
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
|
||||
from mavros_msgs.msg import RTCM
|
||||
|
||||
|
||||
class NtripClientNode(Node):
|
||||
"""連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。"""
|
||||
|
||||
RECONNECT_BASE_SEC = 2.0
|
||||
RECONNECT_MAX_SEC = 60.0
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('ntrip_client')
|
||||
|
||||
self.declare_parameter('host', 'rtk2go.com')
|
||||
self.declare_parameter('port', 2101)
|
||||
self.declare_parameter('mountpoint', '')
|
||||
self.declare_parameter('username', '')
|
||||
self.declare_parameter('password', '')
|
||||
|
||||
rtcm_qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
durability=DurabilityPolicy.VOLATILE,
|
||||
)
|
||||
self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
|
||||
|
||||
self._running = True
|
||||
self._thread = threading.Thread(target=self._receive_loop, daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
self.get_logger().info('NtripClientNode 已啟動')
|
||||
|
||||
# ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
|
||||
|
||||
def _receive_loop(self):
|
||||
backoff = self.RECONNECT_BASE_SEC
|
||||
|
||||
while self._running:
|
||||
host = self.get_parameter('host').value
|
||||
port = self.get_parameter('port').value
|
||||
mount = self.get_parameter('mountpoint').value
|
||||
user = self.get_parameter('username').value
|
||||
pwd = self.get_parameter('password').value
|
||||
|
||||
if not mount:
|
||||
self.get_logger().error('mountpoint 參數未設定，等待重試...')
|
||||
time.sleep(backoff)
|
||||
continue
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
sock.settimeout(10)
|
||||
|
||||
try:
|
||||
self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...')
|
||||
sock.connect((host, port))
|
||||
|
||||
auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
|
||||
request = (
|
||||
f'GET /{mount} HTTP/1.0\r\n'
|
||||
f'User-Agent: NTRIP PythonClient\r\n'
|
||||
f'Authorization: Basic {auth}\r\n'
|
||||
f'Connection: close\r\n\r\n'
|
||||
)
|
||||
sock.sendall(request.encode())
|
||||
|
||||
resp = sock.recv(4096)
|
||||
if b'ICY 200 OK' not in resp:
|
||||
self.get_logger().error(
|
||||
f'NTRIP 握手失敗: {resp.decode(errors="ignore").strip()}'
|
||||
)
|
||||
raise ConnectionError('handshake failed')
|
||||
|
||||
self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流')
|
||||
backoff = self.RECONNECT_BASE_SEC
|
||||
|
||||
self._read_stream(sock)
|
||||
|
||||
except Exception as e:
|
||||
if self._running:
|
||||
self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
|
||||
time.sleep(backoff)
|
||||
backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
|
||||
finally:
|
||||
sock.close()
|
||||
|
||||
def _read_stream(self, sock: socket.socket):
|
||||
buf = b''
|
||||
|
||||
while self._running:
|
||||
try:
|
||||
chunk = sock.recv(4096)
|
||||
except socket.timeout:
|
||||
continue
|
||||
if not chunk:
|
||||
break
|
||||
buf += chunk
|
||||
|
||||
while len(buf) >= 6:
|
||||
if buf[0] != 0xD3:
|
||||
buf = buf[1:]
|
||||
continue
|
||||
|
||||
payload_len = ((buf[1] & 0x03) << 8) | buf[2]
|
||||
frame_len = payload_len + 6 # header(3) + payload + CRC(3)
|
||||
|
||||
if len(buf) < frame_len:
|
||||
break
|
||||
|
||||
packet = buf[:frame_len]
|
||||
buf = buf[frame_len:]
|
||||
|
||||
self._publish_rtcm(packet)
|
||||
|
||||
def _publish_rtcm(self, raw_packet: bytes):
|
||||
msg = RTCM()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.data = list(raw_packet)
|
||||
self.publisher_.publish(msg)
|
||||
|
||||
def destroy_node(self):
|
||||
self._running = False
|
||||
self._thread.join(timeout=3.0)
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = NtripClientNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,22 @@
|
||||
<?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_module</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="chiyu1468@hotmail.com">picars</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>mavros_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<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_module
|
||||
[install]
|
||||
install_scripts=$base/lib/fc_network_module
|
||||
@@ -0,0 +1,26 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'fc_network_module'
|
||||
|
||||
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': [
|
||||
'ntrip_client = fc_network_module.ntrip_client:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,25 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
@@ -0,0 +1,25 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
@@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
Reference in New Issue
Block a user