Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

output lane lines for ros bridge #1

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

output lane lines for ros bridge #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 51 additions & 0 deletions Messages/Lgsvl.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
{
Expand Down
1 change: 1 addition & 0 deletions RosBridgeFactory.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
99 changes: 99 additions & 0 deletions RosConversions.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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.
Expand Down