feat: 已添加win_ubuntu_bridge
This commit is contained in:
li-shihao-code
@@ -0,0 +1,56 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# NO CHECKED-IN PROTOBUF GENCODE
|
||||
# source: agv_calib_control.proto
|
||||
# Protobuf Python Version: 6.31.1
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import runtime_version as _runtime_version
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
from google.protobuf.internal import builder as _builder
|
||||
_runtime_version.ValidateProtobufRuntimeVersion(
|
||||
_runtime_version.Domain.PUBLIC,
|
||||
6,
|
||||
31,
|
||||
1,
|
||||
'',
|
||||
'agv_calib_control.proto'
|
||||
)
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_sym_db = _symbol_database.Default()
|
||||
|
||||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x17\x61gv_calib_control.proto\x12\x17\x61gv.calibration.control\"\x07\n\x05\x45mpty\"4\n\x10StandardResponse\x12\x0f\n\x07success\x18\x01 \x01(\x08\x12\x0f\n\x07message\x18\x02 \x01(\t\"\x8b\x01\n\x0bModeRequest\x12>\n\x0btarget_mode\x18\x01 \x01(\x0e\x32).agv.calibration.control.ModeRequest.Mode\"<\n\x04Mode\x12\x0f\n\x0bNORMAL_MODE\x10\x00\x12\x12\n\x0eOPEN_LOOP_MODE\x10\x01\x12\x0f\n\x0bTUNING_MODE\x10\x02\"p\n\x0fOpenLoopRequest\x12\x16\n\x0eleft_motor_cmd\x18\x01 \x01(\x01\x12\x17\n\x0fright_motor_cmd\x18\x02 \x01(\x01\x12\x16\n\x0esteering_angle\x18\x03 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x04 \x01(\x01\"h\n\x0fTrajectoryPoint\x12\x0b\n\x03x_m\x18\x01 \x01(\x01\x12\x0b\n\x03y_m\x18\x02 \x01(\x01\x12\x0f\n\x07yaw_rad\x18\x03 \x01(\x01\x12\x17\n\x0ftarget_speed_ms\x18\x04 \x01(\x01\x12\x11\n\tcurvature\x18\x05 \x01(\x01\"a\n\x11TrajectoryRequest\x12\x14\n\x0ctest_case_id\x18\x01 \x01(\t\x12\x36\n\x04path\x18\x02 \x03(\x0b\x32(.agv.calibration.control.TrajectoryPoint\"G\n\x13StepResponseRequest\x12\x1a\n\x12target_velocity_ms\x18\x01 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x02 \x01(\x01\"\xf9\x05\n\rControlParams\x12$\n\x17wheel_radius_left_ratio\x18\x01 \x01(\x01H\x00\x88\x01\x01\x12%\n\x18wheel_radius_right_ratio\x18\x02 \x01(\x01H\x01\x88\x01\x01\x12$\n\x17\x65\x66\x66\x65\x63tive_track_width_m\x18\x03 \x01(\x01H\x02\x88\x01\x01\x12%\n\x18steering_zero_offset_deg\x18\x04 \x01(\x01H\x03\x88\x01\x01\x12\x1b\n\x0epid_kp_lateral\x18\x05 \x01(\x01H\x04\x88\x01\x01\x12\x1b\n\x0epid_ki_lateral\x18\x06 \x01(\x01H\x05\x88\x01\x01\x12\x1b\n\x0epid_kd_lateral\x18\x07 \x01(\x01H\x06\x88\x01\x01\x12\x1b\n\x0epid_kp_heading\x18\x08 \x01(\x01H\x07\x88\x01\x01\x12\x1b\n\x0epid_ki_heading\x18\t \x01(\x01H\x08\x88\x01\x01\x12\x1b\n\x0epid_kd_heading\x18\n \x01(\x01H\t\x88\x01\x01\x12%\n\x18pure_pursuit_lookahead_m\x18\x0b \x01(\x01H\n\x88\x01\x01\x12!\n\x14mpc_weight_q_lateral\x18\x0c \x01(\x01H\x0b\x88\x01\x01\x12\"\n\x15mpc_weight_r_steering\x18\r \x01(\x01H\x0c\x88\x01\x01\x42\x1a\n\x18_wheel_radius_left_ratioB\x1b\n\x19_wheel_radius_right_ratioB\x1a\n\x18_effective_track_width_mB\x1b\n\x19_steering_zero_offset_degB\x11\n\x0f_pid_kp_lateralB\x11\n\x0f_pid_ki_lateralB\x11\n\x0f_pid_kd_lateralB\x11\n\x0f_pid_kp_headingB\x11\n\x0f_pid_ki_headingB\x11\n\x0f_pid_kd_headingB\x1b\n\x19_pure_pursuit_lookahead_mB\x17\n\x15_mpc_weight_q_lateralB\x18\n\x16_mpc_weight_r_steering\"\xad\x02\n\rTelemetryData\x12\x1d\n\x15hardware_timestamp_us\x18\x01 \x01(\x03\x12\x10\n\x08odom_x_m\x18\x02 \x01(\x01\x12\x10\n\x08odom_y_m\x18\x03 \x01(\x01\x12\x14\n\x0codom_yaw_rad\x18\x04 \x01(\x01\x12\x1e\n\x16\x66\x65\x65\x64\x62\x61\x63k_linear_vel_ms\x18\x05 \x01(\x01\x12!\n\x19\x66\x65\x65\x64\x62\x61\x63k_angular_vel_rads\x18\x06 \x01(\x01\x12\x1e\n\x16left_motor_current_amp\x18\x07 \x01(\x01\x12\x1f\n\x17right_motor_current_amp\x18\x08 \x01(\x01\x12\"\n\x1asteering_motor_current_amp\x18\t \x01(\x01\x12\x1b\n\x13\x63md_steering_output\x18\n \x01(\x01\x32\xd1\x06\n\x16\x41gvCalibControlService\x12\x61\n\x0eSetControlMode\x12$.agv.calibration.control.ModeRequest\x1a).agv.calibration.control.StandardResponse\x12Z\n\rEmergencyStop\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12i\n\x12\x45xecuteOpenLoopCmd\x12(.agv.calibration.control.OpenLoopRequest\x1a).agv.calibration.control.StandardResponse\x12m\n\x14\x46ollowTestTrajectory\x12*.agv.calibration.control.TrajectoryRequest\x1a).agv.calibration.control.StandardResponse\x12n\n\x13\x45xecuteStepResponse\x12,.agv.calibration.control.StepResponseRequest\x1a).agv.calibration.control.StandardResponse\x12k\n\x16InjectTuningParameters\x12&.agv.calibration.control.ControlParams\x1a).agv.calibration.control.StandardResponse\x12\x64\n\x17\x43ommitControlParameters\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12[\n\x0fStreamTelemetry\x12\x1e.agv.calibration.control.Empty\x1a&.agv.calibration.control.TelemetryData0\x01\x62\x06proto3')
|
||||
|
||||
_globals = globals()
|
||||
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
|
||||
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'agv_calib_control_pb2', _globals)
|
||||
if not _descriptor._USE_C_DESCRIPTORS:
|
||||
DESCRIPTOR._loaded_options = None
|
||||
_globals['_EMPTY']._serialized_start=52
|
||||
_globals['_EMPTY']._serialized_end=59
|
||||
_globals['_STANDARDRESPONSE']._serialized_start=61
|
||||
_globals['_STANDARDRESPONSE']._serialized_end=113
|
||||
_globals['_MODEREQUEST']._serialized_start=116
|
||||
_globals['_MODEREQUEST']._serialized_end=255
|
||||
_globals['_MODEREQUEST_MODE']._serialized_start=195
|
||||
_globals['_MODEREQUEST_MODE']._serialized_end=255
|
||||
_globals['_OPENLOOPREQUEST']._serialized_start=257
|
||||
_globals['_OPENLOOPREQUEST']._serialized_end=369
|
||||
_globals['_TRAJECTORYPOINT']._serialized_start=371
|
||||
_globals['_TRAJECTORYPOINT']._serialized_end=475
|
||||
_globals['_TRAJECTORYREQUEST']._serialized_start=477
|
||||
_globals['_TRAJECTORYREQUEST']._serialized_end=574
|
||||
_globals['_STEPRESPONSEREQUEST']._serialized_start=576
|
||||
_globals['_STEPRESPONSEREQUEST']._serialized_end=647
|
||||
_globals['_CONTROLPARAMS']._serialized_start=650
|
||||
_globals['_CONTROLPARAMS']._serialized_end=1411
|
||||
_globals['_TELEMETRYDATA']._serialized_start=1414
|
||||
_globals['_TELEMETRYDATA']._serialized_end=1715
|
||||
_globals['_AGVCALIBCONTROLSERVICE']._serialized_start=1718
|
||||
_globals['_AGVCALIBCONTROLSERVICE']._serialized_end=2567
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@@ -0,0 +1,444 @@
|
||||
# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
|
||||
"""Client and server classes corresponding to protobuf-defined services."""
|
||||
import grpc
|
||||
import warnings
|
||||
|
||||
import agv_calib_control_pb2 as agv__calib__control__pb2
|
||||
|
||||
GRPC_GENERATED_VERSION = '1.78.0'
|
||||
GRPC_VERSION = grpc.__version__
|
||||
_version_not_supported = False
|
||||
|
||||
try:
|
||||
from grpc._utilities import first_version_is_lower
|
||||
_version_not_supported = first_version_is_lower(GRPC_VERSION, GRPC_GENERATED_VERSION)
|
||||
except ImportError:
|
||||
_version_not_supported = True
|
||||
|
||||
if _version_not_supported:
|
||||
raise RuntimeError(
|
||||
f'The grpc package installed is at version {GRPC_VERSION},'
|
||||
+ ' but the generated code in agv_calib_control_pb2_grpc.py depends on'
|
||||
+ f' grpcio>={GRPC_GENERATED_VERSION}.'
|
||||
+ f' Please upgrade your grpc module to grpcio>={GRPC_GENERATED_VERSION}'
|
||||
+ f' or downgrade your generated code using grpcio-tools<={GRPC_VERSION}.'
|
||||
)
|
||||
|
||||
|
||||
class AgvCalibControlServiceStub(object):
|
||||
"""=========================================================
|
||||
核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理
|
||||
[部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
def __init__(self, channel):
|
||||
"""Constructor.
|
||||
|
||||
Args:
|
||||
channel: A grpc.Channel.
|
||||
"""
|
||||
self.SetControlMode = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/SetControlMode',
|
||||
request_serializer=agv__calib__control__pb2.ModeRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.EmergencyStop = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/EmergencyStop',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.ExecuteOpenLoopCmd = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd',
|
||||
request_serializer=agv__calib__control__pb2.OpenLoopRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.FollowTestTrajectory = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory',
|
||||
request_serializer=agv__calib__control__pb2.TrajectoryRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.ExecuteStepResponse = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse',
|
||||
request_serializer=agv__calib__control__pb2.StepResponseRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.InjectTuningParameters = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/InjectTuningParameters',
|
||||
request_serializer=agv__calib__control__pb2.ControlParams.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.CommitControlParameters = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/CommitControlParameters',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.StreamTelemetry = channel.unary_stream(
|
||||
'/agv.calibration.control.AgvCalibControlService/StreamTelemetry',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.TelemetryData.FromString,
|
||||
_registered_method=True)
|
||||
|
||||
|
||||
class AgvCalibControlServiceServicer(object):
|
||||
"""=========================================================
|
||||
核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理
|
||||
[部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
def SetControlMode(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第一步：权限接管与生命周期安全管控
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]：要求切断避障，但保留底层 PID/MPC 算法就绪
|
||||
🚙 [Windows 返回 -> Linux]：回复模式切换成功，准备好接考题
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def EmergencyStop(self, request, context):
|
||||
"""💻 [Linux 发送 -> Windows]：断网或飞车时的最高级别急停，无视一切直接刹车
|
||||
🚙 [Windows 返回 -> Linux]：返回底层抱死结果
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def ExecuteOpenLoopCmd(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第二步：运动考题下发 (开环排雷 / 闭环寻优 / 波峰对齐)
|
||||
---------------------------------------------------------
|
||||
【场景A: 纯物理开环备用】
|
||||
💻 [Linux 发送 -> Windows]：要求切断算法盲跑，多用于摸底或辅助验证
|
||||
🚙 [Windows 返回 -> Linux]：确认已按指定 RPM/PWM 运转
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def FollowTestTrajectory(self, request, context):
|
||||
"""【场景B: 算法闭环调优】
|
||||
💻 [Linux 发送 -> Windows]：下发一条由几百个点组成的测试轨迹(如 S型贝塞尔曲线)
|
||||
🚙 [Windows 返回 -> Linux]：收到轨迹后，车端立刻使用它自带的 PID/MPC 算法努力贴合轨迹跑圈
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def ExecuteStepResponse(self, request, context):
|
||||
"""【场景C: 波峰时序对齐】
|
||||
💻 [Linux 发送 -> Windows]：下发极短促的阶跃加速指令，人为制造绝对速度波峰
|
||||
🚙 [Windows 返回 -> Linux]：确认加速。(Linux 借此波峰算出网络的绝对 Time Offset)
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def InjectTuningParameters(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第三步：运控参数 AI 寻优：动态热注入与最终固化
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]：Linux 发现上一圈跑得差，AI算出了新的 PID/前瞻距离，要求立即热注入
|
||||
🚙 [Windows 返回 -> Linux]：车端将新参数瞬间覆写进运行内存(不重启系统)，随时准备用新参数重跑
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def CommitControlParameters(self, request, context):
|
||||
"""💻 [Linux 发送 -> Windows]：Linux 判定误差极小，调优结束，命令固化目前内存里的最高分参数
|
||||
🚙 [Windows 返回 -> Linux]：车端将这组完美参数永久覆写进硬盘的 config.yaml 或系统注册表
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def StreamTelemetry(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第四步：高频数字孪生体感上报 (50Hz)
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]：空包触发，命令车端开始疯狂推流
|
||||
🚙 [Windows 持续流式返回 -> Linux]：以 50Hz 频率，持续上报自己的里程计坐标、速度和单调时间戳
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
|
||||
def add_AgvCalibControlServiceServicer_to_server(servicer, server):
|
||||
rpc_method_handlers = {
|
||||
'SetControlMode': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.SetControlMode,
|
||||
request_deserializer=agv__calib__control__pb2.ModeRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'EmergencyStop': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.EmergencyStop,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'ExecuteOpenLoopCmd': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.ExecuteOpenLoopCmd,
|
||||
request_deserializer=agv__calib__control__pb2.OpenLoopRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'FollowTestTrajectory': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.FollowTestTrajectory,
|
||||
request_deserializer=agv__calib__control__pb2.TrajectoryRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'ExecuteStepResponse': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.ExecuteStepResponse,
|
||||
request_deserializer=agv__calib__control__pb2.StepResponseRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'InjectTuningParameters': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.InjectTuningParameters,
|
||||
request_deserializer=agv__calib__control__pb2.ControlParams.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'CommitControlParameters': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.CommitControlParameters,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'StreamTelemetry': grpc.unary_stream_rpc_method_handler(
|
||||
servicer.StreamTelemetry,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.TelemetryData.SerializeToString,
|
||||
),
|
||||
}
|
||||
generic_handler = grpc.method_handlers_generic_handler(
|
||||
'agv.calibration.control.AgvCalibControlService', rpc_method_handlers)
|
||||
server.add_generic_rpc_handlers((generic_handler,))
|
||||
server.add_registered_method_handlers('agv.calibration.control.AgvCalibControlService', rpc_method_handlers)
|
||||
|
||||
|
||||
# This class is part of an EXPERIMENTAL API.
|
||||
class AgvCalibControlService(object):
|
||||
"""=========================================================
|
||||
核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理
|
||||
[部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
@staticmethod
|
||||
def SetControlMode(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/SetControlMode',
|
||||
agv__calib__control__pb2.ModeRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def EmergencyStop(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/EmergencyStop',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def ExecuteOpenLoopCmd(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd',
|
||||
agv__calib__control__pb2.OpenLoopRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def FollowTestTrajectory(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory',
|
||||
agv__calib__control__pb2.TrajectoryRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def ExecuteStepResponse(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse',
|
||||
agv__calib__control__pb2.StepResponseRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def InjectTuningParameters(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/InjectTuningParameters',
|
||||
agv__calib__control__pb2.ControlParams.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def CommitControlParameters(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/CommitControlParameters',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def StreamTelemetry(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_stream(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/StreamTelemetry',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.TelemetryData.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
@@ -5,74 +5,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# 1. 寻找 ROS 2 核心依赖
|
||||
# 1. 寻找 ROS 2 和 行为树 核心依赖
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(behaviortree_cpp_v3 REQUIRED)
|
||||
find_package(ament_index_cpp REQUIRED)
|
||||
|
||||
# ==========================================================
|
||||
# 🚨 核心排雷修复：放弃 find_package，改用 PkgConfig 寻找 gRPC
|
||||
# ==========================================================
|
||||
find_package(Protobuf REQUIRED) # Protobuf 依然可以用自带的找
|
||||
find_package(PkgConfig REQUIRED) # 引入 Linux 底层 PkgConfig 工具
|
||||
# 强制让系统去底层寻找 grpc++ 库，并映射为虚拟目标 PkgConfig::GRPC
|
||||
pkg_check_modules(GRPC REQUIRED IMPORTED_TARGET grpc++)
|
||||
# ==========================================================
|
||||
# 2. 编译主节点
|
||||
add_executable(master_node src/master_node.cpp)
|
||||
target_include_directories(master_node PUBLIC src)
|
||||
ament_target_dependencies(master_node rclcpp behaviortree_cpp_v3 ament_index_cpp)
|
||||
|
||||
# 3. 设置 Proto 文件夹路径与输出路径
|
||||
set(PROTO_DIR "${CMAKE_CURRENT_SOURCE_DIR}/proto")
|
||||
file(GLOB PROTO_FILES "${PROTO_DIR}/*.proto")
|
||||
|
||||
set(PROTO_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/grpc_gen")
|
||||
file(MAKE_DIRECTORY ${PROTO_OUT_DIR})
|
||||
|
||||
# 4. 自动生成 gRPC C++ 源码
|
||||
find_program(GRPC_CPP_PLUGIN_EXECUTABLE grpc_cpp_plugin REQUIRED)
|
||||
|
||||
set(PROTO_SRCS "")
|
||||
set(PROTO_HDRS "")
|
||||
foreach(PROTO_FILE ${PROTO_FILES})
|
||||
get_filename_component(FIL_WE ${PROTO_FILE} NAME_WE)
|
||||
list(APPEND PROTO_SRCS "${PROTO_OUT_DIR}/${FIL_WE}.pb.cc" "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.cc")
|
||||
list(APPEND PROTO_HDRS "${PROTO_OUT_DIR}/${FIL_WE}.pb.h" "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.h")
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT "${PROTO_OUT_DIR}/${FIL_WE}.pb.cc"
|
||||
"${PROTO_OUT_DIR}/${FIL_WE}.pb.h"
|
||||
"${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.cc"
|
||||
"${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.h"
|
||||
COMMAND protoc
|
||||
# ==========================================================
|
||||
# 🚨 核心修复：强行解锁 proto3 的 optional 实验性特性语法
|
||||
# ==========================================================
|
||||
ARGS --experimental_allow_proto3_optional # <--- 🚨 就是加了这一行！
|
||||
--proto_path="${PROTO_DIR}"
|
||||
--cpp_out="${PROTO_OUT_DIR}"
|
||||
--grpc_out="${PROTO_OUT_DIR}"
|
||||
--plugin=protoc-gen-grpc="${GRPC_CPP_PLUGIN_EXECUTABLE}"
|
||||
"${PROTO_FILE}"
|
||||
DEPENDS "${PROTO_FILE}"
|
||||
)
|
||||
endforeach()
|
||||
|
||||
# 5. 将所有生成的网络源码统一打包成独立的 C++ 共享库
|
||||
add_library(${PROJECT_NAME}_grpc_lib SHARED ${PROTO_SRCS})
|
||||
target_include_directories(${PROJECT_NAME}_grpc_lib PUBLIC "${PROTO_OUT_DIR}")
|
||||
|
||||
# 🚨 核心修复 2：这里必须改成链接 PkgConfig::GRPC
|
||||
target_link_libraries(${PROJECT_NAME}_grpc_lib
|
||||
PkgConfig::GRPC
|
||||
protobuf::libprotobuf
|
||||
)
|
||||
|
||||
# 6. 编译具体的标定业务主控节点
|
||||
add_executable(brain_node src/brain_node.cpp)
|
||||
ament_target_dependencies(brain_node rclcpp)
|
||||
|
||||
# 必须链接刚才生成的协议库
|
||||
target_link_libraries(brain_node ${PROJECT_NAME}_grpc_lib)
|
||||
|
||||
# 7. ROS 2 安装规则
|
||||
install(TARGETS brain_node DESTINATION lib/${PROJECT_NAME})
|
||||
install(DIRECTORY proto/ DESTINATION share/${PROJECT_NAME}/proto)
|
||||
# 3. 安装规则 (极其重要：把剧本和程序装到系统目录，让 ROS 2 能找到它)
|
||||
install(TARGETS master_node DESTINATION lib/${PROJECT_NAME})
|
||||
install(DIRECTORY behavior_trees/ DESTINATION share/${PROJECT_NAME}/behavior_trees)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,23 @@
|
||||
<root main_tree_to_execute="MainCalibrationFlow">
|
||||
<BehaviorTree ID="MainCalibrationFlow">
|
||||
<Sequence name="全自动标定主干">
|
||||
|
||||
<Sequence name="Phase_1_Chassis">
|
||||
<MockConnectAGV />
|
||||
<MockCallChassisAlgo />
|
||||
</Sequence>
|
||||
|
||||
<Sequence name="Phase_2_Control">
|
||||
<MockTuneControl />
|
||||
<MockCallControlAlgo />
|
||||
</Sequence>
|
||||
|
||||
<Sequence name="Phase_3_Sensor">
|
||||
<MockMoveAndCapture />
|
||||
<MockCallSensorAlgo />
|
||||
<MockCommitAllParams />
|
||||
</Sequence>
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -9,6 +9,10 @@
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>behaviortree_cpp_v3</depend>
|
||||
<depend>ament_index_cpp</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
|
||||
@@ -0,0 +1,82 @@
|
||||
#pragma once
|
||||
#include <behaviortree_cpp_v3/action_node.h>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
// 1. 假装连接车端并夺权
|
||||
class MockConnectAGV : public BT::SyncActionNode {
|
||||
public:
|
||||
MockConnectAGV(const std::string& name) : BT::SyncActionNode(name, {}) {}
|
||||
BT::NodeStatus tick() override {
|
||||
std::cout << "💻 [gRPC 对外] 📡 正在连接 Windows 车端... 夺权成功！" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500)); // 假装网络耗时 0.5 秒
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
// 2. 假装呼叫底盘算法
|
||||
class MockCallChassisAlgo : public BT::SyncActionNode {
|
||||
public:
|
||||
MockCallChassisAlgo(const std::string& name) : BT::SyncActionNode(name, {}) {}
|
||||
BT::NodeStatus tick() override {
|
||||
std::cout << "🧮 [Action 对内] 🚙 丢给底盘算法团队... 算好了！左轮径 0.098m。" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(800));
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
// 3. 假装控制车子跑S型曲线
|
||||
class MockTuneControl : public BT::SyncActionNode {
|
||||
public:
|
||||
MockTuneControl(const std::string& name) : BT::SyncActionNode(name, {}) {}
|
||||
BT::NodeStatus tick() override {
|
||||
std::cout << "💻 [gRPC 对外] 📈 正在下发S型曲线测试考题... 车端已跑完。" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
// 4. 假装呼叫AI寻优算法
|
||||
class MockCallControlAlgo : public BT::SyncActionNode {
|
||||
public:
|
||||
MockCallControlAlgo(const std::string& name) : BT::SyncActionNode(name, {}) {}
|
||||
BT::NodeStatus tick() override {
|
||||
std::cout << "🧮 [Action 对内] 🧠 AI贝叶斯打分完毕... PID 最优参数已锁定！" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(800));
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
// 5. 假装走停拍并下载大文件
|
||||
class MockMoveAndCapture : public BT::SyncActionNode {
|
||||
public:
|
||||
MockMoveAndCapture(const std::string& name) : BT::SyncActionNode(name, {}) {}
|
||||
BT::NodeStatus tick() override {
|
||||
std::cout << "💻 [gRPC 对外] 🛑 刹车静止...咔嚓！5MB大文件已下载至 /tmp/cam.png" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(800));
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
// 6. 假装调用外参标定视觉算法
|
||||
class MockCallSensorAlgo : public BT::SyncActionNode {
|
||||
public:
|
||||
MockCallSensorAlgo(const std::string& name) : BT::SyncActionNode(name, {}) {}
|
||||
BT::NodeStatus tick() override {
|
||||
std::cout << "🧮 [Action 对内] 📷 视觉团队正在算矩阵... 拿到 4x4 外参矩阵！" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(800));
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
// 7. 假装出厂固化
|
||||
class MockCommitAllParams : public BT::SyncActionNode {
|
||||
public:
|
||||
MockCommitAllParams(const std::string& name) : BT::SyncActionNode(name, {}) {}
|
||||
BT::NodeStatus tick() override {
|
||||
std::cout << "💻 [gRPC 对外] 💾 正在把所有完美参数烧录进 AGV... 标定闭环，可以出厂！\n" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(300));
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,54 @@
|
||||
#pragma once
|
||||
#include <behaviortree_cpp_v3/action_node.h>
|
||||
#include <grpcpp/grpcpp.h>
|
||||
// 引入 CMake 自动生成的 C++ 网络契约头文件！
|
||||
#include "agv_calib_control.grpc.pb.h"
|
||||
|
||||
using namespace agv::calibration::control;
|
||||
|
||||
// =========================================================
|
||||
// 🌟 真实网络积木：连接车端并夺取控制权
|
||||
// =========================================================
|
||||
class ConnectAGVNode : public BT::SyncActionNode {
|
||||
public:
|
||||
// 注意：构造函数这里多了一个 const BT::NodeConfiguration& config 参数
|
||||
ConnectAGVNode(const std::string& name, const BT::NodeConfiguration& config) : BT::SyncActionNode(name, config) {
|
||||
// 1. 初始化时，拨号连接到车端 (因为我们要自己测，所以先连本机 127.0.0.1 端口)
|
||||
channel_ = grpc::CreateChannel("127.0.0.1:50051", grpc::InsecureChannelCredentials());
|
||||
stub_ = AgvCalibControlService::NewStub(channel_);
|
||||
}
|
||||
|
||||
// 行为树必须的静态函数 (定义端口)
|
||||
static BT::PortsList providedPorts() { return {}; }
|
||||
|
||||
BT::NodeStatus tick() override {
|
||||
std::cout << "\n💻 [行为树真节点] 正在通过 gRPC 向车端发起夺权请求..." << std::endl;
|
||||
|
||||
// 2. 准备发送载荷：要求进入调优模式
|
||||
ModeRequest request;
|
||||
request.set_target_mode(ModeRequest::TUNING_MODE);
|
||||
|
||||
StandardResponse response;
|
||||
grpc::ClientContext context;
|
||||
|
||||
// 🚨 架构师防线：设置 2 秒网络超时！如果网络断了，绝不让主线程死锁卡住！
|
||||
context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
|
||||
|
||||
// 3. 发射真正的网络脉冲！
|
||||
grpc::Status status = stub_->SetControlMode(&context, request, &response);
|
||||
|
||||
// 4. 根据网络回复，决定行为树这根树枝是亮绿灯还是红灯
|
||||
if (status.ok() && response.success()) {
|
||||
std::cout << "✅ [网络通信成功] 车端回执: " << response.message() << std::endl;
|
||||
return BT::NodeStatus::SUCCESS; // 绿灯，允许行为树执行下一步
|
||||
} else {
|
||||
std::cerr << "❌ [网络通信失败] 错误码: " << status.error_code()
|
||||
<< " 详情: " << status.error_message() << std::endl;
|
||||
return BT::NodeStatus::FAILURE; // 红灯，触发行为树重试或报警
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<grpc::Channel> channel_;
|
||||
std::unique_ptr<AgvCalibControlService::Stub> stub_;
|
||||
};
|
||||
@@ -1,12 +1,44 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <behaviortree_cpp_v3/bt_factory.h>
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
|
||||
// 引入刚才写的假节点
|
||||
#include "bt_nodes/dummy_nodes.hpp"
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = rclcpp::Node::make_shared("master_node");
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "🚀 AGV 标定主控大脑编译成功，gRPC 契约已挂载！");
|
||||
|
||||
rclcpp::spin(node);
|
||||
std::cout << "\n=========================================" << std::endl;
|
||||
std::cout << "🚀 AGV 标定车间中央大脑 [空转测试版] 启动！" << std::endl;
|
||||
std::cout << "=========================================\n" << std::endl;
|
||||
|
||||
BT::BehaviorTreeFactory factory;
|
||||
|
||||
// 1. 把 C++ 类注册到工厂，名字必须和 XML 里的一模一样！
|
||||
factory.registerNodeType<MockConnectAGV>("MockConnectAGV");
|
||||
factory.registerNodeType<MockCallChassisAlgo>("MockCallChassisAlgo");
|
||||
factory.registerNodeType<MockTuneControl>("MockTuneControl");
|
||||
factory.registerNodeType<MockCallControlAlgo>("MockCallControlAlgo");
|
||||
factory.registerNodeType<MockMoveAndCapture>("MockMoveAndCapture");
|
||||
factory.registerNodeType<MockCallSensorAlgo>("MockCallSensorAlgo");
|
||||
factory.registerNodeType<MockCommitAllParams>("MockCommitAllParams");
|
||||
|
||||
try {
|
||||
// 2. 动态获取 XML 剧本的绝对路径 (防止你运行程序时路径不对找不到文件)
|
||||
std::string pkg_path = ament_index_cpp::get_package_share_directory("agv_calib_core");
|
||||
std::string xml_file = pkg_path + "/behavior_trees/main_pipeline.xml";
|
||||
|
||||
auto tree = factory.createTreeFromFile(xml_file);
|
||||
std::cout << "📜 行为树剧本加载完毕，开始全自动流水线...\n" << std::endl;
|
||||
|
||||
// 3. 开始执行总控流！
|
||||
tree.tickRoot();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "❌ 加载 XML 失败: " << e.what() << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "🎉 全流程执行完毕，完美收工！\n" << std::endl;
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# NO CHECKED-IN PROTOBUF GENCODE
|
||||
# source: agv_calib_control.proto
|
||||
# Protobuf Python Version: 6.31.1
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import runtime_version as _runtime_version
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
from google.protobuf.internal import builder as _builder
|
||||
_runtime_version.ValidateProtobufRuntimeVersion(
|
||||
_runtime_version.Domain.PUBLIC,
|
||||
6,
|
||||
31,
|
||||
1,
|
||||
'',
|
||||
'agv_calib_control.proto'
|
||||
)
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_sym_db = _symbol_database.Default()
|
||||
|
||||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x17\x61gv_calib_control.proto\x12\x17\x61gv.calibration.control\"\x07\n\x05\x45mpty\"4\n\x10StandardResponse\x12\x0f\n\x07success\x18\x01 \x01(\x08\x12\x0f\n\x07message\x18\x02 \x01(\t\"\x8b\x01\n\x0bModeRequest\x12>\n\x0btarget_mode\x18\x01 \x01(\x0e\x32).agv.calibration.control.ModeRequest.Mode\"<\n\x04Mode\x12\x0f\n\x0bNORMAL_MODE\x10\x00\x12\x12\n\x0eOPEN_LOOP_MODE\x10\x01\x12\x0f\n\x0bTUNING_MODE\x10\x02\"p\n\x0fOpenLoopRequest\x12\x16\n\x0eleft_motor_cmd\x18\x01 \x01(\x01\x12\x17\n\x0fright_motor_cmd\x18\x02 \x01(\x01\x12\x16\n\x0esteering_angle\x18\x03 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x04 \x01(\x01\"h\n\x0fTrajectoryPoint\x12\x0b\n\x03x_m\x18\x01 \x01(\x01\x12\x0b\n\x03y_m\x18\x02 \x01(\x01\x12\x0f\n\x07yaw_rad\x18\x03 \x01(\x01\x12\x17\n\x0ftarget_speed_ms\x18\x04 \x01(\x01\x12\x11\n\tcurvature\x18\x05 \x01(\x01\"a\n\x11TrajectoryRequest\x12\x14\n\x0ctest_case_id\x18\x01 \x01(\t\x12\x36\n\x04path\x18\x02 \x03(\x0b\x32(.agv.calibration.control.TrajectoryPoint\"G\n\x13StepResponseRequest\x12\x1a\n\x12target_velocity_ms\x18\x01 \x01(\x01\x12\x14\n\x0c\x64uration_sec\x18\x02 \x01(\x01\"\xf9\x05\n\rControlParams\x12$\n\x17wheel_radius_left_ratio\x18\x01 \x01(\x01H\x00\x88\x01\x01\x12%\n\x18wheel_radius_right_ratio\x18\x02 \x01(\x01H\x01\x88\x01\x01\x12$\n\x17\x65\x66\x66\x65\x63tive_track_width_m\x18\x03 \x01(\x01H\x02\x88\x01\x01\x12%\n\x18steering_zero_offset_deg\x18\x04 \x01(\x01H\x03\x88\x01\x01\x12\x1b\n\x0epid_kp_lateral\x18\x05 \x01(\x01H\x04\x88\x01\x01\x12\x1b\n\x0epid_ki_lateral\x18\x06 \x01(\x01H\x05\x88\x01\x01\x12\x1b\n\x0epid_kd_lateral\x18\x07 \x01(\x01H\x06\x88\x01\x01\x12\x1b\n\x0epid_kp_heading\x18\x08 \x01(\x01H\x07\x88\x01\x01\x12\x1b\n\x0epid_ki_heading\x18\t \x01(\x01H\x08\x88\x01\x01\x12\x1b\n\x0epid_kd_heading\x18\n \x01(\x01H\t\x88\x01\x01\x12%\n\x18pure_pursuit_lookahead_m\x18\x0b \x01(\x01H\n\x88\x01\x01\x12!\n\x14mpc_weight_q_lateral\x18\x0c \x01(\x01H\x0b\x88\x01\x01\x12\"\n\x15mpc_weight_r_steering\x18\r \x01(\x01H\x0c\x88\x01\x01\x42\x1a\n\x18_wheel_radius_left_ratioB\x1b\n\x19_wheel_radius_right_ratioB\x1a\n\x18_effective_track_width_mB\x1b\n\x19_steering_zero_offset_degB\x11\n\x0f_pid_kp_lateralB\x11\n\x0f_pid_ki_lateralB\x11\n\x0f_pid_kd_lateralB\x11\n\x0f_pid_kp_headingB\x11\n\x0f_pid_ki_headingB\x11\n\x0f_pid_kd_headingB\x1b\n\x19_pure_pursuit_lookahead_mB\x17\n\x15_mpc_weight_q_lateralB\x18\n\x16_mpc_weight_r_steering\"\xad\x02\n\rTelemetryData\x12\x1d\n\x15hardware_timestamp_us\x18\x01 \x01(\x03\x12\x10\n\x08odom_x_m\x18\x02 \x01(\x01\x12\x10\n\x08odom_y_m\x18\x03 \x01(\x01\x12\x14\n\x0codom_yaw_rad\x18\x04 \x01(\x01\x12\x1e\n\x16\x66\x65\x65\x64\x62\x61\x63k_linear_vel_ms\x18\x05 \x01(\x01\x12!\n\x19\x66\x65\x65\x64\x62\x61\x63k_angular_vel_rads\x18\x06 \x01(\x01\x12\x1e\n\x16left_motor_current_amp\x18\x07 \x01(\x01\x12\x1f\n\x17right_motor_current_amp\x18\x08 \x01(\x01\x12\"\n\x1asteering_motor_current_amp\x18\t \x01(\x01\x12\x1b\n\x13\x63md_steering_output\x18\n \x01(\x01\x32\xd1\x06\n\x16\x41gvCalibControlService\x12\x61\n\x0eSetControlMode\x12$.agv.calibration.control.ModeRequest\x1a).agv.calibration.control.StandardResponse\x12Z\n\rEmergencyStop\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12i\n\x12\x45xecuteOpenLoopCmd\x12(.agv.calibration.control.OpenLoopRequest\x1a).agv.calibration.control.StandardResponse\x12m\n\x14\x46ollowTestTrajectory\x12*.agv.calibration.control.TrajectoryRequest\x1a).agv.calibration.control.StandardResponse\x12n\n\x13\x45xecuteStepResponse\x12,.agv.calibration.control.StepResponseRequest\x1a).agv.calibration.control.StandardResponse\x12k\n\x16InjectTuningParameters\x12&.agv.calibration.control.ControlParams\x1a).agv.calibration.control.StandardResponse\x12\x64\n\x17\x43ommitControlParameters\x12\x1e.agv.calibration.control.Empty\x1a).agv.calibration.control.StandardResponse\x12[\n\x0fStreamTelemetry\x12\x1e.agv.calibration.control.Empty\x1a&.agv.calibration.control.TelemetryData0\x01\x62\x06proto3')
|
||||
|
||||
_globals = globals()
|
||||
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
|
||||
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'agv_calib_control_pb2', _globals)
|
||||
if not _descriptor._USE_C_DESCRIPTORS:
|
||||
DESCRIPTOR._loaded_options = None
|
||||
_globals['_EMPTY']._serialized_start=52
|
||||
_globals['_EMPTY']._serialized_end=59
|
||||
_globals['_STANDARDRESPONSE']._serialized_start=61
|
||||
_globals['_STANDARDRESPONSE']._serialized_end=113
|
||||
_globals['_MODEREQUEST']._serialized_start=116
|
||||
_globals['_MODEREQUEST']._serialized_end=255
|
||||
_globals['_MODEREQUEST_MODE']._serialized_start=195
|
||||
_globals['_MODEREQUEST_MODE']._serialized_end=255
|
||||
_globals['_OPENLOOPREQUEST']._serialized_start=257
|
||||
_globals['_OPENLOOPREQUEST']._serialized_end=369
|
||||
_globals['_TRAJECTORYPOINT']._serialized_start=371
|
||||
_globals['_TRAJECTORYPOINT']._serialized_end=475
|
||||
_globals['_TRAJECTORYREQUEST']._serialized_start=477
|
||||
_globals['_TRAJECTORYREQUEST']._serialized_end=574
|
||||
_globals['_STEPRESPONSEREQUEST']._serialized_start=576
|
||||
_globals['_STEPRESPONSEREQUEST']._serialized_end=647
|
||||
_globals['_CONTROLPARAMS']._serialized_start=650
|
||||
_globals['_CONTROLPARAMS']._serialized_end=1411
|
||||
_globals['_TELEMETRYDATA']._serialized_start=1414
|
||||
_globals['_TELEMETRYDATA']._serialized_end=1715
|
||||
_globals['_AGVCALIBCONTROLSERVICE']._serialized_start=1718
|
||||
_globals['_AGVCALIBCONTROLSERVICE']._serialized_end=2567
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@@ -0,0 +1,444 @@
|
||||
# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
|
||||
"""Client and server classes corresponding to protobuf-defined services."""
|
||||
import grpc
|
||||
import warnings
|
||||
|
||||
import agv_calib_control_pb2 as agv__calib__control__pb2
|
||||
|
||||
GRPC_GENERATED_VERSION = '1.78.0'
|
||||
GRPC_VERSION = grpc.__version__
|
||||
_version_not_supported = False
|
||||
|
||||
try:
|
||||
from grpc._utilities import first_version_is_lower
|
||||
_version_not_supported = first_version_is_lower(GRPC_VERSION, GRPC_GENERATED_VERSION)
|
||||
except ImportError:
|
||||
_version_not_supported = True
|
||||
|
||||
if _version_not_supported:
|
||||
raise RuntimeError(
|
||||
f'The grpc package installed is at version {GRPC_VERSION},'
|
||||
+ ' but the generated code in agv_calib_control_pb2_grpc.py depends on'
|
||||
+ f' grpcio>={GRPC_GENERATED_VERSION}.'
|
||||
+ f' Please upgrade your grpc module to grpcio>={GRPC_GENERATED_VERSION}'
|
||||
+ f' or downgrade your generated code using grpcio-tools<={GRPC_VERSION}.'
|
||||
)
|
||||
|
||||
|
||||
class AgvCalibControlServiceStub(object):
|
||||
"""=========================================================
|
||||
核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理
|
||||
[部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
def __init__(self, channel):
|
||||
"""Constructor.
|
||||
|
||||
Args:
|
||||
channel: A grpc.Channel.
|
||||
"""
|
||||
self.SetControlMode = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/SetControlMode',
|
||||
request_serializer=agv__calib__control__pb2.ModeRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.EmergencyStop = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/EmergencyStop',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.ExecuteOpenLoopCmd = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd',
|
||||
request_serializer=agv__calib__control__pb2.OpenLoopRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.FollowTestTrajectory = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory',
|
||||
request_serializer=agv__calib__control__pb2.TrajectoryRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.ExecuteStepResponse = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse',
|
||||
request_serializer=agv__calib__control__pb2.StepResponseRequest.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.InjectTuningParameters = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/InjectTuningParameters',
|
||||
request_serializer=agv__calib__control__pb2.ControlParams.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.CommitControlParameters = channel.unary_unary(
|
||||
'/agv.calibration.control.AgvCalibControlService/CommitControlParameters',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.StandardResponse.FromString,
|
||||
_registered_method=True)
|
||||
self.StreamTelemetry = channel.unary_stream(
|
||||
'/agv.calibration.control.AgvCalibControlService/StreamTelemetry',
|
||||
request_serializer=agv__calib__control__pb2.Empty.SerializeToString,
|
||||
response_deserializer=agv__calib__control__pb2.TelemetryData.FromString,
|
||||
_registered_method=True)
|
||||
|
||||
|
||||
class AgvCalibControlServiceServicer(object):
|
||||
"""=========================================================
|
||||
核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理
|
||||
[部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
def SetControlMode(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第一步：权限接管与生命周期安全管控
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]：要求切断避障，但保留底层 PID/MPC 算法就绪
|
||||
🚙 [Windows 返回 -> Linux]：回复模式切换成功，准备好接考题
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def EmergencyStop(self, request, context):
|
||||
"""💻 [Linux 发送 -> Windows]：断网或飞车时的最高级别急停，无视一切直接刹车
|
||||
🚙 [Windows 返回 -> Linux]：返回底层抱死结果
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def ExecuteOpenLoopCmd(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第二步：运动考题下发 (开环排雷 / 闭环寻优 / 波峰对齐)
|
||||
---------------------------------------------------------
|
||||
【场景A: 纯物理开环备用】
|
||||
💻 [Linux 发送 -> Windows]：要求切断算法盲跑，多用于摸底或辅助验证
|
||||
🚙 [Windows 返回 -> Linux]：确认已按指定 RPM/PWM 运转
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def FollowTestTrajectory(self, request, context):
|
||||
"""【场景B: 算法闭环调优】
|
||||
💻 [Linux 发送 -> Windows]：下发一条由几百个点组成的测试轨迹(如 S型贝塞尔曲线)
|
||||
🚙 [Windows 返回 -> Linux]：收到轨迹后，车端立刻使用它自带的 PID/MPC 算法努力贴合轨迹跑圈
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def ExecuteStepResponse(self, request, context):
|
||||
"""【场景C: 波峰时序对齐】
|
||||
💻 [Linux 发送 -> Windows]：下发极短促的阶跃加速指令，人为制造绝对速度波峰
|
||||
🚙 [Windows 返回 -> Linux]：确认加速。(Linux 借此波峰算出网络的绝对 Time Offset)
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def InjectTuningParameters(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第三步：运控参数 AI 寻优：动态热注入与最终固化
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]：Linux 发现上一圈跑得差，AI算出了新的 PID/前瞻距离，要求立即热注入
|
||||
🚙 [Windows 返回 -> Linux]：车端将新参数瞬间覆写进运行内存(不重启系统)，随时准备用新参数重跑
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def CommitControlParameters(self, request, context):
|
||||
"""💻 [Linux 发送 -> Windows]：Linux 判定误差极小，调优结束，命令固化目前内存里的最高分参数
|
||||
🚙 [Windows 返回 -> Linux]：车端将这组完美参数永久覆写进硬盘的 config.yaml 或系统注册表
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
def StreamTelemetry(self, request, context):
|
||||
"""---------------------------------------------------------
|
||||
第四步：高频数字孪生体感上报 (50Hz)
|
||||
---------------------------------------------------------
|
||||
💻 [Linux 发送 -> Windows]：空包触发，命令车端开始疯狂推流
|
||||
🚙 [Windows 持续流式返回 -> Linux]：以 50Hz 频率，持续上报自己的里程计坐标、速度和单调时间戳
|
||||
"""
|
||||
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
|
||||
context.set_details('Method not implemented!')
|
||||
raise NotImplementedError('Method not implemented!')
|
||||
|
||||
|
||||
def add_AgvCalibControlServiceServicer_to_server(servicer, server):
|
||||
rpc_method_handlers = {
|
||||
'SetControlMode': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.SetControlMode,
|
||||
request_deserializer=agv__calib__control__pb2.ModeRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'EmergencyStop': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.EmergencyStop,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'ExecuteOpenLoopCmd': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.ExecuteOpenLoopCmd,
|
||||
request_deserializer=agv__calib__control__pb2.OpenLoopRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'FollowTestTrajectory': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.FollowTestTrajectory,
|
||||
request_deserializer=agv__calib__control__pb2.TrajectoryRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'ExecuteStepResponse': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.ExecuteStepResponse,
|
||||
request_deserializer=agv__calib__control__pb2.StepResponseRequest.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'InjectTuningParameters': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.InjectTuningParameters,
|
||||
request_deserializer=agv__calib__control__pb2.ControlParams.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'CommitControlParameters': grpc.unary_unary_rpc_method_handler(
|
||||
servicer.CommitControlParameters,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.StandardResponse.SerializeToString,
|
||||
),
|
||||
'StreamTelemetry': grpc.unary_stream_rpc_method_handler(
|
||||
servicer.StreamTelemetry,
|
||||
request_deserializer=agv__calib__control__pb2.Empty.FromString,
|
||||
response_serializer=agv__calib__control__pb2.TelemetryData.SerializeToString,
|
||||
),
|
||||
}
|
||||
generic_handler = grpc.method_handlers_generic_handler(
|
||||
'agv.calibration.control.AgvCalibControlService', rpc_method_handlers)
|
||||
server.add_generic_rpc_handlers((generic_handler,))
|
||||
server.add_registered_method_handlers('agv.calibration.control.AgvCalibControlService', rpc_method_handlers)
|
||||
|
||||
|
||||
# This class is part of an EXPERIMENTAL API.
|
||||
class AgvCalibControlService(object):
|
||||
"""=========================================================
|
||||
核心服务：AGV 运控大脑（PID/MPC）参数自动化寻优调教代理
|
||||
[部署端 Server]：Windows车端 (满血保留自身算法，负责执行闭环追踪与高频汇报)
|
||||
[调用端 Client]：Linux标定服务器 (上帝视角，负责发轨迹、看误差、AI打分与发新参数)
|
||||
=========================================================
|
||||
"""
|
||||
|
||||
@staticmethod
|
||||
def SetControlMode(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/SetControlMode',
|
||||
agv__calib__control__pb2.ModeRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def EmergencyStop(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/EmergencyStop',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def ExecuteOpenLoopCmd(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteOpenLoopCmd',
|
||||
agv__calib__control__pb2.OpenLoopRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def FollowTestTrajectory(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/FollowTestTrajectory',
|
||||
agv__calib__control__pb2.TrajectoryRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def ExecuteStepResponse(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/ExecuteStepResponse',
|
||||
agv__calib__control__pb2.StepResponseRequest.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def InjectTuningParameters(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/InjectTuningParameters',
|
||||
agv__calib__control__pb2.ControlParams.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def CommitControlParameters(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_unary(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/CommitControlParameters',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.StandardResponse.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
|
||||
@staticmethod
|
||||
def StreamTelemetry(request,
|
||||
target,
|
||||
options=(),
|
||||
channel_credentials=None,
|
||||
call_credentials=None,
|
||||
insecure=False,
|
||||
compression=None,
|
||||
wait_for_ready=None,
|
||||
timeout=None,
|
||||
metadata=None):
|
||||
return grpc.experimental.unary_stream(
|
||||
request,
|
||||
target,
|
||||
'/agv.calibration.control.AgvCalibControlService/StreamTelemetry',
|
||||
agv__calib__control__pb2.Empty.SerializeToString,
|
||||
agv__calib__control__pb2.TelemetryData.FromString,
|
||||
options,
|
||||
channel_credentials,
|
||||
insecure,
|
||||
call_credentials,
|
||||
compression,
|
||||
wait_for_ready,
|
||||
timeout,
|
||||
metadata,
|
||||
_registered_method=True)
|
||||
@@ -0,0 +1,26 @@
|
||||
import grpc
|
||||
from concurrent import futures
|
||||
import time
|
||||
import agv_calib_control_pb2 as pb2
|
||||
import agv_calib_control_pb2_grpc as pb2_grpc
|
||||
|
||||
# 扮演 Windows 车端的角色
|
||||
class FakeAgvServer(pb2_grpc.AgvCalibControlServiceServicer):
|
||||
def SetControlMode(self, request, context):
|
||||
print(f"\n[🚙 Windows 假车端] 收到 Linux 夺权指令! 目标模式: {request.target_mode}")
|
||||
time.sleep(0.5) # 假装底层继电器切换花了点时间
|
||||
print("[🚙 Windows 假车端] 避障已切断，乖乖交出控制权！")
|
||||
|
||||
# 返回成功回执给 Linux
|
||||
return pb2.StandardResponse(success=True, message="Windows: 已交出底盘控制权！")
|
||||
|
||||
def serve():
|
||||
server = grpc.server(futures.ThreadPoolExecutor(max_workers=10))
|
||||
pb2_grpc.add_AgvCalibControlServiceServicer_to_server(FakeAgvServer(), server)
|
||||
server.add_insecure_port('[::]:50051')
|
||||
print("🚀 [Windows 假车端] 已启动，正在监听 50051 端口，等待 Linux 大脑连接...")
|
||||
server.start()
|
||||
server.wait_for_termination()
|
||||
|
||||
if __name__ == '__main__':
|
||||
serve()
|
||||
@@ -0,0 +1,159 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(win_ubuntu_bridge)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# ==========================================================
|
||||
# 1. 寻找 ROS 2 核心库与消息生成器
|
||||
# ==========================================================
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_components REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
find_package(action_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(behaviortree_cpp_v3 REQUIRED)
|
||||
|
||||
# ==========================================================
|
||||
# 2. 编译 ROS 2 自定义接口 (Msg / Srv / Action)
|
||||
# ==========================================================
|
||||
# 🚨 已经严格按照你截图中的目录结构为你写好
|
||||
set(interface_files
|
||||
"msg/HardwareState.msg"
|
||||
"msg/ControlTelemetry.msg"
|
||||
"msg/TrajectoryPoint.msg"
|
||||
"msg/CameraIntrinsic.msg"
|
||||
"msg/SensorExtrinsic.msg"
|
||||
|
||||
# chassis
|
||||
"srv/CommitKinematics.srv"
|
||||
"srv/ExecuteRawDrive.srv"
|
||||
"srv/ExecuteRawSteer.srv"
|
||||
"srv/SetDiagnosticMode.srv"
|
||||
|
||||
# control
|
||||
"srv/ExecuteOpenLoopCmd.srv"
|
||||
"srv/ExecuteStepResponse.srv"
|
||||
"srv/FollowTestTrajectory.srv"
|
||||
"srv/InjectTuningParams.srv"
|
||||
"srv/SetControlMode.srv"
|
||||
|
||||
# sensor
|
||||
"srv/CommitCalibrationResults.srv"
|
||||
"srv/MoveToObservationPose.srv"
|
||||
"srv/TriggerSyncCapture.srv"
|
||||
|
||||
"action/DownloadSensorData.action"
|
||||
)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
${interface_files}
|
||||
DEPENDENCIES std_msgs std_srvs action_msgs
|
||||
)
|
||||
|
||||
# ==========================================================
|
||||
# 3. 寻找系统底层的 gRPC 并编译 .proto 文件
|
||||
# ==========================================================
|
||||
find_package(Protobuf REQUIRED)
|
||||
find_package(PkgConfig REQUIRED)
|
||||
pkg_check_modules(GRPC REQUIRED IMPORTED_TARGET grpc++)
|
||||
|
||||
set(PROTO_DIR "${CMAKE_CURRENT_SOURCE_DIR}/proto")
|
||||
file(GLOB PROTO_FILES "${PROTO_DIR}/*.proto")
|
||||
set(PROTO_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/grpc_gen")
|
||||
file(MAKE_DIRECTORY ${PROTO_OUT_DIR})
|
||||
|
||||
find_program(GRPC_CPP_PLUGIN_EXECUTABLE grpc_cpp_plugin REQUIRED)
|
||||
|
||||
set(PROTO_SRCS "")
|
||||
set(PROTO_HDRS "")
|
||||
foreach(PROTO_FILE ${PROTO_FILES})
|
||||
get_filename_component(FIL_WE ${PROTO_FILE} NAME_WE)
|
||||
list(APPEND PROTO_SRCS "${PROTO_OUT_DIR}/${FIL_WE}.pb.cc" "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.cc")
|
||||
list(APPEND PROTO_HDRS "${PROTO_OUT_DIR}/${FIL_WE}.pb.h" "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.h")
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT "${PROTO_OUT_DIR}/${FIL_WE}.pb.cc" "${PROTO_OUT_DIR}/${FIL_WE}.pb.h"
|
||||
"${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.cc" "${PROTO_OUT_DIR}/${FIL_WE}.grpc.pb.h"
|
||||
COMMAND ${Protobuf_PROTOC_EXECUTABLE}
|
||||
"--experimental_allow_proto3_optional"
|
||||
"--proto_path=${PROTO_DIR}"
|
||||
"--cpp_out=${PROTO_OUT_DIR}"
|
||||
"--grpc_out=${PROTO_OUT_DIR}"
|
||||
"--plugin=protoc-gen-grpc=${GRPC_CPP_PLUGIN_EXECUTABLE}"
|
||||
"${PROTO_FILE}"
|
||||
DEPENDS "${PROTO_FILE}"
|
||||
COMMENT "🚀 正在翻译 ${FIL_WE}.proto 到 C++ 源码..."
|
||||
)
|
||||
endforeach()
|
||||
|
||||
# 打包 gRPC 协议库
|
||||
add_library(${PROJECT_NAME}_grpc_lib SHARED ${PROTO_SRCS})
|
||||
target_include_directories(${PROJECT_NAME}_grpc_lib PUBLIC "${PROTO_OUT_DIR}")
|
||||
target_link_libraries(${PROJECT_NAME}_grpc_lib PkgConfig::GRPC protobuf::libprotobuf)
|
||||
|
||||
# ==========================================================
|
||||
# 4. 编译三大 Gateway 组件 (ROS 2 Plugins)
|
||||
# ==========================================================
|
||||
# 💡 定义一个宏来简化三个网关的编译代码，让 CMake 极度整洁！
|
||||
macro(build_gateway_component target_name source_file plugin_class)
|
||||
add_library(${target_name} SHARED ${source_file})
|
||||
target_include_directories(${target_name} PUBLIC
|
||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||
"$<BUILD_INTERFACE:${PROTO_OUT_DIR}>"
|
||||
)
|
||||
ament_target_dependencies(${target_name} rclcpp rclcpp_components rclcpp_action std_msgs std_srvs action_msgs)
|
||||
target_link_libraries(${target_name} ${PROJECT_NAME}_grpc_lib)
|
||||
|
||||
# 🚨 极度关键：因为接口在同一个包里，必须让 C++ 代码等待 Msg 编译完成后才能编译！
|
||||
rosidl_target_interfaces(${target_name} ${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
|
||||
rclcpp_components_register_node(${target_name}
|
||||
PLUGIN "${plugin_class}"
|
||||
EXECUTABLE ${target_name}_exe
|
||||
)
|
||||
endmacro()
|
||||
|
||||
# ⚠️ 注意：基于你截图的拼写 gatways。如果你改名了，记得同步改掉这里的路径！
|
||||
build_gateway_component(chassis_gateway_node "gateways/chassis_gateway_node.cpp" "win_ubuntu_bridge::ChassisGatewayNode")
|
||||
build_gateway_component(control_gateway_node "gateways/control_gateway_node.cpp" "win_ubuntu_bridge::ControlGatewayNode")
|
||||
build_gateway_component(sensor_gateway_node "gateways/sensor_gateway_node.cpp" "win_ubuntu_bridge::SensorGatewayNode")
|
||||
|
||||
# ==========================================================
|
||||
# 5. 编译其他主节点 (master_node 和 brain_node)
|
||||
# ==========================================================
|
||||
add_executable(master_node src/master_node.cpp)
|
||||
target_include_directories(master_node PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>" "${PROTO_OUT_DIR}")
|
||||
ament_target_dependencies(master_node rclcpp behaviortree_cpp_v3)
|
||||
target_link_libraries(master_node ${PROJECT_NAME}_grpc_lib)
|
||||
rosidl_target_interfaces(master_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
|
||||
add_executable(brain_node src/brain_node.cpp)
|
||||
target_include_directories(brain_node PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>" "${PROTO_OUT_DIR}")
|
||||
ament_target_dependencies(brain_node rclcpp)
|
||||
target_link_libraries(brain_node ${PROJECT_NAME}_grpc_lib)
|
||||
rosidl_target_interfaces(brain_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
|
||||
# ==========================================================
|
||||
# 6. 安装规则 (Install)
|
||||
# ==========================================================
|
||||
install(TARGETS
|
||||
${PROJECT_NAME}_grpc_lib
|
||||
chassis_gateway_node chassis_gateway_node_exe
|
||||
control_gateway_node control_gateway_node_exe
|
||||
sensor_gateway_node sensor_gateway_node_exe
|
||||
master_node brain_node
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib/${PROJECT_NAME}
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY proto/ DESTINATION share/${PROJECT_NAME}/proto)
|
||||
# 若有 launch 或 behavior_trees 文件，随时解开下面这行的注释
|
||||
# install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,15 @@
|
||||
# === [Goal] 行为树给网关下发的下载任务 ===
|
||||
int64 capture_timestamp_us # 刚才拿到的取件码
|
||||
string sensor_id # 例如 "cam_front"
|
||||
uint8 DATA_TYPE_IMAGE = 0
|
||||
uint8 DATA_TYPE_POINTCLOUD = 1
|
||||
uint8 data_type # 告诉网关下图片还是下点云
|
||||
string save_directory # 保存的 Ubuntu 目录，如 "/tmp/calib_data"
|
||||
---
|
||||
# === [Result] 网关下完后返回给行为树的结果 ===
|
||||
bool success
|
||||
string saved_file_path # 🚨 终极目的：返回存好的绝对路径 (如 /tmp/calib_data/cam_front_167888.png)
|
||||
string error_message
|
||||
---
|
||||
# === [Feedback] 网关实时汇报的下载进度 ===
|
||||
uint64 downloaded_bytes # 已下载的字节数 (供行为树监控是否卡死)
|
||||
@@ -0,0 +1,227 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
|
||||
// 🚨 引入自定义的 ROS 2 接口头文件 (由 win_ubuntu_bridge 包生成)
|
||||
#include "win_ubuntu_bridge/msg/hardware_state.hpp"
|
||||
#include "win_ubuntu_bridge/srv/set_diagnostic_mode.hpp"
|
||||
#include "win_ubuntu_bridge/srv/execute_raw_drive.hpp"
|
||||
#include "win_ubuntu_bridge/srv/execute_raw_steer.hpp"
|
||||
#include "win_ubuntu_bridge/srv/commit_kinematics.hpp"
|
||||
#include <std_srvs/srv/trigger.hpp> // 紧急刹车可以直接用标准库的 Trigger
|
||||
|
||||
// 引入 gRPC 自动生成的契约头文件
|
||||
#include <grpcpp/grpcpp.h>
|
||||
#include "agv_calib_chassis.grpc.pb.h"
|
||||
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
|
||||
using namespace agv::calibration::chassis;
|
||||
|
||||
namespace win_ubuntu_bridge {
|
||||
|
||||
class ChassisGatewayNode : public rclcpp::Node {
|
||||
public:
|
||||
explicit ChassisGatewayNode(const rclcpp::NodeOptions & options)
|
||||
: Node("chassis_gateway", options), stream_running_(false) {
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "🔌 底盘 gRPC 网关插件启动中... 正在挂载 6 大完整接口！");
|
||||
|
||||
// 1. 拨号连接 Windows 车端
|
||||
std::string target_ip = this->declare_parameter("agv_ip", "127.0.0.1:50051");
|
||||
channel_ = grpc::CreateChannel(target_ip, grpc::InsecureChannelCredentials());
|
||||
stub_ = AgvCalibChassisService::NewStub(channel_);
|
||||
|
||||
// ==========================================================
|
||||
// 2. 注册 5 个 ROS 2 服务 (拦截内部行为树的请求，翻译给车端)
|
||||
// ==========================================================
|
||||
|
||||
// 接口 1：夺权
|
||||
srv_set_mode_ = this->create_service<win_ubuntu_bridge::srv::SetDiagnosticMode>(
|
||||
"~/set_diagnostic_mode", std::bind(&ChassisGatewayNode::cb_set_mode, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
// 接口 2：最高优急停 (无参数，用 Trigger)
|
||||
srv_estop_ = this->create_service<std_srvs::srv::Trigger>(
|
||||
"~/hardware_emergency_brake", std::bind(&ChassisGatewayNode::cb_emergency_brake, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
// 接口 3：开环直行 (测轮径)
|
||||
srv_raw_drive_ = this->create_service<win_ubuntu_bridge::srv::ExecuteRawDrive>(
|
||||
"~/execute_raw_drive", std::bind(&ChassisGatewayNode::cb_raw_drive, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
// 接口 4：开环转向 (测死区/零位)
|
||||
srv_raw_steer_ = this->create_service<win_ubuntu_bridge::srv::ExecuteRawSteer>(
|
||||
"~/execute_raw_steer", std::bind(&ChassisGatewayNode::cb_raw_steer, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
// 接口 5：物理参数定稿固化
|
||||
srv_commit_kinematics_ = this->create_service<win_ubuntu_bridge::srv::CommitKinematics>(
|
||||
"~/commit_kinematics", std::bind(&ChassisGatewayNode::cb_commit_kinematics, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
// ==========================================================
|
||||
// 3. 注册 1 个 ROS 2 话题 (发布从车端抽上来的 50Hz 流数据)
|
||||
// ==========================================================
|
||||
pub_telemetry_ = this->create_publisher<win_ubuntu_bridge::msg::HardwareState>("~/hardware_telemetry", 50);
|
||||
|
||||
// 4. 启动异步抽水机线程
|
||||
stream_running_ = true;
|
||||
stream_thread_ = std::thread(&ChassisGatewayNode::grpc_stream_to_ros2_topic, this);
|
||||
}
|
||||
|
||||
~ChassisGatewayNode() {
|
||||
stream_running_ = false;
|
||||
if (stream_context_) stream_context_->TryCancel(); // 唤醒阻塞的 gRPC
|
||||
if (stream_thread_.joinable()) stream_thread_.join();
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<grpc::Channel> channel_;
|
||||
std::unique_ptr<AgvCalibChassisService::Stub> stub_;
|
||||
std::unique_ptr<grpc::ClientContext> stream_context_;
|
||||
|
||||
// ROS 2 接口句柄
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::SetDiagnosticMode>::SharedPtr srv_set_mode_;
|
||||
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_estop_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::ExecuteRawDrive>::SharedPtr srv_raw_drive_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::ExecuteRawSteer>::SharedPtr srv_raw_steer_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::CommitKinematics>::SharedPtr srv_commit_kinematics_;
|
||||
rclcpp::Publisher<win_ubuntu_bridge::msg::HardwareState>::SharedPtr pub_telemetry_;
|
||||
|
||||
std::thread stream_thread_;
|
||||
std::atomic<bool> stream_running_;
|
||||
|
||||
// ----------------------------------------------------------------------
|
||||
// ⬇️ 以下全是完美的 1:1 翻译逻辑 (ROS Request -> gRPC -> ROS Response) ⬇️
|
||||
// ----------------------------------------------------------------------
|
||||
|
||||
// [翻译] 接口 1: SetDiagnosticMode
|
||||
void cb_set_mode(const std::shared_ptr<win_ubuntu_bridge::srv::SetDiagnosticMode::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::SetDiagnosticMode::Response> res) {
|
||||
DiagnosticModeRequest grpc_req;
|
||||
grpc_req.set_target_mode(req->target_mode == 1 ? DiagnosticModeRequest::DIRECT_RAW_DRIVE : DiagnosticModeRequest::NORMAL_KINEMATICS);
|
||||
StandardResponse grpc_reply;
|
||||
grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
|
||||
grpc::Status status = stub_->SetDiagnosticMode(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success();
|
||||
res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// [翻译] 接口 2: HardwareEmergencyBrake
|
||||
void cb_emergency_brake(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
|
||||
std::shared_ptr<std_srvs::srv::Trigger::Response> res) {
|
||||
(void)req; // 忽略未使用变量
|
||||
Empty grpc_req; StandardResponse grpc_reply;
|
||||
grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(1)); // 急停超时设为1秒
|
||||
grpc::Status status = stub_->HardwareEmergencyBrake(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success();
|
||||
res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// [翻译] 接口 3: ExecuteRawDriveCommand
|
||||
void cb_raw_drive(const std::shared_ptr<win_ubuntu_bridge::srv::ExecuteRawDrive::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::ExecuteRawDrive::Response> res) {
|
||||
RawDriveRequest grpc_req;
|
||||
grpc_req.set_test_case_id(req->test_case_id);
|
||||
grpc_req.set_fl_motor_rpm(req->fl_motor_rpm);
|
||||
grpc_req.set_fr_motor_rpm(req->fr_motor_rpm);
|
||||
grpc_req.set_rl_motor_rpm(req->rl_motor_rpm);
|
||||
grpc_req.set_rr_motor_rpm(req->rr_motor_rpm);
|
||||
grpc_req.set_duration_sec(req->duration_sec);
|
||||
|
||||
StandardResponse grpc_reply;
|
||||
grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
|
||||
grpc::Status status = stub_->ExecuteRawDriveCommand(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success();
|
||||
res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// [翻译] 接口 4: ExecuteRawSteerCommand
|
||||
void cb_raw_steer(const std::shared_ptr<win_ubuntu_bridge::srv::ExecuteRawSteer::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::ExecuteRawSteer::Response> res) {
|
||||
RawSteerRequest grpc_req;
|
||||
grpc_req.set_test_case_id(req->test_case_id);
|
||||
grpc_req.set_front_steer_angle_deg(req->front_steer_angle_deg);
|
||||
grpc_req.set_rear_steer_angle_deg(req->rear_steer_angle_deg);
|
||||
grpc_req.set_duration_sec(req->duration_sec);
|
||||
|
||||
// 处理 proto 的 optional 字段 (依靠 ROS 2 传来的 bool 判断)
|
||||
if (req->has_sweep) {
|
||||
grpc_req.set_sweep_amplitude_deg(req->sweep_amplitude_deg);
|
||||
grpc_req.set_sweep_frequency_hz(req->sweep_frequency_hz);
|
||||
}
|
||||
|
||||
StandardResponse grpc_reply;
|
||||
grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
|
||||
grpc::Status status = stub_->ExecuteRawSteerCommand(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success();
|
||||
res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// [翻译] 接口 5: CommitKinematicParameters
|
||||
void cb_commit_kinematics(const std::shared_ptr<win_ubuntu_bridge::srv::CommitKinematics::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::CommitKinematics::Response> res) {
|
||||
KinematicParams grpc_req;
|
||||
// ROS 2 请求中依靠 bool 标志位来判断是否 set 进 grpc (实现真正的 optional)
|
||||
if (req->has_wheel_radius_fl) grpc_req.set_wheel_radius_fl_m(req->wheel_radius_fl_m);
|
||||
if (req->has_wheel_radius_fr) grpc_req.set_wheel_radius_fr_m(req->wheel_radius_fr_m);
|
||||
if (req->has_wheel_radius_rl) grpc_req.set_wheel_radius_rl_m(req->wheel_radius_rl_m);
|
||||
if (req->has_wheel_radius_rr) grpc_req.set_wheel_radius_rr_m(req->wheel_radius_rr_m);
|
||||
if (req->has_steer_zero_offset_front) grpc_req.set_steer_zero_offset_front_deg(req->steer_zero_offset_front_deg);
|
||||
if (req->has_steer_zero_offset_rear) grpc_req.set_steer_zero_offset_rear_deg(req->steer_zero_offset_rear_deg);
|
||||
if (req->has_effective_track_width) grpc_req.set_effective_track_width_m(req->effective_track_width_m);
|
||||
if (req->has_effective_wheel_base) grpc_req.set_effective_wheel_base_m(req->effective_wheel_base_m);
|
||||
if (req->has_icr_offset_x) grpc_req.set_icr_offset_x_m(req->icr_offset_x_m);
|
||||
if (req->has_icr_offset_y) grpc_req.set_icr_offset_y_m(req->icr_offset_y_m);
|
||||
|
||||
StandardResponse grpc_reply;
|
||||
grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(3));
|
||||
grpc::Status status = stub_->CommitKinematicParameters(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success();
|
||||
res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// ----------------------------------------------------------------------
|
||||
// 🌊 接口 6 [异步水龙头]: 后台读取 50Hz 流并发布 ROS 2 Topic
|
||||
// ----------------------------------------------------------------------
|
||||
void grpc_stream_to_ros2_topic() {
|
||||
while (stream_running_ && rclcpp::ok()) {
|
||||
stream_context_ = std::make_unique<grpc::ClientContext>();
|
||||
Empty grpc_req;
|
||||
|
||||
auto reader = stub_->StreamHardwareTelemetry(stream_context_.get(), grpc_req);
|
||||
win_ubuntu_bridge::msg::HardwareState ros_msg;
|
||||
HardwareState grpc_state;
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "🌊 车端底盘 50Hz 裸数据流连接已建立，开始广播 ROS 2 Topic...");
|
||||
|
||||
// 只要网不断，Read() 就会以 50Hz 的频率一直被触发
|
||||
while (stream_running_ && reader->Read(&grpc_state) && rclcpp::ok()) {
|
||||
// 完美 1:1 结构体拷贝
|
||||
ros_msg.hardware_timestamp_us = grpc_state.hardware_timestamp_us();
|
||||
ros_msg.encoder_ticks_fl = grpc_state.encoder_ticks_fl();
|
||||
ros_msg.encoder_ticks_fr = grpc_state.encoder_ticks_fr();
|
||||
ros_msg.encoder_ticks_rl = grpc_state.encoder_ticks_rl();
|
||||
ros_msg.encoder_ticks_rr = grpc_state.encoder_ticks_rr();
|
||||
ros_msg.actual_steer_angle_front_deg = grpc_state.actual_steer_angle_front_deg();
|
||||
ros_msg.actual_steer_angle_rear_deg = grpc_state.actual_steer_angle_rear_deg();
|
||||
ros_msg.current_fl_amp = grpc_state.current_fl_amp();
|
||||
ros_msg.current_fr_amp = grpc_state.current_fr_amp();
|
||||
ros_msg.current_rl_amp = grpc_state.current_rl_amp();
|
||||
ros_msg.current_rr_amp = grpc_state.current_rr_amp();
|
||||
ros_msg.current_steer_front_amp = grpc_state.current_steer_front_amp();
|
||||
ros_msg.driver_error_code = grpc_state.driver_error_code();
|
||||
|
||||
// 发布到 ROS 2 网络中！
|
||||
pub_telemetry_->publish(ros_msg);
|
||||
}
|
||||
|
||||
// 如果走到这里，说明网络断了或者发生了异常，稍等 1 秒后自动重试
|
||||
if (stream_running_) {
|
||||
RCLCPP_WARN(this->get_logger(), "⚠️ gRPC 到底端的流意外中断！尝试重连...");
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace agv_calib_core
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(win_ubuntu_bridge::ChassisGatewayNode)
|
||||
@@ -0,0 +1,235 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
|
||||
// 引入自定义的 ROS 2 接口头文件
|
||||
#include "win_ubuntu_bridge/msg/trajectory_point.hpp"
|
||||
#include "win_ubuntu_bridge/msg/control_telemetry.hpp"
|
||||
#include "win_ubuntu_bridge/srv/set_control_mode.hpp"
|
||||
#include "win_ubuntu_bridge/srv/execute_open_loop_cmd.hpp"
|
||||
#include "win_ubuntu_bridge/srv/follow_test_trajectory.hpp"
|
||||
#include "win_ubuntu_bridge/srv/execute_step_response.hpp"
|
||||
#include "win_ubuntu_bridge/srv/inject_tuning_params.hpp"
|
||||
#include <std_srvs/srv/trigger.hpp>
|
||||
|
||||
// 引入 gRPC 网络契约
|
||||
#include <grpcpp/grpcpp.h>
|
||||
#include "agv_calib_control.grpc.pb.h"
|
||||
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
|
||||
using namespace agv::calibration::control;
|
||||
|
||||
namespace win_ubuntu_bridge {
|
||||
|
||||
class ControlGatewayNode : public rclcpp::Node {
|
||||
public:
|
||||
explicit ControlGatewayNode(const rclcpp::NodeOptions & options)
|
||||
: Node("control_gateway", options), stream_running_(false) {
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "🧠 运控 gRPC 网关插件启动中... 正在挂载闭环调优 7 大接口！");
|
||||
|
||||
std::string target_ip = this->declare_parameter("agv_ip", "127.0.0.1:50051");
|
||||
channel_ = grpc::CreateChannel(target_ip, grpc::InsecureChannelCredentials());
|
||||
stub_ = AgvCalibControlService::NewStub(channel_);
|
||||
|
||||
// ==========================================================
|
||||
// 1. 挂载 7 个 ROS 2 翻译服务
|
||||
// ==========================================================
|
||||
srv_set_mode_ = this->create_service<win_ubuntu_bridge::srv::SetControlMode>(
|
||||
"~/set_control_mode", std::bind(&ControlGatewayNode::cb_set_mode, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
srv_estop_ = this->create_service<std_srvs::srv::Trigger>(
|
||||
"~/emergency_stop", std::bind(&ControlGatewayNode::cb_estop, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
srv_open_loop_ = this->create_service<win_ubuntu_bridge::srv::ExecuteOpenLoopCmd>(
|
||||
"~/execute_open_loop_cmd", std::bind(&ControlGatewayNode::cb_open_loop, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
srv_follow_traj_ = this->create_service<win_ubuntu_bridge::srv::FollowTestTrajectory>(
|
||||
"~/follow_test_trajectory", std::bind(&ControlGatewayNode::cb_follow_traj, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
srv_step_resp_ = this->create_service<win_ubuntu_bridge::srv::ExecuteStepResponse>(
|
||||
"~/execute_step_response", std::bind(&ControlGatewayNode::cb_step_resp, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
srv_inject_params_ = this->create_service<win_ubuntu_bridge::srv::InjectTuningParams>(
|
||||
"~/inject_tuning_params", std::bind(&ControlGatewayNode::cb_inject_params, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
srv_commit_params_ = this->create_service<std_srvs::srv::Trigger>(
|
||||
"~/commit_control_parameters", std::bind(&ControlGatewayNode::cb_commit_params, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
// ==========================================================
|
||||
// 2. 挂载 1 个 ROS 2 遥测发布者 (50Hz)
|
||||
// ==========================================================
|
||||
pub_telemetry_ = this->create_publisher<win_ubuntu_bridge::msg::ControlTelemetry>("~/control_telemetry", 50);
|
||||
|
||||
// 3. 启动独立推流抽水机线程
|
||||
stream_running_ = true;
|
||||
stream_thread_ = std::thread(&ControlGatewayNode::grpc_stream_to_ros2_topic, this);
|
||||
}
|
||||
|
||||
~ControlGatewayNode() {
|
||||
stream_running_ = false;
|
||||
if (stream_context_) stream_context_->TryCancel();
|
||||
if (stream_thread_.joinable()) stream_thread_.join();
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<grpc::Channel> channel_;
|
||||
std::unique_ptr<AgvCalibControlService::Stub> stub_;
|
||||
std::unique_ptr<grpc::ClientContext> stream_context_;
|
||||
|
||||
// Service & Publisher 句柄
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::SetControlMode>::SharedPtr srv_set_mode_;
|
||||
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_estop_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::ExecuteOpenLoopCmd>::SharedPtr srv_open_loop_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::FollowTestTrajectory>::SharedPtr srv_follow_traj_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::ExecuteStepResponse>::SharedPtr srv_step_resp_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::InjectTuningParams>::SharedPtr srv_inject_params_;
|
||||
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_commit_params_;
|
||||
|
||||
rclcpp::Publisher<win_ubuntu_bridge::msg::ControlTelemetry>::SharedPtr pub_telemetry_;
|
||||
|
||||
std::thread stream_thread_;
|
||||
std::atomic<bool> stream_running_;
|
||||
|
||||
// ----------------------------------------------------------------------
|
||||
// ⬇️ ROS 2 to gRPC 核心翻译逻辑 ⬇️
|
||||
// ----------------------------------------------------------------------
|
||||
|
||||
void cb_set_mode(const std::shared_ptr<win_ubuntu_bridge::srv::SetControlMode::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::SetControlMode::Response> res) {
|
||||
ModeRequest grpc_req;
|
||||
grpc_req.set_target_mode(static_cast<ModeRequest::Mode>(req->target_mode));
|
||||
StandardResponse grpc_reply;
|
||||
grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
|
||||
grpc::Status status = stub_->SetControlMode(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
void cb_estop(const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) {
|
||||
(void)req; Empty grpc_req; StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(1));
|
||||
grpc::Status status = stub_->EmergencyStop(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
void cb_open_loop(const std::shared_ptr<win_ubuntu_bridge::srv::ExecuteOpenLoopCmd::Request> req, std::shared_ptr<win_ubuntu_bridge::srv::ExecuteOpenLoopCmd::Response> res) {
|
||||
OpenLoopRequest grpc_req;
|
||||
grpc_req.set_left_motor_cmd(req->left_motor_cmd); grpc_req.set_right_motor_cmd(req->right_motor_cmd);
|
||||
grpc_req.set_steering_angle(req->steering_angle); grpc_req.set_duration_sec(req->duration_sec);
|
||||
StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
|
||||
grpc::Status status = stub_->ExecuteOpenLoopCmd(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// 🚨 核心难点 1：翻译不定长数组 (轨迹考卷下发) + 超长超时时间
|
||||
void cb_follow_traj(const std::shared_ptr<win_ubuntu_bridge::srv::FollowTestTrajectory::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::FollowTestTrajectory::Response> res) {
|
||||
TrajectoryRequest grpc_req;
|
||||
grpc_req.set_test_case_id(req->test_case_id);
|
||||
|
||||
// 遍历 ROS 2 的变长数组，依次 Add 进 gRPC 结构体 (使用 proto 的 add_ 方法分配内存)
|
||||
for (const auto& ros_pt : req->path) {
|
||||
TrajectoryPoint* grpc_pt = grpc_req.add_path();
|
||||
grpc_pt->set_x_m(ros_pt.x_m);
|
||||
grpc_pt->set_y_m(ros_pt.y_m);
|
||||
grpc_pt->set_yaw_rad(ros_pt.yaw_rad);
|
||||
grpc_pt->set_target_speed_ms(ros_pt.target_speed_ms);
|
||||
grpc_pt->set_curvature(ros_pt.curvature);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "正在下发包含 %zu 个轨迹点的考卷...", req->path.size());
|
||||
|
||||
StandardResponse grpc_reply;
|
||||
grpc::ClientContext context;
|
||||
|
||||
// 🚨 [架构师级防线]：车辆跑一圈可能需要 120 秒！这里的超时时间决不能像其他指令那样设为 2 秒，否则必死锁报错！
|
||||
context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(180));
|
||||
|
||||
grpc::Status status = stub_->FollowTestTrajectory(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
void cb_step_resp(const std::shared_ptr<win_ubuntu_bridge::srv::ExecuteStepResponse::Request> req, std::shared_ptr<win_ubuntu_bridge::srv::ExecuteStepResponse::Response> res) {
|
||||
StepResponseRequest grpc_req;
|
||||
grpc_req.set_target_velocity_ms(req->target_velocity_ms); grpc_req.set_duration_sec(req->duration_sec);
|
||||
StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(10)); // 阶跃可能耗时几秒
|
||||
grpc::Status status = stub_->ExecuteStepResponse(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// 🚨 核心难点 2：解决 Optional 热更新映射
|
||||
void cb_inject_params(const std::shared_ptr<win_ubuntu_bridge::srv::InjectTuningParams::Request> req, std::shared_ptr<win_ubuntu_bridge::srv::InjectTuningParams::Response> res) {
|
||||
ControlParams grpc_req;
|
||||
|
||||
// 只有当 ROS 2 里的 has_ 标志位为 true 时，才向 gRPC 里 set 值
|
||||
if (req->has_wheel_radius_left_ratio) grpc_req.set_wheel_radius_left_ratio(req->wheel_radius_left_ratio);
|
||||
if (req->has_wheel_radius_right_ratio) grpc_req.set_wheel_radius_right_ratio(req->wheel_radius_right_ratio);
|
||||
if (req->has_effective_track_width_m) grpc_req.set_effective_track_width_m(req->effective_track_width_m);
|
||||
if (req->has_steering_zero_offset_deg) grpc_req.set_steering_zero_offset_deg(req->steering_zero_offset_deg);
|
||||
|
||||
if (req->has_pid_kp_lateral) grpc_req.set_pid_kp_lateral(req->pid_kp_lateral);
|
||||
if (req->has_pid_ki_lateral) grpc_req.set_pid_ki_lateral(req->pid_ki_lateral);
|
||||
if (req->has_pid_kd_lateral) grpc_req.set_pid_kd_lateral(req->pid_kd_lateral);
|
||||
|
||||
if (req->has_pid_kp_heading) grpc_req.set_pid_kp_heading(req->pid_kp_heading);
|
||||
if (req->has_pid_ki_heading) grpc_req.set_pid_ki_heading(req->pid_ki_heading);
|
||||
if (req->has_pid_kd_heading) grpc_req.set_pid_kd_heading(req->pid_kd_heading);
|
||||
|
||||
if (req->has_pure_pursuit_lookahead_m) grpc_req.set_pure_pursuit_lookahead_m(req->pure_pursuit_lookahead_m);
|
||||
if (req->has_mpc_weight_q_lateral) grpc_req.set_mpc_weight_q_lateral(req->mpc_weight_q_lateral);
|
||||
if (req->has_mpc_weight_r_steering) grpc_req.set_mpc_weight_r_steering(req->mpc_weight_r_steering);
|
||||
|
||||
StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(2));
|
||||
grpc::Status status = stub_->InjectTuningParameters(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
void cb_commit_params(const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) {
|
||||
(void)req; Empty grpc_req; StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(3));
|
||||
grpc::Status status = stub_->CommitControlParameters(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// ==========================================================
|
||||
// 🌊 异步抽水机：读取 50Hz 运控状态 -> 转化为 ROS 2 Topic
|
||||
// ==========================================================
|
||||
void grpc_stream_to_ros2_topic() {
|
||||
while (stream_running_ && rclcpp::ok()) {
|
||||
stream_context_ = std::make_unique<grpc::ClientContext>();
|
||||
Empty grpc_req;
|
||||
auto reader = stub_->StreamTelemetry(stream_context_.get(), grpc_req);
|
||||
|
||||
win_ubuntu_bridge::msg::ControlTelemetry ros_msg;
|
||||
TelemetryData grpc_state;
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "🌊 车端运控 50Hz ODOM 与体感数据流已建立，开始广播...");
|
||||
|
||||
while (stream_running_ && reader->Read(&grpc_state) && rclcpp::ok()) {
|
||||
// 将 gRPC Odom 与速度赋值给 ROS msg
|
||||
ros_msg.hardware_timestamp_us = grpc_state.hardware_timestamp_us();
|
||||
ros_msg.odom_x_m = grpc_state.odom_x_m();
|
||||
ros_msg.odom_y_m = grpc_state.odom_y_m();
|
||||
ros_msg.odom_yaw_rad = grpc_state.odom_yaw_rad();
|
||||
ros_msg.feedback_linear_vel_ms = grpc_state.feedback_linear_vel_ms();
|
||||
ros_msg.feedback_angular_vel_rads = grpc_state.feedback_angular_vel_rads();
|
||||
ros_msg.left_motor_current_amp = grpc_state.left_motor_current_amp();
|
||||
ros_msg.right_motor_current_amp = grpc_state.right_motor_current_amp();
|
||||
ros_msg.steering_motor_current_amp = grpc_state.steering_motor_current_amp();
|
||||
ros_msg.cmd_steering_output = grpc_state.cmd_steering_output();
|
||||
|
||||
// 广播供行为树和 AI 节点白嫖！
|
||||
pub_telemetry_->publish(ros_msg);
|
||||
}
|
||||
|
||||
if (stream_running_) {
|
||||
RCLCPP_WARN(this->get_logger(), "⚠️ 运控推流中断！1秒后重连...");
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace agv_calib_core
|
||||
|
||||
// 🚨 注册为 ROS 2 组件
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(win_ubuntu_bridge::ControlGatewayNode)
|
||||
@@ -0,0 +1,230 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
// 引入自定义 ROS 2 接口
|
||||
#include "win_ubuntu_bridge/msg/camera_intrinsic.hpp"
|
||||
#include "win_ubuntu_bridge/msg/sensor_extrinsic.hpp"
|
||||
#include "win_ubuntu_bridge/srv/move_to_observation_pose.hpp"
|
||||
#include "win_ubuntu_bridge/srv/trigger_sync_capture.hpp"
|
||||
#include "win_ubuntu_bridge/srv/commit_calibration_results.hpp"
|
||||
#include "win_ubuntu_bridge/action/download_sensor_data.hpp"
|
||||
|
||||
// 引入 gRPC 契约
|
||||
#include <grpcpp/grpcpp.h>
|
||||
#include "agv_calib_sensor.grpc.pb.h"
|
||||
|
||||
// 文件 I/O 与多线程
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
#include <thread>
|
||||
|
||||
using namespace agv::calibration::sensor;
|
||||
namespace fs = std::filesystem;
|
||||
|
||||
namespace win_ubuntu_bridge {
|
||||
|
||||
class SensorGatewayNode : public rclcpp::Node {
|
||||
public:
|
||||
using DownloadAction = win_ubuntu_bridge::action::DownloadSensorData;
|
||||
using GoalHandleDownload = rclcpp_action::ServerGoalHandle<DownloadAction>;
|
||||
|
||||
explicit SensorGatewayNode(const rclcpp::NodeOptions & options)
|
||||
: Node("sensor_gateway", options) {
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "📷 传感器 gRPC 网关插件启动中... 准备接管大文件流式拉取引擎！");
|
||||
|
||||
std::string target_ip = this->declare_parameter("agv_ip", "127.0.0.1:50051");
|
||||
channel_ = grpc::CreateChannel(target_ip, grpc::InsecureChannelCredentials());
|
||||
stub_ = SensorCalibrationService::NewStub(channel_);
|
||||
|
||||
// 1. 挂载 3 个瞬间动作服务 (Service)
|
||||
srv_move_ = this->create_service<win_ubuntu_bridge::srv::MoveToObservationPose>(
|
||||
"~/move_to_pose", std::bind(&SensorGatewayNode::cb_move, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
srv_trigger_ = this->create_service<win_ubuntu_bridge::srv::TriggerSyncCapture>(
|
||||
"~/trigger_sync_capture", std::bind(&SensorGatewayNode::cb_trigger, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
srv_commit_ = this->create_service<win_ubuntu_bridge::srv::CommitCalibrationResults>(
|
||||
"~/commit_calibration_results", std::bind(&SensorGatewayNode::cb_commit, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
// 2. 🌟 挂载 1 个耗时动作服务器 (Action Server) —— 专治大文件流
|
||||
action_download_ = rclcpp_action::create_server<DownloadAction>(
|
||||
this,
|
||||
"~/download_sensor_data",
|
||||
std::bind(&SensorGatewayNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
|
||||
std::bind(&SensorGatewayNode::handle_cancel, this, std::placeholders::_1),
|
||||
std::bind(&SensorGatewayNode::handle_accepted, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<grpc::Channel> channel_;
|
||||
std::unique_ptr<SensorCalibrationService::Stub> stub_;
|
||||
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::MoveToObservationPose>::SharedPtr srv_move_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::TriggerSyncCapture>::SharedPtr srv_trigger_;
|
||||
rclcpp::Service<win_ubuntu_bridge::srv::CommitCalibrationResults>::SharedPtr srv_commit_;
|
||||
rclcpp_action::Server<DownloadAction>::SharedPtr action_download_;
|
||||
|
||||
// ==========================================================
|
||||
// 瞬时服务翻译 (Service)
|
||||
// ==========================================================
|
||||
void cb_move(const std::shared_ptr<win_ubuntu_bridge::srv::MoveToObservationPose::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::MoveToObservationPose::Response> res) {
|
||||
PoseRequest grpc_req;
|
||||
grpc_req.set_target_x_m(req->target_x_m); grpc_req.set_target_y_m(req->target_y_m);
|
||||
grpc_req.set_target_yaw_deg(req->target_yaw_deg); grpc_req.set_is_relative(req->is_relative);
|
||||
StandardResponse grpc_reply; grpc::ClientContext context;
|
||||
context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(60)); // 走位可能需要较长时间
|
||||
grpc::Status status = stub_->MoveToObservationPose(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
void cb_trigger(const std::shared_ptr<win_ubuntu_bridge::srv::TriggerSyncCapture::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::TriggerSyncCapture::Response> res) {
|
||||
CaptureRequest grpc_req;
|
||||
for (const auto& id : req->sensor_ids) { grpc_req.add_sensor_ids(id); }
|
||||
CaptureResponse grpc_reply; grpc::ClientContext context;
|
||||
context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(3));
|
||||
grpc::Status status = stub_->TriggerSyncCapture(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success();
|
||||
res->capture_timestamp_us = grpc_reply.capture_timestamp_us(); // 🚨 取回取件码
|
||||
res->error_message = status.ok() ? grpc_reply.error_message() : status.error_message();
|
||||
}
|
||||
|
||||
void cb_commit(const std::shared_ptr<win_ubuntu_bridge::srv::CommitCalibrationResults::Request> req,
|
||||
std::shared_ptr<win_ubuntu_bridge::srv::CommitCalibrationResults::Response> res) {
|
||||
CalibrationPayload grpc_req;
|
||||
grpc_req.set_task_id(req->task_id);
|
||||
|
||||
// 映射结构体数组
|
||||
for (const auto& ros_in : req->updated_intrinsics) {
|
||||
auto* grpc_in = grpc_req.add_updated_intrinsics();
|
||||
grpc_in->set_camera_id(ros_in.camera_id); grpc_in->set_fx(ros_in.fx); grpc_in->set_fy(ros_in.fy);
|
||||
grpc_in->set_cx(ros_in.cx); grpc_in->set_cy(ros_in.cy);
|
||||
for (auto d : ros_in.dist_coeffs) grpc_in->add_dist_coeffs(d);
|
||||
}
|
||||
for (const auto& ros_ex : req->updated_extrinsics) {
|
||||
auto* grpc_ex = grpc_req.add_updated_extrinsics();
|
||||
grpc_ex->set_source_frame(ros_ex.source_frame); grpc_ex->set_target_frame(ros_ex.target_frame);
|
||||
grpc_ex->set_trans_x_mm(ros_ex.trans_x_mm); grpc_ex->set_trans_y_mm(ros_ex.trans_y_mm); grpc_ex->set_trans_z_mm(ros_ex.trans_z_mm);
|
||||
grpc_ex->set_roll_deg(ros_ex.roll_deg); grpc_ex->set_pitch_deg(ros_ex.pitch_deg); grpc_ex->set_yaw_deg(ros_ex.yaw_deg);
|
||||
}
|
||||
|
||||
StandardResponse grpc_reply; grpc::ClientContext context; context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(5));
|
||||
grpc::Status status = stub_->CommitCalibrationResults(&context, grpc_req, &grpc_reply);
|
||||
res->success = status.ok() && grpc_reply.success(); res->message = status.ok() ? grpc_reply.message() : status.error_message();
|
||||
}
|
||||
|
||||
// ==========================================================
|
||||
// 🚨 灵魂逻辑：大文件流式下载 转换为 ROS 2 Action 落盘
|
||||
// ==========================================================
|
||||
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const DownloadAction::Goal> goal) {
|
||||
(void)uuid;
|
||||
RCLCPP_INFO(this->get_logger(), "📥 收到大文件下载工单! 凭证: %ld, 传感器: %s", goal->capture_timestamp_us, goal->sensor_id.c_str());
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleDownload> goal_handle) {
|
||||
(void)goal_handle;
|
||||
RCLCPP_WARN(this->get_logger(), "🛑 行为树请求强行取消下载");
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void handle_accepted(const std::shared_ptr<GoalHandleDownload> goal_handle) {
|
||||
// 🚨 必须在独立线程中执行阻塞的网络流读取，否则会死锁整个 ROS 2 容器！
|
||||
std::thread{std::bind(&SensorGatewayNode::execute_download, this, std::placeholders::_1), goal_handle}.detach();
|
||||
}
|
||||
|
||||
// 后台真实下载线程
|
||||
void execute_download(const std::shared_ptr<GoalHandleDownload> goal_handle) {
|
||||
const auto goal = goal_handle->get_goal();
|
||||
auto result = std::make_shared<DownloadAction::Result>();
|
||||
auto feedback = std::make_shared<DownloadAction::Feedback>();
|
||||
|
||||
DataFetchRequest grpc_req;
|
||||
grpc_req.set_capture_timestamp_us(goal->capture_timestamp_us);
|
||||
grpc_req.set_sensor_id(goal->sensor_id);
|
||||
|
||||
grpc::ClientContext context;
|
||||
context.set_deadline(std::chrono::system_clock::now() + std::chrono::seconds(60)); // 大文件给足 60 秒传输时间
|
||||
|
||||
std::unique_ptr<grpc::ClientReader<FileChunk>> reader;
|
||||
if (goal->data_type == DownloadAction::Goal::DATA_TYPE_IMAGE) {
|
||||
reader = stub_->DownloadImage(&context, grpc_req);
|
||||
} else {
|
||||
reader = stub_->DownloadPointCloud(&context, grpc_req);
|
||||
}
|
||||
|
||||
FileChunk chunk;
|
||||
std::ofstream outfile;
|
||||
std::string final_file_path;
|
||||
uint64_t total_bytes = 0;
|
||||
bool is_first_chunk = true;
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "🚀 开始从车端流式拉取传感器大文件...");
|
||||
|
||||
// 🌊 像蚂蚁搬家一样，收到一块，就用二进制追加写入硬盘一块！
|
||||
while (reader->Read(&chunk) && rclcpp::ok()) {
|
||||
|
||||
// 1. 检查行为树是否发出了取消指令
|
||||
if (goal_handle->is_canceling()) {
|
||||
context.TryCancel(); // 物理打断 gRPC 传输
|
||||
if (outfile.is_open()) outfile.close();
|
||||
if (!final_file_path.empty() && fs::exists(final_file_path)) fs::remove(final_file_path); // 删掉下了一半的残次品
|
||||
result->success = false; result->error_message = "下载被系统强制中止";
|
||||
goal_handle->canceled(result);
|
||||
return;
|
||||
}
|
||||
|
||||
// 2. 如果是第一包数据，创建目录并打开硬盘文件
|
||||
if (is_first_chunk) {
|
||||
if (!fs::exists(goal->save_directory)) { fs::create_directories(goal->save_directory); }
|
||||
|
||||
std::string ext = chunk.format_ext().empty() ? "bin" : chunk.format_ext(); // 动态获取文件后缀
|
||||
final_file_path = goal->save_directory + "/" + goal->sensor_id + "_" + std::to_string(goal->capture_timestamp_us) + "." + ext;
|
||||
|
||||
// 🚨 以纯二进制 (binary) 和截断清空 (trunc) 模式打开硬盘文件！
|
||||
outfile.open(final_file_path, std::ios::out | std::ios::binary | std::ios::trunc);
|
||||
if (!outfile.is_open()) {
|
||||
result->success = false; result->error_message = "无法在 Ubuntu 硬盘创建文件！权限不足？";
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
is_first_chunk = false;
|
||||
}
|
||||
|
||||
// 3. 把网络收到的内存碎块直接写入硬盘！(内存占用极低，绝对不会爆内存)
|
||||
outfile.write(chunk.chunk_data().data(), chunk.chunk_data().size());
|
||||
total_bytes += chunk.chunk_data().size();
|
||||
|
||||
// 4. 每收到一块，通过 Action Feedback 汇报一次进度
|
||||
feedback->downloaded_bytes = total_bytes;
|
||||
goal_handle->publish_feedback(feedback);
|
||||
|
||||
if (chunk.is_last_chunk()) break;
|
||||
}
|
||||
|
||||
grpc::Status status = reader->Finish();
|
||||
if (outfile.is_open()) outfile.close();
|
||||
|
||||
// 5. 善后汇报
|
||||
if (status.ok() && !is_first_chunk) {
|
||||
RCLCPP_INFO(this->get_logger(), "✅ 下载落盘成功！共计 %.2f MB -> %s", total_bytes / 1024.0 / 1024.0, final_file_path.c_str());
|
||||
result->success = true;
|
||||
result->saved_file_path = final_file_path; // 🚨 核心：将绝对路径交还给行为树！
|
||||
goal_handle->succeed(result);
|
||||
} else {
|
||||
if (!final_file_path.empty() && fs::exists(final_file_path)) fs::remove(final_file_path); // 失败清理垃圾
|
||||
RCLCPP_ERROR(this->get_logger(), "❌ 下载大文件失败: %s", status.error_message().c_str());
|
||||
result->success = false;
|
||||
result->error_message = "网络传输中断: " + status.error_message();
|
||||
goal_handle->abort(result);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace agv_calib_core
|
||||
|
||||
// 注册为 ROS 2 组件
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(win_ubuntu_bridge::SensorGatewayNode)
|
||||
@@ -0,0 +1,6 @@
|
||||
string camera_id
|
||||
float64 fx
|
||||
float64 fy
|
||||
float64 cx
|
||||
float64 cy
|
||||
float64[] dist_coeffs
|
||||
@@ -0,0 +1,10 @@
|
||||
int64 hardware_timestamp_us
|
||||
float64 odom_x_m
|
||||
float64 odom_y_m
|
||||
float64 odom_yaw_rad
|
||||
float64 feedback_linear_vel_ms
|
||||
float64 feedback_angular_vel_rads
|
||||
float64 left_motor_current_amp
|
||||
float64 right_motor_current_amp
|
||||
float64 steering_motor_current_amp
|
||||
float64 cmd_steering_output
|
||||
@@ -0,0 +1,13 @@
|
||||
int64 hardware_timestamp_us
|
||||
int64 encoder_ticks_fl
|
||||
int64 encoder_ticks_fr
|
||||
int64 encoder_ticks_rl
|
||||
int64 encoder_ticks_rr
|
||||
float64 actual_steer_angle_front_deg
|
||||
float64 actual_steer_angle_rear_deg
|
||||
float64 current_fl_amp
|
||||
float64 current_fr_amp
|
||||
float64 current_rl_amp
|
||||
float64 current_rr_amp
|
||||
float64 current_steer_front_amp
|
||||
uint32 driver_error_code
|
||||
@@ -0,0 +1,8 @@
|
||||
string source_frame
|
||||
string target_frame
|
||||
float64 trans_x_mm
|
||||
float64 trans_y_mm
|
||||
float64 trans_z_mm
|
||||
float64 roll_deg
|
||||
float64 pitch_deg
|
||||
float64 yaw_deg
|
||||
@@ -0,0 +1,5 @@
|
||||
float64 x_m
|
||||
float64 y_m
|
||||
float64 yaw_rad
|
||||
float64 target_speed_ms
|
||||
float64 curvature
|
||||
@@ -0,0 +1,28 @@
|
||||
<?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>win_ubuntu_bridge</name>
|
||||
<version>1.0.0</version>
|
||||
<description>AGV 自动化标定车间跨平台网关与总控核心</description>
|
||||
<maintainer email="2469171725@qq.com">nvidia</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>action_msgs</depend>
|
||||
<depend>behaviortree_cpp_v3</depend> <test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+1
-1
@@ -18,7 +18,7 @@ service SensorCalibrationService {
|
||||
// 🚙 [Windows 返回 -> Linux]：物理到位抱死刹车后，返回成功回执
|
||||
// 🚨 业务潜台词：Linux 收到回执后，必须在代码里强制 sleep(0.5s) 等待避震悬挂平息，冻结物理空间！
|
||||
rpc MoveToObservationPose (PoseRequest) returns (StandardResponse);
|
||||
|
||||
|
||||
// ---------------------------------------------------------
|
||||
// 第二步：防延迟同步锁存 (停与拍)
|
||||
// ---------------------------------------------------------
|
||||
@@ -0,0 +1,12 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = rclcpp::Node::make_shared("master_node");
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "🚀 AGV 标定主控大脑编译成功，gRPC 契约已挂载！");
|
||||
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,6 @@
|
||||
string task_id
|
||||
win_ubuntu_bridge/CameraIntrinsic[] updated_intrinsics
|
||||
win_ubuntu_bridge/SensorExtrinsic[] updated_extrinsics
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,26 @@
|
||||
bool has_wheel_radius_fl
|
||||
float64 wheel_radius_fl_m
|
||||
bool has_wheel_radius_fr
|
||||
float64 wheel_radius_fr_m
|
||||
bool has_wheel_radius_rl
|
||||
float64 wheel_radius_rl_m
|
||||
bool has_wheel_radius_rr
|
||||
float64 wheel_radius_rr_m
|
||||
|
||||
bool has_steer_zero_offset_front
|
||||
float64 steer_zero_offset_front_deg
|
||||
bool has_steer_zero_offset_rear
|
||||
float64 steer_zero_offset_rear_deg
|
||||
|
||||
bool has_effective_track_width
|
||||
float64 effective_track_width_m
|
||||
bool has_effective_wheel_base
|
||||
float64 effective_wheel_base_m
|
||||
|
||||
bool has_icr_offset_x
|
||||
float64 icr_offset_x_m
|
||||
bool has_icr_offset_y
|
||||
float64 icr_offset_y_m
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,7 @@
|
||||
float64 left_motor_cmd
|
||||
float64 right_motor_cmd
|
||||
float64 steering_angle
|
||||
float64 duration_sec
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,9 @@
|
||||
string test_case_id
|
||||
float64 fl_motor_rpm
|
||||
float64 fr_motor_rpm
|
||||
float64 rl_motor_rpm
|
||||
float64 rr_motor_rpm
|
||||
float64 duration_sec
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,10 @@
|
||||
string test_case_id
|
||||
float64 front_steer_angle_deg
|
||||
float64 rear_steer_angle_deg
|
||||
bool has_sweep
|
||||
float64 sweep_amplitude_deg
|
||||
float64 sweep_frequency_hz
|
||||
float64 duration_sec
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,5 @@
|
||||
float64 target_velocity_ms
|
||||
float64 duration_sec
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,5 @@
|
||||
string test_case_id
|
||||
win_ubuntu_bridge/TrajectoryPoint[] path
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,32 @@
|
||||
bool has_wheel_radius_left_ratio
|
||||
float64 wheel_radius_left_ratio
|
||||
bool has_wheel_radius_right_ratio
|
||||
float64 wheel_radius_right_ratio
|
||||
bool has_effective_track_width_m
|
||||
float64 effective_track_width_m
|
||||
bool has_steering_zero_offset_deg
|
||||
float64 steering_zero_offset_deg
|
||||
|
||||
bool has_pid_kp_lateral
|
||||
float64 pid_kp_lateral
|
||||
bool has_pid_ki_lateral
|
||||
float64 pid_ki_lateral
|
||||
bool has_pid_kd_lateral
|
||||
float64 pid_kd_lateral
|
||||
|
||||
bool has_pid_kp_heading
|
||||
float64 pid_kp_heading
|
||||
bool has_pid_ki_heading
|
||||
float64 pid_ki_heading
|
||||
bool has_pid_kd_heading
|
||||
float64 pid_kd_heading
|
||||
|
||||
bool has_pure_pursuit_lookahead_m
|
||||
float64 pure_pursuit_lookahead_m
|
||||
bool has_mpc_weight_q_lateral
|
||||
float64 mpc_weight_q_lateral
|
||||
bool has_mpc_weight_r_steering
|
||||
float64 mpc_weight_r_steering
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,7 @@
|
||||
float64 target_x_m
|
||||
float64 target_y_m
|
||||
float64 target_yaw_deg
|
||||
bool is_relative
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,4 @@
|
||||
uint8 target_mode # 0: NORMAL, 1: OPEN_LOOP, 2: TUNING_MODE
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,4 @@
|
||||
uint8 target_mode # 0: NORMAL_KINEMATICS, 1: DIRECT_RAW_DRIVE
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -0,0 +1,5 @@
|
||||
string[] sensor_ids
|
||||
---
|
||||
bool success
|
||||
int64 capture_timestamp_us # 🚨 极其关键的“取件码”
|
||||
string error_message
|
||||
@@ -0,0 +1,76 @@
|
||||
===================================================================================================
|
||||
[?????????] (??????????) [???Ubuntu ???? (?)] [???Windows??] (????)
|
||||
===================================================================================================
|
||||
|
||||
ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿
|
||||
³ ?? ??????? ³ <--- main_pipeline.xml
|
||||
³ BehaviorTree.CPP ?? ³ (???????????)
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÂÄÄÄÄÄÄÄÄÄÄÄÄÄÙ
|
||||
³
|
||||
ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿ ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÁÄÄÄÄÄÄÄÄÄÄÄÄÄ¿ ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿
|
||||
³ ?? 1.???????? ³ ³ ?? BT ????? (C++) ³ ³ ?? AGV ???? ³
|
||||
³ (?????????) ³<~~~~~³ - ?????? API ³------->³ (agv_calib_chassis) ³
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ API 1³ - ?????? API ³ gRPC 1 ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ
|
||||
(ROS 2 Action Server) ³ - ???????? API ³
|
||||
³ ³
|
||||
ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿ ³ ³ ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿
|
||||
³ ?? 2.???????? ³ ³ ³ ³ ?? AGV ???? ³
|
||||
³ (?????? PID) ³<~~~~~³ ------------------------- ³------->³ (agv_calib_control) ³
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ API 2³ ³ gRPC 2 ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ
|
||||
(ROS 2 Action Server) ³ ??? ??????? (/tmp/) ³
|
||||
³ - ??? .csv ???? ³
|
||||
ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿ ³ - ??? .png / .pcd ³ ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿
|
||||
³ ?? 3.???????? ³ ³ ³ ³ ?? AGV ????? ³
|
||||
³ (Ceres ? 4x4 ??) ³<~~~~~³ ------------------------- ³------->³ (sensor_calibration) ³
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ API 3ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ gRPC 3 ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ
|
||||
(ROS 2 Action Server)
|
||||
|
||||
===================================================================================================
|
||||
|
||||
|
||||
=====================================================================================================
|
||||
[??? ROS 2 ?? (?? & ???)] [??? ROS 2 ???? (??????)] [????? (gRPC)]
|
||||
=====================================================================================================
|
||||
|
||||
ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿ ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿
|
||||
³ ?? ???????³ ³ ?? ChassisGatewayComponent ³ [Wi-Fi 6]
|
||||
³ (?? ROS 2 Srv) ³<-- ROS Srv -->³ (?? agv_calib_chassis.proto) ³<--- gRPC ---> ?? ????
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ ³ ?????????? gRPC ?? ³
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ
|
||||
ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿ ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿
|
||||
³ ?? ?????? ³<-- ROS Topic--³ ?? ControlGatewayComponent ³ [Wi-Fi 6]
|
||||
³ (???? Topic) ³ ³ (?? agv_calib_control.proto) ³<--- gRPC ---> ?? ????
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ<-- ROS Action-³ ?????? gRPC ??? Topic ³
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ
|
||||
ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿ ÚÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄ¿
|
||||
³ ?? ?????? ³ ³ ?? SensorGatewayComponent ³ [Wi-Fi 6]
|
||||
³ (???????) ³<-- ROS Action-³ (?? sensor_calibration.proto) ³<--- gRPC ---> ?? ???
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ ³ ?????????Action????³
|
||||
ÀÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÄÙ
|
||||
=====================================================================================================
|
||||
|
||||
[?? Windows ????]
|
||||
³
|
||||
³ (C++ ?????? Ticks ? ??)
|
||||
?
|
||||
[?? Windows gRPC Server]
|
||||
³
|
||||
³ (? Wi-Fi 6 ? 50Hz Stream ???????)
|
||||
?
|
||||
==================== ?????? ====================
|
||||
³
|
||||
[?? Linux ChassisGatewayNode (???????)]
|
||||
³ (?? gRPC Client ?? reader->Read() ???)
|
||||
³
|
||||
³ (1:1 ?????? ROS 2 ? msg ???)
|
||||
?
|
||||
[?? Linux ChassisGatewayNode (ROS 2 Publisher)]
|
||||
³
|
||||
³ (?? pub_telemetry_->publish() ? ROS 2 ????)
|
||||
?
|
||||
================= ROS 2 ?????? =================
|
||||
³
|
||||
ÃÄ? [?? ????????] (?? Subscriber ?????????)
|
||||
ÃÄ? [?? ????????] (?? Subscriber ??????? > 20A ????)
|
||||
ÃÄ? [?? ???????] (?? Subscriber ?????????)
|
||||
ÀÄ? [?? ROS Bag ???] (????????? 50Hz ????????)
|
||||
Reference in New Issue
Block a user