public static string ToLatLongString(this NavSatFixMsg message)
{
string lat = (message.latitude > 0) ? "ºN" : "ºS";
string lon = (message.longitude > 0) ? "ºE" : "ºW";
return($"{message.latitude}{lat} {message.longitude}{lon}");
}
/// <param name="data">X: Angular speed in meter/s, Y: Linear speed in meter/s</param>
public void PublishData(GeoPointWGS84 data)
{
if (_publisher == null)
{
return;
}
NavSatFixMsg navSatFix = new NavSatFixMsg(data.latitude, data.longitude, data.altitude);
_publisher.PublishData(navSatFix);
}
public void ReceivedWaypoint(ROSBridgeMsg newGoal)
{
NavSatFixMsg waypoint = (NavSatFixMsg)newGoal;
GeoPointWGS84 coordinate = new GeoPointWGS84()
{
latitude = waypoint._latitude,
longitude = waypoint._longitude,
altitude = waypoint._altitude
};
goal = coordinate.ToUTM().ToUnity();
goal_set = true;
subState = "STOP";
}
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);
}
public ROSLocomotionWaypoint(AgentJob job, ROSBridgeWebSocketConnection rosConnection, string topicName)
{
if (job == AgentJob.Publisher)
{
_publisher = new ROSGenericPublisher(rosConnection, topicName, NavSatFixMsg.GetMessageType());
rosConnection.AddPublisher(_publisher);
}
else if (job == AgentJob.Subscriber)
{
_subscriber = new ROSGenericSubscriber <NavSatFixMsg>(rosConnection, topicName, NavSatFixMsg.GetMessageType(), (msg) => new NavSatFixMsg(msg));
_subscriber.OnDataReceived += (data) =>
{
if (OnDataReceived != null)
{
OnDataReceived(data);
}
};
rosConnection.AddSubscriber(_subscriber);
}
}
// Update is called once per frame
void Update()
{
if (dji_sdk == null)
{
dji_sdk = GetComponent <DJI_SDK>();
}
if (dji_sdk != null && dji_sdk.sdk_ready)
{
if (origin_position == null)
{
origin_position = dji_sdk.GetGPSPosition();
}
else
{
Vector3 ground_displacement = DJI_SDK.VectorFromGPS(origin_position, dji_sdk.GetGPSPosition());
transform.localPosition = new Vector3(ground_displacement.x, dji_sdk.GetHeightAboveTakeoff(), ground_displacement.y);
}
transform.localRotation = dji_sdk.GetAttitude();
//if (Input.GetKeyUp(KeyCode.W))
//{
// dji_sdk.PublishRelativeSetPoint(new Vector2(0, 1), dji_sdk.GetHeightAboveTakeoff(), 0);
//} else if (Input.GetKeyUp(KeyCode.A))
//{
// dji_sdk.PublishRelativeSetPoint(new Vector2(-1, 0), dji_sdk.GetHeightAboveTakeoff(), 0);
//} else if (Input.GetKeyUp(KeyCode.S))
//{
// dji_sdk.PublishRelativeSetPoint(new Vector2(0, -1), dji_sdk.GetHeightAboveTakeoff(), 0);
//} else if (Input.GetKeyUp(KeyCode.D))
//{
// dji_sdk.PublishRelativeSetPoint(new Vector2(1, 0), dji_sdk.GetHeightAboveTakeoff(), 0);
//}
if (Input.GetKey(KeyCode.W))
{
dji_sdk.PublishRateSetpoint(0, 5, dji_sdk.GetHeightAboveTakeoff(), 0);
}
else if (Input.GetKey(KeyCode.A))
{
dji_sdk.PublishRateSetpoint(-5, 0, dji_sdk.GetHeightAboveTakeoff(), 0);
}
else if (Input.GetKey(KeyCode.S))
{
dji_sdk.PublishRateSetpoint(0, -5, dji_sdk.GetHeightAboveTakeoff(), 0);
}
else if (Input.GetKey(KeyCode.D))
{
dji_sdk.PublishRateSetpoint(5, 0, dji_sdk.GetHeightAboveTakeoff(), 0);
}
else
{
dji_sdk.PublishRateSetpoint(0, 0, dji_sdk.GetHeightAboveTakeoff(), 0);
}
if (Input.GetKeyUp(KeyCode.Space))
{
if (!dji_sdk.HasAuthority())
{
dji_sdk.SetSDKControl(true);
}
else if (dji_sdk.GetFlightStatusM100() == DJI_SDK.FlightStatusM100.ON_GROUND_STANDBY)
{
dji_sdk.ExecuteTask(DJI_SDK.DroneTask.TAKEOFF);
}
else if (dji_sdk.GetFlightStatusM100() == DJI_SDK.FlightStatusM100.IN_AIR_STANDBY)
{
dji_sdk.ExecuteTask(DJI_SDK.DroneTask.LAND);
}
}
else if (Input.GetKeyUp(KeyCode.Escape))
{
if (dji_sdk.HasAuthority())
{
dji_sdk.ExecuteTask(DJI_SDK.DroneTask.GO_HOME);
}
}
else if (Input.GetKeyUp(KeyCode.R))
{
if (dji_sdk.HasAuthority())
{
dji_sdk.SetSDKControl(false);
}
}
}
}
// 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())));
}
public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMsg parsed = null)
{
ROSBridgeMsg result = null;
// Writing all code in here for now. May need to move out to separate handler functions when it gets too unwieldy.
switch (topic)
{
case "/dji_sdk/attitude":
QuaternionMsg attitudeMsg = (parsed == null) ? new QuaternionMsg(raw_msg["quaternion"]) : (QuaternionMsg)parsed;
attitude = offset * (new Quaternion(attitudeMsg.GetX(), attitudeMsg.GetY(), attitudeMsg.GetZ(), attitudeMsg.GetW()));
result = attitudeMsg;
break;
case "/dji_sdk/battery_state":
battery_state = (parsed == null) ? new BatteryStateMsg(raw_msg) : (BatteryStateMsg)parsed;
result = battery_state;
break;
case "/dji_sdk/flight_status":
if (isM100)
{
m100_flight_status = (FlightStatusM100)(new UInt8Msg(raw_msg)).GetData();
}
else
{
flight_status = (FlightStatus)(new UInt8Msg(raw_msg)).GetData();
}
break;
case "/dji_sdk/gimbal_angle":
Vector3Msg gimbleAngleMsg = (parsed == null) ? new Vector3Msg(raw_msg["vector"]) : (Vector3Msg)parsed;
gimble_joint_angles = new Vector3((float)gimbleAngleMsg.GetX(), (float)gimbleAngleMsg.GetY(), (float)gimbleAngleMsg.GetZ());
result = gimbleAngleMsg;
break;
case "/dji_sdk/gps_health":
gps_health = (parsed == null) ? (new UInt8Msg(raw_msg)).GetData() : ((UInt8Msg)parsed).GetData();
break;
case "/dji_sdk/gps_position":
gps_position = (parsed == null) ? new NavSatFixMsg(raw_msg) : (NavSatFixMsg)parsed;
result = gps_position;
break;
case "/dji_sdk/imu":
imu = (parsed == null) ? new IMUMsg(raw_msg) : (IMUMsg)parsed;
result = imu;
break;
case "/dji_sdk/rc":
remote_controller_msg = (parsed == null) ? new JoyMsg(raw_msg) : (JoyMsg)parsed;
result = remote_controller_msg;
break;
case "/dji_sdk/velocity":
Vector3Msg velocityMsg = (parsed == null) ? new Vector3Msg(raw_msg["vector"]) : (Vector3Msg)parsed;
velocity = new Vector3((float)velocityMsg.GetX(), (float)velocityMsg.GetY(), (float)velocityMsg.GetZ());
result = velocityMsg;
break;
case "/dji_sdk/height_above_takeoff":
relative_altitude = (parsed == null) ? (new Float32Msg(raw_msg)).GetData() : ((Float32Msg)parsed).GetData();
break;
case "/dji_sdk/local_position":
PointMsg pointMsg = (parsed == null) ? new PointMsg(raw_msg["point"]) : (PointMsg)parsed;
local_position = new Vector3(pointMsg.GetX(), pointMsg.GetY(), pointMsg.GetZ());
result = pointMsg;
Debug.Log(result);
break;
default:
Debug.LogError("Topic not implemented: " + topic);
break;
}
return(result);
}
