public MissionWaypointTaskMsg(JSONNode msg)
{
_velocity_range = msg["velocity_range"].AsFloat;
_idle_velocity = msg["idle_velocity"].AsFloat;
_action_on_finish = (ActionOnFinish)msg["action_on_finish"].AsInt;
_mission_exec_times = (uint)msg["mission_exec_times"].AsInt;
_yaw_mode = (YawMode)msg["yaw_mode"].AsInt;
_trace_mode = (TraceMode)msg["trace_mode"].AsInt;
_action_on_rc_lost = (ActionOnRCLost)msg["action_on_rc_lost"].AsInt;
_gimbal_pitch_mode = (GimbalPitchMode)msg["gimbal_pitch_mode"].AsInt;
JSONArray waypoints = msg["mission_waypoint"].AsArray;
_mission_waypoints = new MissionWaypointMsg[waypoints.Count];
for (int i = 0; i < _mission_waypoints.Length; i++)
{
_mission_waypoints[i] = new MissionWaypointMsg(waypoints[i]);
}
}
// 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);
}
}
}
