/// <summary>
/// update the ui with the current selected sensor, upper bound of 6 subscribers per sensor
/// </summary>
public void updateSensorUI(ROSSensorConnectionInterface inputSensor)
{
deleteOldSensors();
sensorText.text = inputSensor.GetSensorName();
subscriberDict = new Dictionary <string, bool>(inputSensor.GetSensorSubscribers());
int subscribercount = 0;
foreach (string subscriber in subscriberDict.Keys)
{
Debug.Log("Creating button for :" + subscriber);
//Instantiating and positioning toggles
float ypos = 60 - (subscribercount * 40);
var position = new Vector3(-19, ypos, -14);
GameObject toggleUI = Instantiate(togglePrefab, new Vector3(0, 0, 0), Quaternion.identity);
toggleUI.transform.parent = sensorMenu.transform;
toggleUI.transform.localPosition = position;
toggleUI.transform.localScale = Vector3.one * 1.3f;
toggleUI.transform.localRotation = Quaternion.identity;
toggleUI.tag = "TOGGLESENSORS";
//making toggles w/ correct labels and subscribers
Toggle thisToggle = toggleUI.GetComponent <Toggle>();
Text toggleName = toggleUI.GetComponentInChildren <Text>();
toggleName.text = subscriber;
thisToggle.isOn = subscriberDict[subscriber];
thisToggle.onValueChanged.AddListener(delegate { ToggleValueChanged(thisToggle); });
subscribercount++;
}
}
public void ShowPreviousSensor()
{
if (selectedSensorPos == 0)
{
selectedSensorPos = sensorList.Count - 1;
}
else
{
selectedSensorPos -= 1;
}
selectedSensor = sensorList[selectedSensorPos];
updateSensorUI(selectedSensor);
}
/// <summary>
/// Cycle sensor/ display next sensor
/// </summary>
public void ShowNextSensor()
{
if (selectedSensorPos == sensorList.Count - 1)
{
selectedSensorPos = 0;
}
else
{
selectedSensorPos += 1;
}
selectedSensor = sensorList[selectedSensorPos];
updateSensorUI(selectedSensor);
}
// function 1: new drone selected: update all the sensors
// UpdateUI function: paramaters: List of Sensors
// store the list of sensors as current sensors
// selected sensor is the first sensor
// update the ui with the first sensor's subscribers.
public void initializeSensorUI(List <ROSSensorConnectionInterface> allSensors)
{
sensorList.Clear();
deleteOldSensors();
if (allSensors.Count == 0)
{
Debug.Log("No sensors");
sensorText.text = "No attached sensors";
return;
}
foreach (ROSSensorConnectionInterface sensor in allSensors)
{
Debug.Log("Adding sensor to UI: " + sensor.GetSensorName());
sensorList.Add(sensor);
}
Debug.Log("Selecting sensor: " + sensorList[0].GetSensorName());
selectedSensor = sensorList[0];
selectedSensorPos = 0;
updateSensorUI(selectedSensor);
}
// 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>
/// Create a Drone gameobject and attach DroneFlightSim, required ROSDroneConnnection and initialize the ROS connection.
/// </summary>
/// <param name="rosDroneConnectionInput"></param>
private void InstantiateDrone(ROSDroneConnectionInput rosDroneConnectionInput)
{
// All the variables required to create the drone
DroneType droneType = rosDroneConnectionInput.droneType;
string droneIP = rosDroneConnectionInput.url;
int dronePort = rosDroneConnectionInput.port;
bool simFlight = rosDroneConnectionInput.simFlight;
List <string> droneSubscribers = new List <string>();
foreach (DroneSubscribers subscriber in rosDroneConnectionInput.droneSubscribers)
{
droneSubscribers.Add(subscriber.ToString());
}
// Create a new drone
Drone droneInstance = new Drone(WorldProperties.worldObject.transform.position, uniqueID);
Debug.Log("Drone that was just made " + droneInstance.gameObjectPointer.name);
DroneProperties droneProperties = droneInstance.droneProperties;
GameObject droneGameObject = droneInstance.gameObjectPointer;
droneGameObject.name = rosDroneConnectionInput.droneName;
ROSDroneConnectionInterface rosDroneConnection = null;
// Add corresponding ros drone connection script
switch (droneType)
{
case DroneType.M100:
Debug.Log("M100 created");
M100_ROSDroneConnection M100_rosDroneConnection = droneGameObject.AddComponent <M100_ROSDroneConnection>();
M100_rosDroneConnection.InitilizeDrone(uniqueID, droneIP, dronePort, droneSubscribers, simFlight, droneProperties);
rosDroneConnection = M100_rosDroneConnection;
droneGameObject.GetComponent <DroneProperties>().droneROSConnection = M100_rosDroneConnection;
ROSDroneConnections.Add(uniqueID, M100_rosDroneConnection);
break;
case DroneType.M210:
Debug.Log("M210 created");
M210_ROSDroneConnection M210_rosDroneConnection = droneGameObject.AddComponent <M210_ROSDroneConnection>();
M210_rosDroneConnection.InitilizeDrone(uniqueID, droneIP, dronePort, droneSubscribers, simFlight, droneProperties);
rosDroneConnection = M210_rosDroneConnection;
droneGameObject.GetComponent <DroneProperties>().droneROSConnection = M210_rosDroneConnection;
ROSDroneConnections.Add(uniqueID, M210_rosDroneConnection);
break;
case DroneType.M600:
Debug.Log("M600 created");
M600_ROSDroneConnection M600_rosDroneConnection = droneGameObject.AddComponent <M600_ROSDroneConnection>();
M600_rosDroneConnection.InitilizeDrone(uniqueID, droneIP, dronePort, droneSubscribers, simFlight, droneProperties);
rosDroneConnection = M600_rosDroneConnection;
droneGameObject.GetComponent <DroneProperties>().droneROSConnection = M600_rosDroneConnection;
ROSDroneConnections.Add(uniqueID, M600_rosDroneConnection);
break;
case DroneType.Sprite:
Debug.Log("Sprite class not implemented created");
//Sprite_ROSDroneConnection drone_rosDroneConnection = drone.AddComponent<Sprite_ROSDroneConnection>();
break;
default:
Debug.Log("No drone type selected");
return;
}
// Create attached sensors
foreach (ROSSensorConnectionInput rosSensorInput in rosDroneConnectionInput.attachedSensors)
{
ROSSensorConnectionInterface sensor = InstantiateSensor(rosSensorInput);
droneInstance.AddSensor(sensor);
}
// Initilize drone sim manager script on the drone
DroneSimulationManager droneSim = droneGameObject.GetComponent <DroneSimulationManager>();
droneSim.InitDroneSim();
droneProperties.droneSimulationManager = droneSim;
// Get DroneMenu and instansiate.
DroneMenu droneMenu = droneGameObject.GetComponent <DroneMenu>();
droneMenu.InitDroneMenu(rosDroneConnection, droneSubscribers);
droneGameObject.GetComponent <DroneProperties>().droneMenu = droneMenu;
uniqueID++;
}
/// <summary>
/// Create a Sensor gameobject and attach & init required ROSSensorConnnection.
/// </summary>
/// <param name="rosSensorConnectionInput"></param>
private ROSSensorConnectionInterface InstantiateSensor(ROSSensorConnectionInput rosSensorConnectionInput)
{
SensorType sensorType = rosSensorConnectionInput.sensorType;
string sensorIP = rosSensorConnectionInput.url;
int sensorPort = rosSensorConnectionInput.port;
List <string> sensorSubscribers = new List <string>();
ROSSensorConnectionInterface rosSensorConnection = null;
foreach (SensorSubscribers subscriber in rosSensorConnectionInput.sensorSubscribers)
{
sensorSubscribers.Add(subscriber.ToString());
}
GameObject sensor = new GameObject(rosSensorConnectionInput.sensorName);
sensor.transform.parent = this.transform;
switch (sensorType)
{
case SensorType.PointCloud:
Debug.Log("PointCloud Sensor created");
PointCloudSensor_ROSSensorConnection pcSensor_rosSensorConnection = sensor.AddComponent <PointCloudSensor_ROSSensorConnection>();
pcSensor_rosSensorConnection.InitilizeSensor(uniqueID, sensorIP, sensorPort, sensorSubscribers);
ROSSensorConnections.Add(uniqueID, pcSensor_rosSensorConnection);
rosSensorConnection = pcSensor_rosSensorConnection;
break;
case SensorType.Mesh:
Debug.Log("Mesh Sensor created");
MeshSensor_ROSSensorConnection meshSensor_rosSensorConnection = sensor.AddComponent <MeshSensor_ROSSensorConnection>();
meshSensor_rosSensorConnection.InitilizeSensor(uniqueID, sensorIP, sensorPort, sensorSubscribers);
ROSSensorConnections.Add(uniqueID, meshSensor_rosSensorConnection);
rosSensorConnection = meshSensor_rosSensorConnection;
break;
case SensorType.LAMP:
Debug.Log("LAMP Sensor created");
LampSensor_ROSSensorConnection lamp_rosSensorConnection = sensor.AddComponent <LampSensor_ROSSensorConnection>();
lamp_rosSensorConnection.InitilizeSensor(uniqueID, sensorIP, sensorPort, sensorSubscribers);
ROSSensorConnections.Add(uniqueID, lamp_rosSensorConnection);
rosSensorConnection = lamp_rosSensorConnection;
break;
case SensorType.PCFace:
Debug.Log("PCFace Sensor created");
PCFaceSensor_ROSSensorConnection pcFace_rosSensorConnection = sensor.AddComponent <PCFaceSensor_ROSSensorConnection>();
pcFace_rosSensorConnection.InitilizeSensor(uniqueID, sensorIP, sensorPort, sensorSubscribers);
ROSSensorConnections.Add(uniqueID, pcFace_rosSensorConnection);
rosSensorConnection = pcFace_rosSensorConnection;
break;
case SensorType.Image:
Debug.Log("Camera Stream created");
CameraSensor_ROSSensorConnection camera_rosSensorConnection = sensor.AddComponent <CameraSensor_ROSSensorConnection>();
VideoPlayers videoType = rosSensorConnectionInput.videoPlayers;
switch (videoType)
{
case VideoPlayers.BackVideo:
camera_rosSensorConnection.SetVideoType("BackVideo");
break;
case VideoPlayers.FrontVideo:
camera_rosSensorConnection.SetVideoType("FrontVideo");
break;
case VideoPlayers.LeftVideo:
camera_rosSensorConnection.SetVideoType("LeftVideo");
break;
case VideoPlayers.RightVideo:
camera_rosSensorConnection.SetVideoType("RightVideo");
break;
default:
Debug.Log("No Video Type Selected.");
return(null);
}
camera_rosSensorConnection.InitilizeSensor(uniqueID, sensorIP, sensorPort, sensorSubscribers);
ROSSensorConnections.Add(uniqueID, camera_rosSensorConnection);
rosSensorConnection = camera_rosSensorConnection;
break;
default:
Debug.Log("No sensor type selected");
return(null);
}
// Add sensor to list of sensors in World Properties
WorldProperties.AddSensor(uniqueID, sensor);
uniqueID++;
return(rosSensorConnection);
}
/// <summary>
/// Add a sensor attached to this drone instance.
/// </summary>
/// <param name="sensor"></param>
public void AddSensor(ROSSensorConnectionInterface sensor)
{
Debug.Log("Adding sensor: " + sensor.GetSensorName());
attachedSensors.Add(sensor);
}