Commit 49eedf0e authored by bucyril's avatar bucyril
Browse files

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

parent 61b1f217
...@@ -39,6 +39,8 @@ The ``PPSClient`` subscribes to a local topic with which a certain controller ca ...@@ -39,6 +39,8 @@ The ``PPSClient`` subscribes to a local topic with which a certain controller ca
In file ``ClientConfig.yaml``: In file ``ClientConfig.yaml``:
- ``safeController``: the relative ros path to the safe controller - ``safeController``: the relative ros path to the safe controller
- ``customController``: the relative ros path to the custom 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 ### CrazyRadio
......
...@@ -739,6 +739,9 @@ void MainGUIWindow::on_save_in_DB_button_clicked() ...@@ -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.xmax = x_max * FROM_UNITS_TO_METERS;
tmp_entry.crazyflieContext.localArea.ymin = y_min * 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.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); tmp_db.crazyflieEntries.push_back(tmp_entry);
......
safeController: "SafeControllerService/RateController" safeController: "SafeControllerService/RateController"
customController: "SafeControllerService/RateController" customController: "CircleControllerService/CircleController"
strictSafety: true
angleMargin: 0.6
...@@ -15,4 +15,4 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback ...@@ -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 estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw) #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]
...@@ -37,15 +37,20 @@ ...@@ -37,15 +37,20 @@
#define CMD_USE_CRAZYFLY_ENABLE 3 #define CMD_USE_CRAZYFLY_ENABLE 3
#define CMD_USE_CRAZYFLY_DISABLE 4 #define CMD_USE_CRAZYFLY_DISABLE 4
#define PI 3.141592653589
using namespace d_fall_pps; using namespace d_fall_pps;
//studentID, gives namespace and identifier in CentralManagerService //studentID, gives namespace and identifier in CentralManagerService
int studentID; 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; 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; ros::ServiceClient customController;
//values for safteyCheck
bool strictSafety;
float angleMargin;
ros::ServiceClient centralManager; ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher; ros::Publisher controlCommandPublisher;
...@@ -55,37 +60,40 @@ rosbag::Bag bag; ...@@ -55,37 +60,40 @@ rosbag::Bag bag;
//describes the area of the crazyflie and other parameters //describes the area of the crazyflie and other parameters
CrazyflieContext context; CrazyflieContext context;
//gather information about other crazyflies --------------------------------------------------------------------------------
/*bool getOtherCrazyflies;
bool getAllCrazyflies;
std::vector<Setpoint> otherSetpoints;
*/
//------------------------------------------------------------------------------------
//wheter to use safe of custom controller //wheter to use safe of custom controller
bool usingSafeController; bool usingSafeController;
//wheter crazyflie is enabled (ready to fly) or disabled (motors off) //wheter crazyflie is enabled (ready to fly) or disabled (motors off)
bool crazyflieEnabled; bool crazyflieEnabled;
int safetyDelay;
//checks if crazyflie is within allowed area and if custom controller returns no data //checks if crazyflie is within allowed area and if custom controller returns no data
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) { bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
//position check //position check
if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) { if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
safetyDelay--; ROS_INFO_STREAM("x safety failed");
return false; return false;
} }
if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) { if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
safetyDelay--; ROS_INFO_STREAM("y safety failed");
return false; return false;
} }
if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) { if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
safetyDelay--; ROS_INFO_STREAM("z safety failed");
return false; 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; return true;
} }
...@@ -158,6 +166,16 @@ void loadParameters(ros::NodeHandle& nodeHandle) { ...@@ -158,6 +166,16 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
if(!nodeHandle.getParam("studentID", studentID)) { if(!nodeHandle.getParam("studentID", studentID)) {
ROS_ERROR("Failed to get 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() { void loadCrazyflieContext() {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment