From 0e176e11768279f226f35e21eb256b4f08be2a21 Mon Sep 17 00:00:00 2001
From: duanchengwen <duanchengwen07@163.com>
Date: Thu, 7 Apr 2022 16:03:15 +0800
Subject: [PATCH] output lane lines for ros bridge

---
 Messages/Lgsvl.cs   | 51 +++++++++++++++++++++++
 RosBridgeFactory.cs |  1 +
 RosConversions.cs   | 99 +++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 151 insertions(+)

diff --git a/Messages/Lgsvl.cs b/Messages/Lgsvl.cs
index d3c9228..230a218 100644
--- a/Messages/Lgsvl.cs
+++ b/Messages/Lgsvl.cs
@@ -51,6 +51,30 @@ public enum VehicleMode: byte
         VEHICLE_MODE_EMERGENCY_MODE = 4,
     }
 
+    public enum LaneLineType: byte
+    {
+        WhiteDashed = 0,
+        WhiteSolid = 1,
+        YellowDashed = 2,
+        YellowSolid = 3,
+    }
+
+    public enum LaneLinePositionType: sbyte
+    {
+        BollardLeft = -5,
+        FourthLeft = -4,
+        ThirdLeft = -3,
+        AdjacentLeft = -2,
+        EgoLeft = -1,
+        EgoRight = 1,
+        AdjacentRight = 2,
+        ThirdRight = 3,
+        FourthRight = 4,
+        BollardRight = 5,
+        Other = 6,
+        Unknown = 7,
+    }
+
     public enum ObjectState
     {
         STATE_MOVING = 0,
@@ -126,6 +150,33 @@ public class Detection3DArray
         public List<Detection3D> detections;
     }
 
+    [MessageType("lgsvl_msgs/LaneLineCubicCurve")]
+    public class LaneLineCubicCurve
+    {
+        public float longitude_min;
+        public float longitude_max;
+        public float a;
+        public float b;
+        public float c;
+        public float d;
+    }
+
+    [MessageType("lgsvl_msgs/LaneLine")]
+    public class LaneLine
+    {
+        public Ros.Header header;
+        public LaneLineType type;
+        public LaneLinePositionType pos_type;
+        public LaneLineCubicCurve curve_camera_coord;
+    }
+    
+    [MessageType("lgsvl_msgs/LaneLineArray")]
+    public class LaneLineArray
+    {
+        public Ros.Header header;
+        public List<LaneLine> camera_laneline;
+    }
+
     [MessageType("lgsvl_msgs/Signal")]
     public class Signal
     {
diff --git a/RosBridgeFactory.cs b/RosBridgeFactory.cs
index 2aa61a6..d7257e7 100644
--- a/RosBridgeFactory.cs
+++ b/RosBridgeFactory.cs
@@ -42,6 +42,7 @@ public void Register(IBridgePlugin plugin)
             RegPublisher<CameraInfoData, Ros.CameraInfo>(plugin, Conversions.ConvertFrom);
             RegPublisher<Detected2DObjectData, Lgsvl.Detection2DArray>(plugin, Conversions.ConvertFrom);
             RegPublisher<ClockData, Ros.Clock>(plugin, Conversions.ConvertFrom);
+            RegPublisher<LaneLinesData, Lgsvl.LaneLineArray>(plugin, Conversions.ConvertFrom);
 
             RegSubscriber<VehicleStateData, Lgsvl.VehicleStateDataRos>(plugin, Conversions.ConvertTo);
             RegSubscriber<Detected2DObjectArray, Lgsvl.Detection2DArray>(plugin, Conversions.ConvertTo);
diff --git a/RosConversions.cs b/RosConversions.cs
index 485f319..e73303e 100644
--- a/RosConversions.cs
+++ b/RosConversions.cs
@@ -323,6 +323,92 @@ public static PerceptionObstacles ApolloConvertFrom(Detected3DObjectData data)
             return obstacles;
         }
 
+        public static Lgsvl.LaneLineArray ConvertFrom(LaneLinesData data)
+        {
+            var result = new Lgsvl.LaneLineArray()
+            {
+                header = new Ros.Header()
+                {
+                    seq = data.Sequence,
+                    stamp = Conversions.ConvertTime(data.Time),
+                    frame_id = data.Frame,
+                },
+                camera_laneline = new List<Lgsvl.LaneLine>(),
+            };
+
+            foreach (var lineData in data.lineData)
+            {
+                var line = new Lgsvl.LaneLine()
+                {
+                    curve_camera_coord = Convert(lineData.CurveCameraCoord)
+                };
+
+                // Note: Don't cast one enum value to another in case Apollo changes their underlying values
+                switch (lineData.PositionType)
+                {
+                    case LaneLinePositionType.BollardLeft:
+                        line.pos_type = Lgsvl.LaneLinePositionType.BollardLeft;
+                        break;
+                    case LaneLinePositionType.FourthLeft:
+                        line.pos_type = Lgsvl.LaneLinePositionType.FourthLeft;
+                        break;
+                    case LaneLinePositionType.ThirdLeft:
+                        line.pos_type = Lgsvl.LaneLinePositionType.ThirdLeft;
+                        break;
+                    case LaneLinePositionType.AdjacentLeft:
+                        line.pos_type = Lgsvl.LaneLinePositionType.AdjacentLeft;
+                        break;
+                    case LaneLinePositionType.EgoLeft:
+                        line.pos_type = Lgsvl.LaneLinePositionType.EgoLeft;
+                        break;
+                    case LaneLinePositionType.EgoRight:
+                        line.pos_type = Lgsvl.LaneLinePositionType.EgoRight;
+                        break;
+                    case LaneLinePositionType.AdjacentRight:
+                        line.pos_type = Lgsvl.LaneLinePositionType.AdjacentRight;
+                        break;
+                    case LaneLinePositionType.ThirdRight:
+                        line.pos_type = Lgsvl.LaneLinePositionType.ThirdRight;
+                        break;
+                    case LaneLinePositionType.FourthRight:
+                        line.pos_type = Lgsvl.LaneLinePositionType.FourthRight;
+                        break;
+                    case LaneLinePositionType.BollardRight:
+                        line.pos_type = Lgsvl.LaneLinePositionType.BollardRight;
+                        break;
+                    case LaneLinePositionType.Other:
+                        line.pos_type = Lgsvl.LaneLinePositionType.Other;
+                        break;
+                    case LaneLinePositionType.Unknown:
+                        line.pos_type = Lgsvl.LaneLinePositionType.Unknown;
+                        break;
+                }
+
+                // Note: Don't cast one enum value to another in case Apollo changes their underlying values
+                switch (lineData.Type)
+                {
+                    case LaneLineType.WhiteDashed:
+                        line.type = Lgsvl.LaneLineType.WhiteDashed;
+                        break;
+                    case LaneLineType.WhiteSolid:
+                        line.type = Lgsvl.LaneLineType.WhiteSolid;
+                        break;
+                    case LaneLineType.YellowDashed:
+                        line.type = Lgsvl.LaneLineType.YellowDashed;
+                        break;
+                    case LaneLineType.YellowSolid:
+                        line.type = Lgsvl.LaneLineType.YellowSolid;
+                        break;
+                    default:
+                        throw new ArgumentOutOfRangeException();
+                }
+                
+                result.camera_laneline.Add(line);
+            }
+
+            return result;
+        }
+
         public static Lgsvl.SignalArray ConvertFrom(SignalDataArray data)
         {
             return new Lgsvl.SignalArray()
@@ -995,6 +1081,19 @@ static UnityEngine.Quaternion Convert(Ros.Quaternion q)
             return new UnityEngine.Quaternion((float)q.x, (float)q.y, (float)q.z, (float)q.w);
         }
 
+        static Lgsvl.LaneLineCubicCurve Convert(LaneLineCubicCurve c)
+        {
+            return new Lgsvl.LaneLineCubicCurve()
+            {
+                a = c.C0,
+                b = c.C1,
+                c = c.C2,
+                d = c.C3,
+                longitude_max = c.MaxX,
+                longitude_min = c.MinX
+            };
+        }
+
         static UnityEngine.Quaternion ConvertToRfu(UnityEngine.Quaternion q)
         {
             // In Righthanded xyz, rotate by -90 deg around z axis.