public static LGSVL.Detection3DArray ConvertFrom(Detected3DObjectData data)
{
return(new LGSVL.Detection3DArray()
{
header = new Header()
{
seq = data.Sequence,
stamp = Conversions.ConvertTime(data.Time),
frame_id = data.Frame,
},
detections = data.Data.Select(d => new Detection3D()
{
id = d.Id,
label = d.Label,
score = d.Score,
bbox = new BoundingBox3D()
{
position = new Pose()
{
position = ConvertToPoint(d.Position),
orientation = Convert(d.Rotation),
},
size = ConvertToVector(d.Scale),
},
velocity = new Twist()
{
linear = ConvertToVector(d.LinearVelocity),
angular = ConvertToVector(d.AngularVelocity),
}
}).ToList(),
});
}
public static Lgsvl.Detection3DArray ConvertFrom(Detected3DObjectData data)
{
var arr = new Lgsvl.Detection3DArray()
{
header = new Ros.Header()
{
seq = data.Sequence,
stamp = Conversions.ConvertTime(data.Time),
frame_id = data.Frame,
},
detections = new List <Lgsvl.Detection3D>(),
};
foreach (var d in data.Data)
{
// Transform from (Right/Up/Forward) to (Forward/Left/Up)
var position = d.Position;
position.Set(position.z, -position.x, position.y);
var orientation = d.Rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w);
var size = d.Scale;
size.Set(size.z, size.x, size.y);
d.AngularVelocity.z = -d.AngularVelocity.z;
var det = new Lgsvl.Detection3D()
{
id = d.Id,
label = d.Label,
score = d.Score,
bbox = new Lgsvl.BoundingBox3D()
{
position = new Ros.Pose()
{
position = ConvertToPoint(position),
orientation = Convert(orientation),
},
size = ConvertToVector(size),
},
velocity = new Ros.Twist()
{
linear = ConvertToVector(d.LinearVelocity),
angular = ConvertToVector(d.AngularVelocity),
},
};
arr.detections.Add(det);
}
return(arr);
}
private IEnumerator OnPublish()
{
uint seqId = 0;
double nextSend = SimulatorManager.Instance.CurrentTime + 1.0f / Frequency;
while (true)
{
yield return(new WaitForFixedUpdate());
var IDs = new HashSet <uint>(Detected.Keys);
IDs.ExceptWith(CurrentIDs);
foreach (uint id in IDs)
{
Detected.Remove(id);
}
if (Bridge != null && Bridge.Status == Status.Connected)
{
if (SimulatorManager.Instance.CurrentTime < nextSend)
{
continue;
}
nextSend = SimulatorManager.Instance.CurrentTime + 1.0f / Frequency;
var currentObjects = new List <Detected3DObject>();
foreach (uint id in CurrentIDs)
{
currentObjects.Add(Detected[id].Item1);
}
var data = new Detected3DObjectData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = seqId++,
Data = currentObjects.ToArray(),
};
Publish(data);
}
}
}
public static apollo.common.Detection3DArray ConvertFrom(Detected3DObjectData data)
{
var r = new apollo.common.Detection3DArray()
{
header = new apollo.common.Header()
{
sequence_num = data.Sequence,
frame_id = data.Frame,
timestamp_sec = data.Time,
},
};
foreach (var obj in data.Data)
{
r.detections.Add(new apollo.common.Detection3D()
{
header = r.header,
id = obj.Id,
label = obj.Label,
score = obj.Score,
bbox = new apollo.common.BoundingBox3D()
{
position = new apollo.common.Pose()
{
position = ConvertToPoint(obj.Position),
orientation = Convert(obj.Rotation),
},
size = ConvertToVector(obj.Scale),
},
velocity = new apollo.common.Twist()
{
linear = ConvertToVector(obj.LinearVelocity),
angular = ConvertToVector(obj.LinearVelocity),
},
});
}
return(r);
}
public static Apollo.Perception.PerceptionObstacles ApolloConvertFrom(Detected3DObjectData data)
{
var obstacles = new Apollo.Perception.PerceptionObstacles()
{
header = new Apollo.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,
perception_obstacle = new List <Apollo.Perception.PerceptionObstacle>(),
};
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.Type type = Apollo.Perception.Type.UNKNOWN;
if (d.Label == "Pedestrian")
{
type = Apollo.Perception.Type.PEDESTRIAN;
}
else
{
type = Apollo.Perception.Type.VEHICLE;
}
var po = new Apollo.Perception.PerceptionObstacle()
{
id = (int)d.Id,
position = ConvertToApolloPoint(d.Gps),
theta = (90 - d.Heading) * UnityEngine.Mathf.Deg2Rad,
velocity = ConvertToApolloPoint(velocity),
width = size.x,
length = size.y,
height = size.z,
polygon_point = new List <Apollo.Point3D>(),
tracking_time = d.TrackingTime,
type = type,
timestamp = data.Time,
};
// 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 = UnityEngine.Mathf.Cos((float)-d.Heading * UnityEngine.Mathf.Deg2Rad);
var s = UnityEngine.Mathf.Sin((float)-d.Heading * UnityEngine.Mathf.Deg2Rad);
var p1 = new Apollo.Point3D()
{
x = -px * c + py * s + cx, y = -px * s - py * c + cy, z = cz
};
var p2 = new Apollo.Point3D()
{
x = px * c + py * s + cx, y = px * s - py * c + cy, z = cz
};
var p3 = new Apollo.Point3D()
{
x = px * c - py * s + cx, y = px * s + py * c + cy, z = cz
};
var p4 = new Apollo.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);
obstacles.perception_obstacle.Add(po);
}
return(obstacles);
}
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);
}
