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
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
......
......@@ -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);
......
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
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]
......@@ -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() {
......
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