public static Vector2 VectorFromGPS(NavSatFixMsg position1, NavSatFixMsg position2)
{
double lon_diff_rad = Mathf.Deg2Rad * (position2.GetLongitude() - position1.GetLongitude());
double a = Math.Pow(Math.Sin(Mathf.Deg2Rad * (position2.GetLatitude() - position1.GetLatitude()) * 0.5), 2) +
Math.Cos(Mathf.Deg2Rad * position1.GetLatitude()) * Math.Cos(Mathf.Deg2Rad * position2.GetLatitude()) *
Math.Pow(Math.Sin(lon_diff_rad * 0.5), 2);
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
double distance = EARTH_RADIUS_METERS * c;
double x = Math.Cos(Mathf.Deg2Rad * position1.GetLatitude()) * Math.Sin(Mathf.Deg2Rad * position2.GetLatitude()) -
Math.Sin(Mathf.Deg2Rad * position1.GetLatitude()) * Math.Cos(Mathf.Deg2Rad * position2.GetLatitude()) *
Math.Cos(lon_diff_rad);
double y = Math.Cos(Mathf.Deg2Rad * position2.GetLatitude()) * Math.Sin(lon_diff_rad);
double bearing_angle = Math.Atan2(y, x);
Vector2 bearing = new Vector2((float)Math.Sin(bearing_angle), (float)Math.Cos(bearing_angle));
return(bearing * (float)distance);
}
// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(init))
{
Drone drone = WorldProperties.SelectNextDrone();
rosDroneConnection = (Matrice_ROSDroneConnection)drone.droneProperties.droneROSConnection;
}
if (Input.GetKeyDown(viewHomeLat))
{
double homeLat = rosDroneConnection.GetHomeLat();
Debug.LogFormat("Home Latitute set to {0}", homeLat);
}
if (Input.GetKeyDown(viewHomeLong))
{
double homeLong = rosDroneConnection.GetHomeLong();
Debug.LogFormat("Home Longitute set to {0}", homeLong);
}
if (printLatLong)
{
NavSatFixMsg gpsPosition = rosDroneConnection.GetGPSPosition();
Debug.LogFormat("Drone Lat,Long : {0},{1}", gpsPosition.GetLatitude(), gpsPosition.GetLongitude());
}
if (printGPSHealth)
{
float gpsHealth = rosDroneConnection.GetGPSHealth();
Debug.LogFormat("GPS Health : {0}", gpsHealth);
}
if (Input.GetKeyUp(hasAuthority))
{
rosDroneConnection.HasAuthority();
}
if (Input.GetKeyUp(getAuthority))
{
rosDroneConnection.SetSDKControl(true);
}
if (Input.GetKeyUp(KeyCode.Space))
{
rosDroneConnection.SetSDKControl(false);
}
if (Input.GetKeyDown(spinMotors))
{
rosDroneConnection.ChangeArmStatusTo(true);
}
if (Input.GetKeyDown(stopMotors))
{
rosDroneConnection.ChangeArmStatusTo(false);
}
if (Input.GetKeyUp(takeoffDrone))
{
rosDroneConnection.ExecuteTask(Matrice_ROSDroneConnection.DroneTask.TAKEOFF);
}
if (Input.GetKeyUp(landDrone))
{
rosDroneConnection.ExecuteTask(Matrice_ROSDroneConnection.DroneTask.LAND);
}
if (Input.GetKeyUp(uploadTestMission))
{
uint[] command_list = new uint[16];
uint[] command_params = new uint[16];
for (int i = 0; i < 16; i++)
{
command_list[i] = 0;
command_params[i] = 0;
}
MissionWaypointMsg test_waypoint_1 = new MissionWaypointMsg(37.915701652f, -122.337967237f, 20.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
MissionWaypointMsg test_waypoint_2 = new MissionWaypointMsg(37.915585270f, -122.338122805f, 20.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
MissionWaypointMsg test_waypoint_3 = new MissionWaypointMsg(37.915457249f, -122.338015517f, 20.0f, 3.0f, 0, 0, MissionWaypointMsg.TurnMode.CLOCKWISE, 0, 30, new MissionWaypointActionMsg(0, command_list, command_params));
Debug.Log("Check float accuracy here" + test_waypoint_1.ToYAMLString());
MissionWaypointMsg[] test_waypoint_array = new MissionWaypointMsg[] { test_waypoint_1, test_waypoint_2, test_waypoint_3 };
MissionWaypointTaskMsg test_Task = new MissionWaypointTaskMsg(15.0f, 15.0f, MissionWaypointTaskMsg.ActionOnFinish.NO_ACTION, 1, MissionWaypointTaskMsg.YawMode.AUTO, MissionWaypointTaskMsg.TraceMode.COORDINATED, MissionWaypointTaskMsg.ActionOnRCLost.FREE, MissionWaypointTaskMsg.GimbalPitchMode.FREE, test_waypoint_array);
rosDroneConnection.UploadWaypointsTask(test_Task);
}
if (Input.GetKeyUp(missionInfo))
{
rosDroneConnection.FetchMissionStatus();
}
if (Input.GetKeyUp(executeUploadedMission))
{
rosDroneConnection.SendWaypointAction(Matrice_ROSDroneConnection.WaypointMissionAction.START);
}
if (Input.GetKeyUp(viewSafeWaypointMission))
{
// 37.915411
// -122.338024
// 37.915159, -122.337983
// LEFT CORNER: 37.915188, -122.337988
// RIGHT CORNER: 37.915262, -122.337984
GameObject waypoint1 = GameObject.CreatePrimitive(PrimitiveType.Sphere);
waypoint1.name = "Waypoint 1";
waypoint1.transform.parent = this.transform;
waypoint1.transform.localPosition = WorldProperties.GPSCoordToUnityCoord(new GPSCoordinate(37.915701652, -122.337967237, 20));
waypoint1.transform.localScale = Vector3.one * (0.05f);
Debug.Log("SANITY CHECK GPS: " + WorldProperties.UnityCoordToGPSCoord(WorldProperties.GPSCoordToUnityCoord(new GPSCoordinate(37.915701652, -122.337967237, 20))).Lat);
Debug.Log("SANITY CHECK GPS: " + WorldProperties.UnityCoordToGPSCoord(WorldProperties.GPSCoordToUnityCoord(new GPSCoordinate(37.915701652, -122.337967237, 20))).Lng);
GameObject waypoint2 = GameObject.CreatePrimitive(PrimitiveType.Sphere);
waypoint2.name = "Waypoint 2";
waypoint2.transform.parent = this.transform;
waypoint2.transform.localPosition = WorldProperties.GPSCoordToUnityCoord(new GPSCoordinate(37.915585270, -122.338122805, 20));
waypoint2.transform.localScale = Vector3.one * (0.05f);
GameObject waypoint3 = GameObject.CreatePrimitive(PrimitiveType.Sphere);
waypoint3.name = "Waypoint 3";
waypoint3.transform.parent = this.transform;
waypoint3.transform.localPosition = WorldProperties.GPSCoordToUnityCoord(new GPSCoordinate(37.915457249, -122.338015517, 20));
waypoint3.transform.localScale = Vector3.one * (0.05f);
}
if (Input.GetKeyUp(uploadUserMission))
{
rosDroneConnection.StartMission();
}
if (Input.GetKeyUp(pauseMission))
{
rosDroneConnection.SendWaypointAction(Matrice_ROSDroneConnection.WaypointMissionAction.PAUSE);
}
if (Input.GetKeyUp(resumeMission))
{
rosDroneConnection.SendWaypointAction(Matrice_ROSDroneConnection.WaypointMissionAction.RESUME);
}
if (Input.GetKeyDown(stopMission))
{
rosDroneConnection.SendWaypointAction(Matrice_ROSDroneConnection.WaypointMissionAction.STOP);
}
if (Input.GetKeyUp(cycleDrone))
{
Drone drone = WorldProperties.SelectNextDrone();
rosDroneConnection = (Matrice_ROSDroneConnection)drone.droneProperties.droneROSConnection;
}
if (Input.GetKeyDown(cycleSensor))
{
WorldProperties.sensorManager.ShowNextSensor();
}
if (Input.GetKeyDown(unsubscribe))
{
ROSSensorConnectionInterface sensor = WorldProperties.sensorManager.getSelectedSensor();
Dictionary <string, bool> subscriberDict = WorldProperties.sensorManager.getSubscriberDict();
foreach (string topic in subscriberDict.Keys)
{
sensor.Unsubscribe(topic);
}
}
}
/// <summary>
/// Convert the given ROS NavSatFixMsg to Unity XYZ space.
/// To Be used to convert drone coordinates to unity space
/// </summary>
/// <returns>Unity position vector to use within World GameObject</returns>
public static Vector3 ROSCoordToUnityCoord(NavSatFixMsg gpsPosition)
{
return(GPSCoordToUnityCoord(new GPSCoordinate(gpsPosition.GetLatitude(), gpsPosition.GetLongitude(), gpsPosition.GetAltitude())));
}