added a z range to the AreaBounds in the GUI, added angle safety feature to PPSClient

pps_wiki/ros_structure.md

@@ -39,6 +39,8 @@ The ``PPSClient`` subscribes to a local topic with which a certain controller ca
 In file ``ClientConfig.yaml``:
 - ``safeController``: the relative ros path to the safe controller
 - ``customController``: the relative ros path to the custom controller
+- ``strictSafety``: turns angle safety check on and off
+- ``angleMargin``: defines maximal allowed angle when strict safety is on, 1 means 90 degrees
 
 ### CrazyRadio
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp

@@ -739,6 +739,9 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
                 tmp_entry.crazyflieContext.localArea.xmax = x_max * FROM_UNITS_TO_METERS;
                 tmp_entry.crazyflieContext.localArea.ymin = y_min * FROM_UNITS_TO_METERS;
                 tmp_entry.crazyflieContext.localArea.ymax = y_max * FROM_UNITS_TO_METERS;
+
+                tmp_entry.crazyflieContext.localArea.zmin = -0.2;
+                tmp_entry.crazyflieContext.localArea.zmax = 2.0;
             }
         }
         tmp_db.crazyflieEntries.push_back(tmp_entry);

pps_ws/src/d_fall_pps/param/ClientConfig.yaml

 safeController: "SafeControllerService/RateController"
-customController: "SafeControllerService/RateController"
+customController: "CircleControllerService/CircleController"
+strictSafety: true
+angleMargin: 0.6
 

pps_ws/src/d_fall_pps/param/SafeController.yaml

@@ -15,4 +15,4 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
 estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
 
 #setpoint in meters (x, y, z, yaw)
-defaultSetpoint: [0.0, 0.0, 0.0, 0.0]
+defaultSetpoint: [0.0, 0.0, 0.3, 0.0]

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -37,15 +37,20 @@
 #define CMD_USE_CRAZYFLY_ENABLE 3
 #define CMD_USE_CRAZYFLY_DISABLE 4
 
+#define PI 3.141592653589
+
 using namespace d_fall_pps;
 
 //studentID, gives namespace and identifier in CentralManagerService
 int studentID;
 
-//the safe controller specified in the ClientConfig.yaml, is considered trusted
+//the safe controller specified in the ClientConfig.yaml, is considered stable
 ros::ServiceClient safeController;
-//the custom controller specified in the ClientConfig.yaml, is considered untrusted
+//the custom controller specified in the ClientConfig.yaml, is considered potentially unstable
 ros::ServiceClient customController;
+//values for safteyCheck
+bool strictSafety;
+float angleMargin;
 
 ros::ServiceClient centralManager;
 ros::Publisher controlCommandPublisher;
@@ -55,37 +60,40 @@ rosbag::Bag bag;
 //describes the area of the crazyflie and other parameters
 CrazyflieContext context;
 
-//gather information about other crazyflies --------------------------------------------------------------------------------
-/*bool getOtherCrazyflies;
-bool getAllCrazyflies;
-std::vector<Setpoint> otherSetpoints;
-*/
-//------------------------------------------------------------------------------------
-
 //wheter to use safe of custom controller
 bool usingSafeController;
 //wheter crazyflie is enabled (ready to fly) or disabled (motors off)
 bool crazyflieEnabled;
 
-int safetyDelay;
-
 //checks if crazyflie is within allowed area and if custom controller returns no data
 bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
-	
 	//position check
 	if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
-		safetyDelay--;
+		ROS_INFO_STREAM("x safety failed");
 		return false;
 	}
 	if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
-		safetyDelay--;
+		ROS_INFO_STREAM("y safety failed");
 		return false;
 	}
 	if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
-		safetyDelay--;
+		ROS_INFO_STREAM("z safety failed");
 		return false;
 	}
 
+	//attitude check
+	//if strictSafety is set to true in ClientConfig.yaml the SafeController takes also over if the roll and pitch angles get to large
+	//the angleMargin is a value in the range (0,1). The closer to 1, the closer to 90 deg are the roll and pitch angles allowed to become before the safeController takes over
+	if(strictSafety){
+		if((data.roll > PI/2*angleMargin) or (data.roll < -PI/2*angleMargin)) {
+			ROS_INFO_STREAM("roll too big.");
+			return false;
+		}
+		if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
+			ROS_INFO_STREAM("pitch too big.");
+			return false;
+		}
+	}
 	
 	return true;
 }
@@ -158,6 +166,16 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
 	if(!nodeHandle.getParam("studentID", studentID)) {
 		ROS_ERROR("Failed to get studentID");
 	}
+	if(!nodeHandle.getParam("strictSafety", strictSafety)) {
+		ROS_ERROR("Failed to get strictSafety param");
+		return;
+	}
+	if(!nodeHandle.getParam("angleMargin", angleMargin)) {
+		ROS_ERROR("Failed to get angleMargin param");
+		return;
+	}
+
+
 }
 
 void loadCrazyflieContext() {