public List <Vector3> ConvertUTMFromDouble3(List <double3> double3List)
{
var vector3List = new List <Vector3>();
foreach (var point in double3List)
{
var vector3 = MapOrigin.FromNorthingEasting(point.y, point.x, false, false);
vector3.y = (float)point.z;
vector3List.Add(vector3);
}
return(vector3List);
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddReader <Ros.Vector3>(ResetTopic, msg =>
{
var position = MapOrigin.FromNorthingEasting(msg.y, msg.x);
int mask = 1 << LayerMask.NameToLayer("Ground And Road");
RaycastHit hit;
if (Physics.Raycast(position + new Vector3(0, 100, 0), new Vector3(0, -1, 0), out hit, Mathf.Infinity, mask))
{
position = hit.point;
position.y += 0.01f;
}
else
{
position.y += 20.0f;
}
var angle = (float)msg.z * Mathf.Rad2Deg - MapOrigin.Angle;
var rotation = Quaternion.AngleAxis(angle, Vector3.up);
// reset position, rotation, velocity and angular velocity
GpsDevice.Agent.GetComponent <VehicleInputController>().controller.ResetSavedPosition(position, rotation);
});
};
}
