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);
}
static Vector3 Convert(apollo.common.Point3D p)
{
return(new Vector3((float)p.x, (float)p.y, (float)p.z));
}
static double3 Convert(apollo.common.Point3D p)
{
return(new double3(p.x, p.y, p.z));
}
