From 7f7616b493774001594e1337755e746c8d0cac79 Mon Sep 17 00:00:00 2001 From: xinfengkun Date: Fri, 26 Jul 2024 19:13:07 +0800 Subject: [PATCH] =?UTF-8?q?[660][adas]=20=E6=96=B0=E5=A2=9E=E8=BD=A6?= =?UTF-8?q?=E9=81=93=E7=BA=BF=E4=B8=8A=E4=BC=A0=E6=8E=A7=E5=88=B6=E5=BC=80?= =?UTF-8?q?=E5=85=B3=E3=80=81=E6=96=B0=E5=A2=9E=E8=BD=A6=E9=81=93=E7=BA=BF?= =?UTF-8?q?pb=E4=BB=A5=E5=8F=8A=E5=9B=9E=E8=B0=83?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../autopilot/adapter/MoGoAdasListenerImpl.kt | 14 ++++ .../zhjt/mogo/adas/common/MessageType.java | 1 + .../src/main/proto/lane_mark.proto | 81 +++++++++++++++++++ .../src/main/proto/message_pad.proto | 3 + .../zhidao/support/adas/high/AdasChannel.java | 20 +++++ .../zhidao/support/adas/high/AdasManager.java | 15 ++++ .../support/adas/high/IAdasNetCommApi.java | 12 +++ .../support/adas/high/OnAdasListener.java | 9 +++ .../adas/high/msg/LaneMarksTranMessage.java | 33 ++++++++ .../adas/high/msg/MyMessageFactory.java | 7 ++ 10 files changed, 195 insertions(+) create mode 100644 libraries/mogo-adas-data/src/main/proto/lane_mark.proto create mode 100644 libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/LaneMarksTranMessage.java diff --git a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt index 97fb8f0dd2..6d749ccdab 100644 --- a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt +++ b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt @@ -117,6 +117,7 @@ import mogo.v2x.RoadOverviewEvents import mogo.yycp.paralleldriving.protocol.ParallelTaskProcessNoticeOuterClass import mogo_msg.MogoReportMsg import perception.FusionTrafficLightOuterClass +import perception.LaneMarkOuterClass import planning.RoboSweeperTaskIndexOuterClass import prediction.Prediction import record_cache.RecordPanelOuterClass @@ -1030,6 +1031,19 @@ class MoGoAdasListenerImpl : OnAdasListener { CallerV2nNioEventListenerManager.invokeV2nNioCongestionEvent(congestion) } + /** + * 车道线 + * + * @param header 头 + * @param laneMarks 数据 + */ + override fun onLaneMarksTran( + header: MessagePad.Header, + laneMarks: LaneMarkOuterClass.LaneMarks + ) { + + } + /** * 所连接的域控的节点状态信息 * 目前包含状态 节点是否存在；节点是否超时； diff --git a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java index 03f470f8d0..6985e1d840 100644 --- a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java +++ b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java @@ -87,6 +87,7 @@ public enum MessageType { TYPE_RECEIVE_V2N_NIO_CROSSOVER_EVENT(MessagePad.MessageType.MsgTypeV2nNioCrossoverEvent, "V2N NIO行人横穿（路侧）"), TYPE_RECEIVE_V2N_NIO_OTHER_RETROGRADE_EVENT(MessagePad.MessageType.MsgTypeV2nNioOtherRetrogradeEvent, "V2N NIO他车逆行（路侧）"), TYPE_RECEIVE_V2N_NIO_CONGESTION_EVENT(MessagePad.MessageType.MsgTypeV2nNioCongestionEvent, "V2N NIO拥堵事件"), + TYPE_RECEIVE_LANE_MARKS_TRAN(MessagePad.MessageType.MsgTypeLaneMarksTran, "车道线"), //TODO 透传原始pb文件中不存在以下type。由于Java中无法强转,所以在mogo-adas-data/message_pad.proto中放开注释 TYPE_RECEIVE_PLANNING_DECISION_STATE(MessagePad.MessageType.MsgTypePlanningDecisionState, "Planning决策状态"), TYPE_RECEIVE_SWEEPER_TASK_INDEX_DATA(MessagePad.MessageType.MsgTypeSweeperTaskIndexData, "清扫车指标数据"), diff --git a/libraries/mogo-adas-data/src/main/proto/lane_mark.proto b/libraries/mogo-adas-data/src/main/proto/lane_mark.proto new file mode 100644 index 0000000000..a37748fdb8 --- /dev/null +++ b/libraries/mogo-adas-data/src/main/proto/lane_mark.proto @@ -0,0 +1,81 @@ +syntax = "proto2"; +package perception; + +import "header.proto"; +import "geometry.proto"; + +enum LaneMarkColor { + COLOR_WHITE = 0; + COLOR_YELLOW = 1; +} + +enum LaneMarkType { + LANE_MARK_NONE = 0; + LANE_MARK_SOLID = 1; + LANE_MARK_BROKEN = 2; +} + + +message LaneLineCubicCurve +{ + optional double x_start = 1; + optional double x_end = 2; + optional double a = 3; + optional double b = 4; + optional double c = 5; + optional double d = 6; +} // add by qianlei + +message LaneMark { + optional LaneMarkColor color = 1; + optional LaneMarkType type = 2; + optional float confidence = 3; // valid length / 60.0 meters + repeated geometry.Point2D points = 4; + optional LaneLineCubicCurve curve_car_coord = 5; // add by qianlei + repeated geometry.Point2D curve_image_coord_point_set = 6; // add by qianlei + repeated geometry.Point3D curve_car_coord_point_set = 7; // add by qianlei + optional int32 track_id = 8; + optional int32 frame_lane_line_id = 9; + optional int32 color_telematics = 10; + optional int32 type_telematics = 11; +} + +message LaneCenter { + repeated geometry.Point2D points = 1; +} + +message RoadSide { + optional int32 type = 1; + optional int32 frame_roadside_id = 2; + repeated geometry.Point2D img_point_set = 3; + repeated geometry.Point3D car_coord_point_set = 4; +} + +message RoadArrow { + optional int32 x = 1; + optional int32 y = 2; + optional int32 w = 3; + optional int32 h = 4; + optional int32 type = 5; +} + +message LaneMarks { + optional common.Header header = 1; + + optional LaneMark left = 2; + optional LaneMark right = 3; + optional LaneMark left2 = 4; + optional LaneMark right2 = 5; + repeated LaneMark lanemarks = 6; // add by qianlei + optional common.Header header_ros_time_start = 7; + optional common.Header header_ros_time_end = 8; + repeated RoadSide roadsides = 9; + repeated RoadArrow arrows = 10; + optional bytes lane_arrow_stopline_crossing_seg = 11; + optional bytes fs_rs_seg = 12; + optional bytes laneline_inst_seg = 13; + optional bool laneline_exist_flag = 14; + optional bool arrow_exist_flag = 15; + optional bool stopline_exist_flag = 16; + optional bool crossing_exist_flag = 17; +} \ No newline at end of file diff --git a/libraries/mogo-adas-data/src/main/proto/message_pad.proto b/libraries/mogo-adas-data/src/main/proto/message_pad.proto index c23d2d8e7d..560b6fd351 100644 --- a/libraries/mogo-adas-data/src/main/proto/message_pad.proto +++ b/libraries/mogo-adas-data/src/main/proto/message_pad.proto @@ -92,6 +92,7 @@ enum MessageType MsgTypeV2nNioCrossoverEvent = 0x10129; //行人横穿（路侧）事件推送, 透传 MsgTypeV2nNioOtherRetrogradeEvent = 0x1012a; //他车逆行（路侧）事件推送, 透传 MsgTypeV2nNioCongestionEvent = 0x1012b; //拥堵事件推送, 透传 + MsgTypeLaneMarksTran = 0x1012c;//车道线上传 } message Header @@ -678,6 +679,7 @@ enum OperatorCmdType OPERATOR_CMD_CHANGE_LANE = 1; OPERATOR_CMD_SET_ACCELERATED_SPEED = 2; OPERATOR_CMD_SET_HORN = 3; + OPERATOR_CMD_SEND_LANE_MARKS = 4; } message OperatorCmdReq @@ -686,6 +688,7 @@ message OperatorCmdReq double value = 2; //OPERATOR_CMD_CHANGE_LANE: 1: left 2: right //OPERATOR_CMD_SET_ACCELERATED_SPEED: accelerated speed //OPERATOR_CMD_SET_HORN: 1: honk 2: stop honking + //OPERATOR_CMD_SEND_LANE_MARKS 0: 开始发送车道线 1:结束发送车道线 } //message definition for MsgTypePlanningDecisionState diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java index d765b0bf3f..03783708cc 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java @@ -2177,6 +2177,26 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec return sendOperatorCmdSetHorn(2); } + /** + * 车道线 + * 此方法存在域控回执，监听{@link OnAdasListener#onReceiveReceivedAck(ReceivedAck)}回调.使用方法：将此方法的返回值与{@link ReceivedAck#getMsgId()}进行比较，如果相同判断{@link ReceivedAck#getStatus()}是否等于{@link ReceivedAck.Status#NORMAL}，详情参见CheckSystemView中的onReceiveReceivedAck + * + * @param value 0: 开始发送车道线 1:结束发送车道线 + * @return 消息是否添加到WS消息发送队列，返回值为非0的正整数时表示下发消息的消息ID + * * >=0：表示添加到WS发送消息队列 + * * =0：表示乘客屏模式添加到WS发送消息队列 + * * -1L：添加到WS发送消息队列失败 + */ + @Override + public long sendOperatorCmdSendLaneMarks(double value) { + MessagePad.OperatorCmdReq req = MessagePad.OperatorCmdReq + .newBuilder() + .setCmdType(MessagePad.OperatorCmdType.OPERATOR_CMD_SEND_LANE_MARKS) + .setValue(value) + .build(); + return sendPBMessage(MessageType.TYPE_SEND_OPERATOR_CMD_REQ, req.toByteArray()); + } + /** * 设置参数命令 * 此方法存在域控回执，监听{@link OnAdasListener#onReceiveReceivedAck(ReceivedAck)}回调.使用方法：将此方法的返回值与{@link ReceivedAck#getMsgId()}进行比较，如果相同判断{@link ReceivedAck#getStatus()}是否等于{@link ReceivedAck.Status#NORMAL}，详情参见CheckSystemView中的onReceiveReceivedAck diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java index ea5dea5bda..49e3723e69 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java @@ -1218,6 +1218,21 @@ public class AdasManager implements IAdasNetCommApi { return mChannel == null ? -1L : mChannel.sendOperatorCmdStopHonking(); } + /** + * 车道线 + * 此方法存在域控回执，监听{@link OnAdasListener#onReceiveReceivedAck(ReceivedAck)}回调.使用方法：将此方法的返回值与{@link ReceivedAck#getMsgId()}进行比较，如果相同判断{@link ReceivedAck#getStatus()}是否等于{@link ReceivedAck.Status#NORMAL}，详情参见CheckSystemView中的onReceiveReceivedAck + * + * @param value 0: 开始发送车道线 1:结束发送车道线 + * @return 消息是否添加到WS消息发送队列，返回值为非0的正整数时表示下发消息的消息ID + * * >=0：表示添加到WS发送消息队列 + * * =0：表示乘客屏模式添加到WS发送消息队列 + * * -1L：添加到WS发送消息队列失败 + */ + @Override + public long sendOperatorCmdSendLaneMarks(double value) { + return mChannel == null ? -1L : mChannel.sendOperatorCmdSendLaneMarks(value); + } + /** * 绕障类功能开关 * 此方法存在域控回执，监听{@link OnAdasListener#onReceiveReceivedAck(ReceivedAck)}回调.使用方法：将此方法的返回值与{@link ReceivedAck#getMsgId()}进行比较，如果相同判断{@link ReceivedAck#getStatus()}是否等于{@link ReceivedAck.Status#NORMAL}，详情参见CheckSystemView中的onReceiveReceivedAck diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java index ae86f55336..2fdfa84f0d 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java @@ -800,6 +800,18 @@ public interface IAdasNetCommApi { */ long sendOperatorCmdStopHonking(); + /** + * 车道线 + * 此方法存在域控回执，监听{@link OnAdasListener#onReceiveReceivedAck(ReceivedAck)}回调.使用方法：将此方法的返回值与{@link ReceivedAck#getMsgId()}进行比较，如果相同判断{@link ReceivedAck#getStatus()}是否等于{@link ReceivedAck.Status#NORMAL}，详情参见CheckSystemView中的onReceiveReceivedAck + * + * @param value 0: 开始发送车道线 1:结束发送车道线 + * @return 消息是否添加到WS消息发送队列，返回值为非0的正整数时表示下发消息的消息ID + * * >=0：表示添加到WS发送消息队列 + * * =0：表示乘客屏模式添加到WS发送消息队列 + * * -1L：添加到WS发送消息队列失败 + */ + long sendOperatorCmdSendLaneMarks(double value); + /*******************************设置参数命令 包括V1************************************/ /** * 绕障类功能开关 diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java index 4f487ac081..091535c69a 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java @@ -40,6 +40,7 @@ import mogo.v2x.RoadOverviewEvents; import mogo.yycp.paralleldriving.protocol.ParallelTaskProcessNoticeOuterClass; import mogo_msg.MogoReportMsg; import perception.FusionTrafficLightOuterClass; +import perception.LaneMarkOuterClass; import planning.RoboSweeperTaskIndexOuterClass; import prediction.Prediction; import record_cache.RecordPanelOuterClass; @@ -534,6 +535,14 @@ public interface OnAdasListener { */ void onV2nNioCongestionEvent(@NonNull MessagePad.Header header, @NonNull MessagePad.V2nCongestion congestion); + /** + * 车道线 + * + * @param header 头 + * @param laneMarks 数据 + */ + void onLaneMarksTran(@NonNull MessagePad.Header header, @NonNull LaneMarkOuterClass.LaneMarks laneMarks); + /** * 所连接的域控的节点状态信息 * 目前包含状态 节点是否存在；节点是否超时； diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/LaneMarksTranMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/LaneMarksTranMessage.java new file mode 100644 index 0000000000..614a4bb40e --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/LaneMarksTranMessage.java @@ -0,0 +1,33 @@ +package com.zhidao.support.adas.high.msg; + +import android.os.SystemClock; + +import com.google.protobuf.InvalidProtocolBufferException; +import com.zhidao.support.adas.high.AdasChannel; +import com.zhidao.support.adas.high.OnAdasListener; +import com.zhidao.support.adas.high.common.CupidLogUtils; +import com.zhidao.support.adas.high.protocol.RawData; + +import perception.LaneMarkOuterClass; + +/** + * 车道线 + */ +public class LaneMarksTranMessage extends MyAbstractMessageHandler { + + public LaneMarksTranMessage() { + } + + @Override + public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { + LaneMarkOuterClass.LaneMarks laneMarks = LaneMarkOuterClass.LaneMarks.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); + AdasChannel.calculateTimeConsumingOnDispatchRaw("车道线", raw.receiveTime); + long nowTime = 0; + if (CupidLogUtils.isEnableLog()) + nowTime = SystemClock.elapsedRealtime(); + if (adasListener != null) { + adasListener.onLaneMarksTran(raw.getHeader(), laneMarks); + } + AdasChannel.calculateTimeConsumingBusiness("车道线", nowTime); + } +} diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java index 798042aada..1dc44cf498 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java @@ -52,6 +52,7 @@ public class MyMessageFactory implements IMyMessageFactory { private IMsg v2nNioCrossoverEventMessage;//V2N NIO行人横穿（路侧） private IMsg v2nNioOtherRetrogradeEventMessage;//V2N NIO他车逆行（路侧） private IMsg v2nNioCongestionEventMessage;//V2N NIO拥堵事件 + private IMsg laneMarksTranMessage;//车道线 private final AutopilotReview autopilotReview; private final TurnLightState lightLeft = new TurnLightState(); @@ -296,6 +297,12 @@ public class MyMessageFactory implements IMyMessageFactory { v2nNioCongestionEventMessage = new V2nNioCongestionEventMessage(); } return v2nNioCongestionEventMessage; + } else if (messageType == MessageType.TYPE_RECEIVE_LANE_MARKS_TRAN.typeCode) { + //车道线 + if (laneMarksTranMessage == null) { + laneMarksTranMessage = new LaneMarksTranMessage(); + } + return laneMarksTranMessage; } else { //MessageType.TYPE_DEFAULT.typeCode return null;