public static Detected3DObjectArray ConvertTo(apollo.perception.PerceptionObstacles data)
{
var detections = new List <Detected3DObject>();
foreach (var obstacle in data.perception_obstacle)
{
String label;
switch (obstacle.type)
{
case apollo.perception.PerceptionObstacle.Type.Vehicle:
label = "Car";
break;
case apollo.perception.PerceptionObstacle.Type.Pedestrian:
label = "Pedestrian";
break;
case apollo.perception.PerceptionObstacle.Type.Bicycle:
label = "Bicycle";
break;
default:
label = "Unknown";
break;
}
GpsData gps = new GpsData()
{
Easting = obstacle.position.x,
Northing = obstacle.position.y,
Altitude = obstacle.position.z,
};
var det = new Detected3DObject()
{
Id = (uint)obstacle.id,
Label = label,
Gps = gps,
Heading = 90 - obstacle.theta * Mathf.Rad2Deg,
Scale = new Vector3
(
(float)obstacle.width,
(float)obstacle.height,
(float)obstacle.length
),
};
detections.Add(det);
}
return(new Detected3DObjectArray()
{
Data = detections.ToArray(),
});
}
public static apollo.perception.PerceptionObstacles ConvertFrom(Detected3DObjectData data)
{
var obstacles = new apollo.perception.PerceptionObstacles()
{
header = new apollo.common.Header()
{
timestamp_sec = data.Time,
module_name = "perception_obstacle",
sequence_num = data.Sequence,
lidar_timestamp = (ulong)(data.Time * 1e9),
},
error_code = apollo.common.ErrorCode.Ok,
};
foreach (var d in data.Data)
{
// Transform from (Right/Up/Forward) to (Right/Forward/Up)
var velocity = d.Velocity;
velocity.Set(velocity.x, velocity.z, velocity.y);
var acceleration = d.Acceleration;
acceleration.Set(acceleration.x, acceleration.z, acceleration.y);
var size = d.Scale;
size.Set(size.x, size.z, size.y);
apollo.perception.PerceptionObstacle.Type type = apollo.perception.PerceptionObstacle.Type.Unknown;
apollo.perception.PerceptionObstacle.SubType subType = apollo.perception.PerceptionObstacle.SubType.StUnknown;
if (d.Label == "Pedestrian")
{
type = apollo.perception.PerceptionObstacle.Type.Pedestrian;
subType = apollo.perception.PerceptionObstacle.SubType.StPedestrian;
}
else if (d.Label == "BoxTruck")
{
type = apollo.perception.PerceptionObstacle.Type.Vehicle;
subType = apollo.perception.PerceptionObstacle.SubType.StTruck;
}
else if (d.Label == "SchoolBus")
{
type = apollo.perception.PerceptionObstacle.Type.Vehicle;
subType = apollo.perception.PerceptionObstacle.SubType.StBus;
}
else
{
type = apollo.perception.PerceptionObstacle.Type.Vehicle;
subType = apollo.perception.PerceptionObstacle.SubType.StCar;
}
var po = new apollo.perception.PerceptionObstacle()
{
id = (int)d.Id,
position = ConvertToPoint(d.Gps),
theta = (90 - d.Heading) * Mathf.Deg2Rad,
velocity = ConvertToPoint(velocity),
width = size.x,
length = size.y,
height = size.z,
tracking_time = d.TrackingTime,
type = type,
timestamp = data.Time,
acceleration = ConvertToPoint(acceleration),
anchor_point = ConvertToPoint(d.Gps),
sub_type = subType,
};
// polygon points := obstacle corner points
var cx = d.Gps.Easting;
var cy = d.Gps.Northing;
var cz = d.Gps.Altitude;
var px = 0.5f * size.x;
var py = 0.5f * size.y;
var c = Mathf.Cos((float)-d.Heading * Mathf.Deg2Rad);
var s = Mathf.Sin((float)-d.Heading * Mathf.Deg2Rad);
var p1 = new apollo.common.Point3D()
{
x = -px * c + py * s + cx, y = -px * s - py * c + cy, z = cz
};
var p2 = new apollo.common.Point3D()
{
x = px * c + py * s + cx, y = px * s - py * c + cy, z = cz
};
var p3 = new apollo.common.Point3D()
{
x = px * c - py * s + cx, y = px * s + py * c + cy, z = cz
};
var p4 = new apollo.common.Point3D()
{
x = -px * c - py * s + cx, y = -px * s + py * c + cy, z = cz
};
po.polygon_point.Add(p1);
po.polygon_point.Add(p2);
po.polygon_point.Add(p3);
po.polygon_point.Add(p4);
po.measurements.Add(new apollo.perception.SensorMeasurement()
{
sensor_id = "velodyne128",
id = (int)d.Id,
position = ConvertToPoint(d.Gps),
theta = (90 - d.Heading) * Mathf.Deg2Rad,
width = size.x,
length = size.y,
height = size.z,
velocity = ConvertToPoint(velocity),
type = type,
sub_type = subType,
timestamp = data.Time,
});
obstacles.perception_obstacle.Add(po);
}
return(obstacles);
}
