First try Take off and landing approach

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp

@@ -103,6 +103,9 @@ void MainWindow::updateBatteryVoltage(float battery_voltage)
     m_battery_voltage = battery_voltage;
     // Need to take voltage, display it and transform it to percentage
     ui->battery_bar->setValue(fromVoltageToPercent(m_battery_voltage));
+    QString qstr = "Raw voltage: ";
+    qstr.append(QString::number(battery_voltage));
+    ui->raw_voltage->setText(qstr);
 }
 
 void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui

@@ -28,7 +28,14 @@
        <string>StudentID # connected to CF #</string>
       </property>
       <layout class="QGridLayout" name="gridLayout_2">
-       <item row="2" column="1">
+       <item row="0" column="3">
+        <widget class="QLabel" name="raw_voltage">
+         <property name="text">
+          <string>Raw voltage: </string>
+         </property>
+        </widget>
+       </item>
+       <item row="3" column="1">
         <widget class="QGroupBox" name="groupBox_2">
          <property name="title">
           <string>Current Position</string>
@@ -72,21 +79,21 @@
          </property>
         </widget>
        </item>
-       <item row="1" column="2">
+       <item row="2" column="3">
         <widget class="QProgressBar" name="battery_bar">
          <property name="value">
           <number>24</number>
          </property>
         </widget>
        </item>
-       <item row="1" column="1">
+       <item row="2" column="1">
         <widget class="QLabel" name="label">
          <property name="text">
           <string>Battery level</string>
          </property>
         </widget>
        </item>
-       <item row="2" column="2">
+       <item row="3" column="3">
         <widget class="QGroupBox" name="groupBox_3">
          <property name="title">
           <string>Controller and current set point</string>

pps_ws/src/d_fall_pps/msg/CrazyflieData.msg

@@ -7,4 +7,3 @@ float64 pitch
 float64 yaw
 float64 acquiringTime #delta t
 bool occluded
-

pps_ws/src/d_fall_pps/scripts/disable_crazyflie → pps_ws/src/d_fall_pps/scripts/motors_off_crazyflie

 #!/bin/bash
 
 if [ "$#" -ne 0 ] 
-then echo "usage: disable_crazyflie <no arguments>"
-else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 4;
+then echo "usage: motors_off_crazyflie <no arguments>"
+else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 5;
 fi
 

pps_ws/src/d_fall_pps/scripts/enable_crazyflie → pps_ws/src/d_fall_pps/scripts/take_off_crazyflie

 #!/bin/bash
 
 if [ "$#" -ne 0 ]
-then echo "usage: enable_crazyflie <no arguments>"
+then echo "usage: take_off crazyfly <no arguments>"
 else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 3;
 fi
 

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -27,15 +27,27 @@
 #include "d_fall_pps/CrazyflieData.h"
 #include "d_fall_pps/ControlCommand.h"
 #include "d_fall_pps/CrazyflieContext.h"
+#include "d_fall_pps/Setpoint.h"
 #include "std_msgs/Int32.h"
 
 
 #include "d_fall_pps/ControlCommand.h"
 
-#define CMD_USE_SAFE_CONTROLLER 1
+#define CMD_USE_SAFE_CONTROLLER   1
 #define CMD_USE_CUSTOM_CONTROLLER 2
-#define CMD_USE_CRAZYFLY_ENABLE 3
-#define CMD_USE_CRAZYFLY_DISABLE 4
+#define CMD_CRAZYFLY_TAKE_OFF     3
+#define CMD_CRAZYFLY_LAND         4
+#define CMD_CRAZYFLY_MOTORS_OFF   5
+
+// Flying states
+
+#define STATE_MOTORS_OFF 1
+#define STATE_TAKE_OFF   2
+#define STATE_FLYING     3
+#define STATE_LAND       4
+
+#define TAKE_OFF_OFFSET  0.7    //in meters
+#define LANDING_DISTANCE 0.3    //in meters
 
 #define PI 3.141592653589
 
@@ -48,6 +60,7 @@ int studentID;
 ros::ServiceClient safeController;
 //the custom controller specified in the ClientConfig.yaml, is considered potentially unstable
 ros::ServiceClient customController;
+
 //values for safteyCheck
 bool strictSafety;
 float angleMargin;
@@ -55,15 +68,49 @@ float angleMargin;
 ros::ServiceClient centralManager;
 ros::Publisher controlCommandPublisher;
 
+// communicate with safeControllerService, setpoint, etc...
+ros::Publisher safeControllerServiceSetpointPublisher;
+
 rosbag::Bag bag;
 
+// variables for the states:
+int flying_state;
+bool changed_state_flag;
+
 //describes the area of the crazyflie and other parameters
 CrazyflieContext context;
 
 //wheter to use safe of custom controller
 bool usingSafeController;
-//wheter crazyflie is enabled (ready to fly) or disabled (motors off)
-bool crazyflieEnabled;
+
+
+void loadSafeController() {
+	ros::NodeHandle nodeHandle("~");
+
+	std::string safeControllerName;
+	if(!nodeHandle.getParam("safeController", safeControllerName)) {
+		ROS_ERROR("Failed to get safe controller name");
+		return;
+	}
+
+	ros::service::waitForService(safeControllerName);
+	safeController = ros::service::createClient<Controller>(safeControllerName, true);
+    ROS_INFO_STREAM("loaded safe controller: " << safeController.getService());
+}
+
+void loadCustomController() {
+	ros::NodeHandle nodeHandle("~");
+
+	std::string customControllerName;
+	if(!nodeHandle.getParam("customController", customControllerName)) {
+		ROS_ERROR("Failed to get custom controller name");
+		return;
+	}
+
+	customController = ros::service::createClient<Controller>(customControllerName, true);
+    ROS_INFO_STREAM("loaded custom controller " << customControllerName);
+}
+
 
 //checks if crazyflie is within allowed area and if custom controller returns no data
 bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
@@ -111,37 +158,143 @@ void coordinatesToLocal(CrazyflieData& cf) {
 	cf.z -= originZ;
 }
 
+
+
+void takeOffCF(CrazyflieData& current_local_coordinates) //local because the setpoint is in local coordinates
+{
+    // set the setpoint and call safe controller
+    Setpoint setpoint_msg;
+    setpoint_msg.x = current_local_coordinates.x;           // previous one
+    setpoint_msg.y = current_local_coordinates.y;           //previous one
+    setpoint_msg.z = current_local_coordinates.z + TAKE_OFF_OFFSET;           //previous one plus some offset
+    setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
+    safeControllerServiceSetpointPublisher.publish(setpoint_msg);
+
+    // now, use safe controller to go to that setpoint
+    usingSafeController = true;
+    loadSafeController();
+    // when do we finish? after some time with delay?
+}
+
+void landCF(CrazyflieData& current_local_coordinates)
+{
+    // set the setpoint and call safe controller
+    Setpoint setpoint_msg;
+    setpoint_msg.x = current_local_coordinates.x;           // previous one
+    setpoint_msg.y = current_local_coordinates.y;           //previous one
+    setpoint_msg.z = LANDING_DISTANCE;           //previous one plus some offset
+    setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
+    safeControllerServiceSetpointPublisher.publish(setpoint_msg);
+
+    // now, use safe controller to go to that setpoint
+    usingSafeController = true;
+    loadSafeController();
+}
+
+void changeFlyingStateTo(int new_state)
+{
+    flying_state = new_state;
+    changed_state_flag = true;
+}
+
+int counter = 0;
+bool finished_take_off = false;
+bool finished_land = false;
+
 //is called when new data from Vicon arrives
 void viconCallback(const ViconData& viconData) {
 	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
 		CrazyflieData global = *it;
 
-        if(!global.occluded || !crazyflieEnabled)    //if it is not occluded, then proceed to compute the controller output. If the command is disabled, even if it is occluded, disable crazyflie
+        if(global.crazyflieName == context.crazyflieName)
         {
-            if(global.crazyflieName == context.crazyflieName) {
-                Controller controllerCall;
+            Controller controllerCall;
+            CrazyflieData local = global;
+            coordinatesToLocal(local);
+            controllerCall.request.ownCrazyflie = local;
+
+            switch(flying_state) //things here repeat every X ms, non-blocking stuff
+            {
+                case STATE_MOTORS_OFF: // here we will put the code of current disabled button
+                    if(changed_state_flag) // stuff that will be run only once when changing state
+                    {
+                        changed_state_flag = false;
+                    }
+                    break;
+                case STATE_TAKE_OFF: //we need to move up from where we are now.
+                    if(changed_state_flag) // stuff that will be run only once when changing state
+                    {
+                        changed_state_flag = false;
+                        takeOffCF(local);
+                        counter = 0;
+                        finished_take_off = false;
+                    }
+                    counter++;
+                    if(counter >= 200)
+                    {
+                        counter = 0;
+                        finished_take_off = true;
+                    }
+                    if(finished_take_off)
+                    {
+                        finished_take_off = false;
+                        changeFlyingStateTo(STATE_FLYING);
+                    }
+                    break;
+                case STATE_FLYING:
+                    if(changed_state_flag) // stuff that will be run only once when changing state
+                    {
+                        changed_state_flag = false;
+                    }
+                    break;
+                case STATE_LAND:
+                    if(changed_state_flag) // stuff that will be run only once when changing state
+                    {
+                        changed_state_flag = false;
+                        landCF(local);
+                        counter = 0;
+                        finished_land = false;
+                    }
+                    counter++;
+                    if(counter >= 200)
+                    {
+                        counter = 0;
+                        finished_land = true;
+                    }
+                    if(finished_land)
+                    {
+                        finished_land = false;
+                        changeFlyingStateTo(STATE_MOTORS_OFF);
+                    }
+                    break;
+                default:
+                    break;
+            }
 
-                CrazyflieData local = global;
-                coordinatesToLocal(local);
-                controllerCall.request.ownCrazyflie = local;
 
-                if(crazyflieEnabled){
-                    if(!usingSafeController) {
+            // control part that should always be running, calls to controller, computation of control output
+            if(flying_state != STATE_MOTORS_OFF)
+            {
+                if(!global.occluded)    //if it is not occluded, then proceed to compute the controller output.
+                {
+                    if(!usingSafeController && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
+                    {
                         bool success = customController.call(controllerCall);
-
-                        if(!success) {
+                        if(!success)
+                        {
                             ROS_ERROR("Failed to call custom controller, switching to safe controller");
                             ROS_ERROR_STREAM("custom controller status: valid: " << customController.isValid() << ", exists: " << customController.exists());
                             ROS_ERROR_STREAM("custom controller name: " << customController.getService());
                             usingSafeController = true;
-                        } else if(!safetyCheck(global, controllerCall.response.controlOutput)) {
+                        }
+                        else if(!safetyCheck(global, controllerCall.response.controlOutput))
+                        {
                             usingSafeController = true;
                             ROS_INFO_STREAM("safety check failed, switching to safe controller");
                         }
                     }
-
-
-                    if(usingSafeController) {
+                    else
+                    {
                         bool success = safeController.call(controllerCall);
                         if(!success) {
                             ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
@@ -154,15 +307,16 @@ void viconCallback(const ViconData& viconData) {
 
                     bag.write("ViconData", ros::Time::now(), local);
                     bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
-
-                } else { //crazyflie disabled
-                    ControlCommand zeroOutput = ControlCommand(); //everything set to zero
-                    zeroOutput.onboardControllerType = 2; //set to motor_mode
-                    controlCommandPublisher.publish(zeroOutput);
-                    bag.write("ViconData", ros::Time::now(), local);
-                    bag.write("ControlOutput", ros::Time::now(), zeroOutput);
                 }
             }
+            else
+            {
+                ControlCommand zeroOutput = ControlCommand(); //everything set to zero
+                zeroOutput.onboardControllerType = 2; //set to motor_mode
+                controlCommandPublisher.publish(zeroOutput);
+                bag.write("ViconData", ros::Time::now(), local);
+                bag.write("ControlOutput", ros::Time::now(), zeroOutput);
+            }
         }
 	}
 }
@@ -201,35 +355,11 @@ void loadCrazyflieContext() {
 	nh.setParam("crazyFlieAddress", context.crazyflieAddress);
 }
 
-void loadSafeController() {
-	ros::NodeHandle nodeHandle("~");
-
-	std::string safeControllerName;
-	if(!nodeHandle.getParam("safeController", safeControllerName)) {
-		ROS_ERROR("Failed to get safe controller name");
-		return;
-	}
-
-	ros::service::waitForService(safeControllerName);
-	safeController = ros::service::createClient<Controller>(safeControllerName, true);
-    ROS_INFO_STREAM("loaded safe controller: " << safeController.getService());
-}
-
-void loadCustomController() {
-	ros::NodeHandle nodeHandle("~");
-
-	std::string customControllerName;
-	if(!nodeHandle.getParam("customController", customControllerName)) {
-		ROS_ERROR("Failed to get custom controller name");
-		return;
-	}
 
-	customController = ros::service::createClient<Controller>(customControllerName, true);
-    ROS_INFO_STREAM("loaded custom controller " << customControllerName);
-}
 
 void commandCallback(const std_msgs::Int32& commandMsg) {
 	int cmd = commandMsg.data;
+
 	switch(cmd) {
     	case CMD_USE_SAFE_CONTROLLER:
     		loadSafeController();
@@ -241,13 +371,16 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
     		usingSafeController = false;
     		break;
 
-    	case CMD_USE_CRAZYFLY_ENABLE:
-    		crazyflieEnabled = true;
+    	case CMD_CRAZYFLY_TAKE_OFF:
+            changeFlyingStateTo(STATE_TAKE_OFF);
     		break;
 
-    	case CMD_USE_CRAZYFLY_DISABLE:
-    		crazyflieEnabled = false;
+    	case CMD_CRAZYFLY_LAND:
+            changeFlyingStateTo(STATE_LAND);
     		break;
+        case CMD_CRAZYFLY_MOTORS_OFF:
+            changeFlyingStateTo(STATE_MOTORS_OFF);
+            break;
 
     	default:
     		ROS_ERROR_STREAM("unexpected command number: " << cmd);
@@ -255,7 +388,8 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
 	}
 }
 
-int main(int argc, char* argv[]){
+int main(int argc, char* argv[])
+{
 	ros::init(argc, argv, "PPSClient");
 	ros::NodeHandle nodeHandle("~");
 	loadParameters(nodeHandle);
@@ -279,8 +413,12 @@ int main(int argc, char* argv[]){
     //this topic lets us use the terminal to communicate with crazyRadio node.
     ros::Publisher crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
 
+    // SafeControllerServicePublisher:
+    ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
+    safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);
+
 	//start with safe controller
-	crazyflieEnabled = false;
+    flying_state = STATE_MOTORS_OFF;
 	usingSafeController = true;
 	loadSafeController();