コード例 #1
0
            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]);
                }
            }
コード例 #2
0
    // 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);
            }
        }
    }