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 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 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(plugin, Conversions.ConvertFrom); RegPublisher(plugin, Conversions.ConvertFrom); RegPublisher(plugin, Conversions.ConvertFrom); + RegPublisher(plugin, Conversions.ConvertFrom); RegSubscriber(plugin, Conversions.ConvertTo); RegSubscriber(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(), + }; + + 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.