diff --git a/agv_calib_brain/src/agv_calib_control_pb2.py b/agv_calib_brain/src/agv_calib_control_pb2.py new file mode 100644 index 0000000..edaf732 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_control_pb2.py @@ -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) diff --git a/agv_calib_brain/src/agv_calib_control_pb2_grpc.py b/agv_calib_brain/src/agv_calib_control_pb2_grpc.py new file mode 100644 index 0000000..5dd44cb --- /dev/null +++ b/agv_calib_brain/src/agv_calib_control_pb2_grpc.py @@ -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) diff --git a/agv_calib_brain/src/agv_calib_core/CMakeLists.txt b/agv_calib_brain/src/agv_calib_core/CMakeLists.txt index 5d02b51..72631e0 100644 --- a/agv_calib_brain/src/agv_calib_core/CMakeLists.txt +++ b/agv_calib_brain/src/agv_calib_core/CMakeLists.txt @@ -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() \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/behavior_trees/main_pipeline.xml b/agv_calib_brain/src/agv_calib_core/behavior_trees/main_pipeline.xml new file mode 100644 index 0000000..fce81fc --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/behavior_trees/main_pipeline.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/agv_calib_brain/src/agv_calib_core/package.xml b/agv_calib_brain/src/agv_calib_core/package.xml index a227b83..89b1683 100644 --- a/agv_calib_brain/src/agv_calib_core/package.xml +++ b/agv_calib_brain/src/agv_calib_core/package.xml @@ -9,6 +9,10 @@ ament_cmake + rclcpp + behaviortree_cpp_v3 + ament_index_cpp + ament_lint_auto ament_lint_common diff --git a/agv_calib_brain/src/agv_calib_core/src/bt_nodes/dummy_nodes.hpp b/agv_calib_brain/src/agv_calib_core/src/bt_nodes/dummy_nodes.hpp new file mode 100644 index 0000000..d41935d --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/src/bt_nodes/dummy_nodes.hpp @@ -0,0 +1,82 @@ +#pragma once +#include +#include +#include +#include + +// 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; + } +}; \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/src/bt_nodes/real_grpc_nodes.hpp b/agv_calib_brain/src/agv_calib_core/src/bt_nodes/real_grpc_nodes.hpp new file mode 100644 index 0000000..d082732 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/src/bt_nodes/real_grpc_nodes.hpp @@ -0,0 +1,54 @@ +#pragma once +#include +#include +// 引入 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 channel_; + std::unique_ptr stub_; +}; \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/src/master_node.cpp b/agv_calib_brain/src/agv_calib_core/src/master_node.cpp index f637f03..6b6db01 100644 --- a/agv_calib_brain/src/agv_calib_core/src/master_node.cpp +++ b/agv_calib_brain/src/agv_calib_core/src/master_node.cpp @@ -1,12 +1,44 @@ #include +#include +#include + +// 引入刚才写的假节点 +#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"); + factory.registerNodeType("MockCallChassisAlgo"); + factory.registerNodeType("MockTuneControl"); + factory.registerNodeType("MockCallControlAlgo"); + factory.registerNodeType("MockMoveAndCapture"); + factory.registerNodeType("MockCallSensorAlgo"); + factory.registerNodeType("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; } \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2.py b/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2.py new file mode 100644 index 0000000..edaf732 --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2.py @@ -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) diff --git a/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2_grpc.py b/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2_grpc.py new file mode 100644 index 0000000..5dd44cb --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/test/agv_calib_control_pb2_grpc.py @@ -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) diff --git a/agv_calib_brain/src/agv_calib_core/test/mock_agv_server.py b/agv_calib_brain/src/agv_calib_core/test/mock_agv_server.py new file mode 100644 index 0000000..b7f04bd --- /dev/null +++ b/agv_calib_brain/src/agv_calib_core/test/mock_agv_server.py @@ -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() \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/CMakeLists.txt b/agv_calib_brain/src/win_ubuntu_bridge/CMakeLists.txt new file mode 100644 index 0000000..38d19d6 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/CMakeLists.txt @@ -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 + "$" + "$" + ) + 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 "$" "${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 "$" "${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() \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/README.md b/agv_calib_brain/src/win_ubuntu_bridge/README.md similarity index 100% rename from agv_calib_brain/src/agv_calib_core/README.md rename to agv_calib_brain/src/win_ubuntu_bridge/README.md diff --git a/agv_calib_brain/src/win_ubuntu_bridge/action/DownloadSensorData.action b/agv_calib_brain/src/win_ubuntu_bridge/action/DownloadSensorData.action new file mode 100644 index 0000000..eca3cfa --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/action/DownloadSensorData.action @@ -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 # 已下载的字节数 (供行为树监控是否卡死) \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/gateways/chassis_gateway_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/gateways/chassis_gateway_node.cpp new file mode 100644 index 0000000..74e9bae --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/gateways/chassis_gateway_node.cpp @@ -0,0 +1,227 @@ +#include +#include + +// 🚨 引入自定义的 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 // 紧急刹车可以直接用标准库的 Trigger + +// 引入 gRPC 自动生成的契约头文件 +#include +#include "agv_calib_chassis.grpc.pb.h" + +#include +#include + +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( + "~/set_diagnostic_mode", std::bind(&ChassisGatewayNode::cb_set_mode, this, std::placeholders::_1, std::placeholders::_2)); + + // 接口 2：最高优急停 (无参数，用 Trigger) + srv_estop_ = this->create_service( + "~/hardware_emergency_brake", std::bind(&ChassisGatewayNode::cb_emergency_brake, this, std::placeholders::_1, std::placeholders::_2)); + + // 接口 3：开环直行 (测轮径) + srv_raw_drive_ = this->create_service( + "~/execute_raw_drive", std::bind(&ChassisGatewayNode::cb_raw_drive, this, std::placeholders::_1, std::placeholders::_2)); + + // 接口 4：开环转向 (测死区/零位) + srv_raw_steer_ = this->create_service( + "~/execute_raw_steer", std::bind(&ChassisGatewayNode::cb_raw_steer, this, std::placeholders::_1, std::placeholders::_2)); + + // 接口 5：物理参数定稿固化 + srv_commit_kinematics_ = this->create_service( + "~/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("~/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 channel_; + std::unique_ptr stub_; + std::unique_ptr stream_context_; + + // ROS 2 接口句柄 + rclcpp::Service::SharedPtr srv_set_mode_; + rclcpp::Service::SharedPtr srv_estop_; + rclcpp::Service::SharedPtr srv_raw_drive_; + rclcpp::Service::SharedPtr srv_raw_steer_; + rclcpp::Service::SharedPtr srv_commit_kinematics_; + rclcpp::Publisher::SharedPtr pub_telemetry_; + + std::thread stream_thread_; + std::atomic stream_running_; + + // ---------------------------------------------------------------------- + // ⬇️ 以下全是完美的 1:1 翻译逻辑 (ROS Request -> gRPC -> ROS Response) ⬇️ + // ---------------------------------------------------------------------- + + // [翻译] 接口 1: SetDiagnosticMode + void cb_set_mode(const std::shared_ptr req, + std::shared_ptr 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 req, + std::shared_ptr 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 req, + std::shared_ptr 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 req, + std::shared_ptr 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 req, + std::shared_ptr 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(); + 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) \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/gateways/control_gateway_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/gateways/control_gateway_node.cpp new file mode 100644 index 0000000..1e45e90 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/gateways/control_gateway_node.cpp @@ -0,0 +1,235 @@ +#include +#include + +// 引入自定义的 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 + +// 引入 gRPC 网络契约 +#include +#include "agv_calib_control.grpc.pb.h" + +#include +#include + +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( + "~/set_control_mode", std::bind(&ControlGatewayNode::cb_set_mode, this, std::placeholders::_1, std::placeholders::_2)); + + srv_estop_ = this->create_service( + "~/emergency_stop", std::bind(&ControlGatewayNode::cb_estop, this, std::placeholders::_1, std::placeholders::_2)); + + srv_open_loop_ = this->create_service( + "~/execute_open_loop_cmd", std::bind(&ControlGatewayNode::cb_open_loop, this, std::placeholders::_1, std::placeholders::_2)); + + srv_follow_traj_ = this->create_service( + "~/follow_test_trajectory", std::bind(&ControlGatewayNode::cb_follow_traj, this, std::placeholders::_1, std::placeholders::_2)); + + srv_step_resp_ = this->create_service( + "~/execute_step_response", std::bind(&ControlGatewayNode::cb_step_resp, this, std::placeholders::_1, std::placeholders::_2)); + + srv_inject_params_ = this->create_service( + "~/inject_tuning_params", std::bind(&ControlGatewayNode::cb_inject_params, this, std::placeholders::_1, std::placeholders::_2)); + + srv_commit_params_ = this->create_service( + "~/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("~/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 channel_; + std::unique_ptr stub_; + std::unique_ptr stream_context_; + + // Service & Publisher 句柄 + rclcpp::Service::SharedPtr srv_set_mode_; + rclcpp::Service::SharedPtr srv_estop_; + rclcpp::Service::SharedPtr srv_open_loop_; + rclcpp::Service::SharedPtr srv_follow_traj_; + rclcpp::Service::SharedPtr srv_step_resp_; + rclcpp::Service::SharedPtr srv_inject_params_; + rclcpp::Service::SharedPtr srv_commit_params_; + + rclcpp::Publisher::SharedPtr pub_telemetry_; + + std::thread stream_thread_; + std::atomic stream_running_; + + // ---------------------------------------------------------------------- + // ⬇️ ROS 2 to gRPC 核心翻译逻辑 ⬇️ + // ---------------------------------------------------------------------- + + void cb_set_mode(const std::shared_ptr req, + std::shared_ptr res) { + ModeRequest grpc_req; + grpc_req.set_target_mode(static_cast(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 req, std::shared_ptr 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 req, std::shared_ptr 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 req, + std::shared_ptr 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 req, std::shared_ptr 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 req, std::shared_ptr 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 req, std::shared_ptr 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(); + 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) \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/gateways/sensor_gateway_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/gateways/sensor_gateway_node.cpp new file mode 100644 index 0000000..ee936f7 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/gateways/sensor_gateway_node.cpp @@ -0,0 +1,230 @@ +#include +#include +#include + +// 引入自定义 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 +#include "agv_calib_sensor.grpc.pb.h" + +// 文件 I/O 与多线程 +#include +#include +#include + +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; + + 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( + "~/move_to_pose", std::bind(&SensorGatewayNode::cb_move, this, std::placeholders::_1, std::placeholders::_2)); + + srv_trigger_ = this->create_service( + "~/trigger_sync_capture", std::bind(&SensorGatewayNode::cb_trigger, this, std::placeholders::_1, std::placeholders::_2)); + + srv_commit_ = this->create_service( + "~/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( + 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 channel_; + std::unique_ptr stub_; + + rclcpp::Service::SharedPtr srv_move_; + rclcpp::Service::SharedPtr srv_trigger_; + rclcpp::Service::SharedPtr srv_commit_; + rclcpp_action::Server::SharedPtr action_download_; + + // ========================================================== + // 瞬时服务翻译 (Service) + // ========================================================== + void cb_move(const std::shared_ptr req, + std::shared_ptr 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 req, + std::shared_ptr 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 req, + std::shared_ptr 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 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 goal_handle) { + (void)goal_handle; + RCLCPP_WARN(this->get_logger(), "🛑 行为树请求强行取消下载"); + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr 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 goal_handle) { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + auto feedback = std::make_shared(); + + 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> 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) \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/CameraIntrinsic.msg b/agv_calib_brain/src/win_ubuntu_bridge/msg/CameraIntrinsic.msg new file mode 100644 index 0000000..6470697 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/msg/CameraIntrinsic.msg @@ -0,0 +1,6 @@ +string camera_id +float64 fx +float64 fy +float64 cx +float64 cy +float64[] dist_coeffs \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/ControlTelemetry.msg b/agv_calib_brain/src/win_ubuntu_bridge/msg/ControlTelemetry.msg new file mode 100644 index 0000000..7183a52 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/msg/ControlTelemetry.msg @@ -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 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/HardwareState.msg b/agv_calib_brain/src/win_ubuntu_bridge/msg/HardwareState.msg new file mode 100644 index 0000000..667ce78 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/msg/HardwareState.msg @@ -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 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/SensorExtrinsic.msg b/agv_calib_brain/src/win_ubuntu_bridge/msg/SensorExtrinsic.msg new file mode 100644 index 0000000..96df4a1 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/msg/SensorExtrinsic.msg @@ -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 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/msg/TrajectoryPoint.msg b/agv_calib_brain/src/win_ubuntu_bridge/msg/TrajectoryPoint.msg new file mode 100644 index 0000000..1e42738 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/msg/TrajectoryPoint.msg @@ -0,0 +1,5 @@ +float64 x_m +float64 y_m +float64 yaw_rad +float64 target_speed_ms +float64 curvature \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/package.xml b/agv_calib_brain/src/win_ubuntu_bridge/package.xml new file mode 100644 index 0000000..9e57938 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/package.xml @@ -0,0 +1,28 @@ + + + + win_ubuntu_bridge + 1.0.0 + AGV 自动化标定车间跨平台网关与总控核心 + nvidia + Apache-2.0 + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + rclcpp + rclcpp_components + rclcpp_action + std_msgs + std_srvs + action_msgs + behaviortree_cpp_v3 ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/agv_calib_brain/src/agv_calib_core/proto/agv_calib_chassis.proto b/agv_calib_brain/src/win_ubuntu_bridge/proto/agv_calib_chassis.proto similarity index 100% rename from agv_calib_brain/src/agv_calib_core/proto/agv_calib_chassis.proto rename to agv_calib_brain/src/win_ubuntu_bridge/proto/agv_calib_chassis.proto diff --git a/agv_calib_brain/src/agv_calib_core/proto/agv_calib_control.proto b/agv_calib_brain/src/win_ubuntu_bridge/proto/agv_calib_control.proto similarity index 100% rename from agv_calib_brain/src/agv_calib_core/proto/agv_calib_control.proto rename to agv_calib_brain/src/win_ubuntu_bridge/proto/agv_calib_control.proto diff --git a/agv_calib_brain/src/agv_calib_core/proto/agv_calib_sensor.proto b/agv_calib_brain/src/win_ubuntu_bridge/proto/agv_calib_sensor.proto similarity index 99% rename from agv_calib_brain/src/agv_calib_core/proto/agv_calib_sensor.proto rename to agv_calib_brain/src/win_ubuntu_bridge/proto/agv_calib_sensor.proto index e9d0a40..ffffb43 100644 --- a/agv_calib_brain/src/agv_calib_core/proto/agv_calib_sensor.proto +++ b/agv_calib_brain/src/win_ubuntu_bridge/proto/agv_calib_sensor.proto @@ -18,7 +18,7 @@ service SensorCalibrationService { // 🚙 [Windows 返回 -> Linux]：物理到位抱死刹车后，返回成功回执 // 🚨 业务潜台词：Linux 收到回执后，必须在代码里强制 sleep(0.5s) 等待避震悬挂平息，冻结物理空间！ rpc MoveToObservationPose (PoseRequest) returns (StandardResponse); - + // --------------------------------------------------------- // 第二步：防延迟同步锁存 (停与拍) // --------------------------------------------------------- diff --git a/agv_calib_brain/src/agv_calib_core/src/brain_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/src/brain_node.cpp similarity index 100% rename from agv_calib_brain/src/agv_calib_core/src/brain_node.cpp rename to agv_calib_brain/src/win_ubuntu_bridge/src/brain_node.cpp diff --git a/agv_calib_brain/src/win_ubuntu_bridge/src/master_node.cpp b/agv_calib_brain/src/win_ubuntu_bridge/src/master_node.cpp new file mode 100644 index 0000000..f637f03 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/src/master_node.cpp @@ -0,0 +1,12 @@ +#include + +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; +} \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/CommitCalibrationResults.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/CommitCalibrationResults.srv new file mode 100644 index 0000000..0d80ab6 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/CommitCalibrationResults.srv @@ -0,0 +1,6 @@ +string task_id +win_ubuntu_bridge/CameraIntrinsic[] updated_intrinsics +win_ubuntu_bridge/SensorExtrinsic[] updated_extrinsics +--- +bool success +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/CommitKinematics.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/CommitKinematics.srv new file mode 100644 index 0000000..a46ed2f --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/CommitKinematics.srv @@ -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 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteOpenLoopCmd.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteOpenLoopCmd.srv new file mode 100644 index 0000000..95dcd1e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteOpenLoopCmd.srv @@ -0,0 +1,7 @@ +float64 left_motor_cmd +float64 right_motor_cmd +float64 steering_angle +float64 duration_sec +--- +bool success +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteRawDrive.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteRawDrive.srv new file mode 100644 index 0000000..e8d8a51 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteRawDrive.srv @@ -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 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteRawSteer.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteRawSteer.srv new file mode 100644 index 0000000..5fb0175 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteRawSteer.srv @@ -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 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteStepResponse.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteStepResponse.srv new file mode 100644 index 0000000..5619664 --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/ExecuteStepResponse.srv @@ -0,0 +1,5 @@ +float64 target_velocity_ms +float64 duration_sec +--- +bool success +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/FollowTestTrajectory.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/FollowTestTrajectory.srv new file mode 100644 index 0000000..b6719fe --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/FollowTestTrajectory.srv @@ -0,0 +1,5 @@ +string test_case_id +win_ubuntu_bridge/TrajectoryPoint[] path +--- +bool success +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/InjectTuningParams.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/InjectTuningParams.srv new file mode 100644 index 0000000..817488b --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/InjectTuningParams.srv @@ -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 \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/MoveToObservationPose.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/MoveToObservationPose.srv new file mode 100644 index 0000000..89c1bbc --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/MoveToObservationPose.srv @@ -0,0 +1,7 @@ +float64 target_x_m +float64 target_y_m +float64 target_yaw_deg +bool is_relative +--- +bool success +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/SetControlMode.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/SetControlMode.srv new file mode 100644 index 0000000..59812fb --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/SetControlMode.srv @@ -0,0 +1,4 @@ +uint8 target_mode # 0: NORMAL, 1: OPEN_LOOP, 2: TUNING_MODE +--- +bool success +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/SetDiagnosticMode.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/SetDiagnosticMode.srv new file mode 100644 index 0000000..7653f1c --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/SetDiagnosticMode.srv @@ -0,0 +1,4 @@ +uint8 target_mode # 0: NORMAL_KINEMATICS, 1: DIRECT_RAW_DRIVE +--- +bool success +string message \ No newline at end of file diff --git a/agv_calib_brain/src/win_ubuntu_bridge/srv/TriggerSyncCapture.srv b/agv_calib_brain/src/win_ubuntu_bridge/srv/TriggerSyncCapture.srv new file mode 100644 index 0000000..aa96d4e --- /dev/null +++ b/agv_calib_brain/src/win_ubuntu_bridge/srv/TriggerSyncCapture.srv @@ -0,0 +1,5 @@ +string[] sensor_ids +--- +bool success +int64 capture_timestamp_us # 🚨 极其关键的“取件码” +string error_message \ No newline at end of file diff --git a/全局架构图.md b/全局架构图.md new file mode 100644 index 0000000..73418f2 --- /dev/null +++ b/全局架构图.md @@ -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 ????????) \ No newline at end of file