From 49eedf0e9ed5bb8f36e0e876b8af57dd16ec1dcd Mon Sep 17 00:00:00 2001
From: Cyrill Burgener <bucyril@student.ethz.ch>
Date: Tue, 6 Jun 2017 16:40:59 +0200
Subject: [PATCH] added a z range to the AreaBounds in the GUI, added angle
safety feature to PPSClient
---
pps_wiki/ros_structure.md | 2 +
.../GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp | 3 ++
pps_ws/src/d_fall_pps/param/ClientConfig.yaml | 4 +-
.../src/d_fall_pps/param/SafeController.yaml | 2 +-
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 48 +++++++++++++------
5 files changed, 42 insertions(+), 17 deletions(-)
diff --git a/pps_wiki/ros_structure.md b/pps_wiki/ros_structure.md
index a439eadd..76cdec8a 100644
--- a/pps_wiki/ros_structure.md
+++ b/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
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
index b4c039c0..49e4b00e 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
+++ b/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);
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 8c81ff60..27a48ab3 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,3 +1,5 @@
safeController: "SafeControllerService/RateController"
-customController: "SafeControllerService/RateController"
+customController: "CircleControllerService/CircleController"
+strictSafety: true
+angleMargin: 0.6
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index a2f9b963..1997d4da 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/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]
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 53977984..225e4d5d 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/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() {
--
GitLab