diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFly.h b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFly.h
index 8f97fca57db13ffb0296180c08cdb39afdaef085..aad2f1de078c508d65a617d10676b36506cdb01d 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFly.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFly.h
@@ -14,8 +14,8 @@
using namespace d_fall_pps;
#endif
-#define DRONE_HEIGHT 100 * FROM_MILIMETERS_TO_UNITS
-#define DRONE_WIDTH 100 * FROM_MILIMETERS_TO_UNITS
+#define DRONE_HEIGHT 100 * FROM_MILIMETERS_TO_UNITS * 1.2
+#define DRONE_WIDTH 100 * FROM_MILIMETERS_TO_UNITS * 1.2
class crazyFly : public QGraphicsSvgItem
{
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 225e4d5d1ec00322dfa5c0a81a6af9614f90b5bd..28f16700981e0e53f022e8d0f82b835fda093e9d 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -94,7 +94,7 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
return false;
}
}
-
+
return true;
}
@@ -102,7 +102,9 @@ void coordinatesToLocal(CrazyflieData& cf) {
AreaBounds area = context.localArea;
float originX = (area.xmin + area.xmax) / 2.0;
float originY = (area.ymin + area.ymax) / 2.0;
- float originZ = (area.zmin + area.zmax) / 2.0;
+ // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
+ float originZ = 0.0;
+ // float originZ = (area.zmin + area.zmax) / 2.0;
cf.x -= originX;
cf.y -= originY;
@@ -113,52 +115,55 @@ void coordinatesToLocal(CrazyflieData& cf) {
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.crazyflieName == context.crazyflieName) {
- Controller controllerCall;
-
- CrazyflieData local = global;
- coordinatesToLocal(local);
- controllerCall.request.ownCrazyflie = local;
-
- if(crazyflieEnabled){
- if(!usingSafeController) {
- bool success = customController.call(controllerCall);
-
- 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)) {
- usingSafeController = true;
- ROS_INFO_STREAM("safety check failed, switching to safe controller");
- }
- }
-
-
- if(usingSafeController) {
- bool success = safeController.call(controllerCall);
- if(!success) {
- ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
- }
- }
-
- //ROS_INFO_STREAM("safe controller active: " << usingSafeController);
-
- controlCommandPublisher.publish(controllerCall.response.controlOutput);
-
- 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);
- }
- }
+
+ if(!global.occluded) //if it is not occluded, then proceed to compute the controller output
+ {
+ if(global.crazyflieName == context.crazyflieName) {
+ Controller controllerCall;
+
+ CrazyflieData local = global;
+ coordinatesToLocal(local);
+ controllerCall.request.ownCrazyflie = local;
+
+ if(crazyflieEnabled){
+ if(!usingSafeController) {
+ bool success = customController.call(controllerCall);
+
+ 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)) {
+ usingSafeController = true;
+ ROS_INFO_STREAM("safety check failed, switching to safe controller");
+ }
+ }
+
+
+ if(usingSafeController) {
+ bool success = safeController.call(controllerCall);
+ if(!success) {
+ ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
+ }
+ }
+
+ //ROS_INFO_STREAM("safe controller active: " << usingSafeController);
+
+ controlCommandPublisher.publish(controllerCall.response.controlOutput);
+
+ 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);
+ }
+ }
+ }
}
}
@@ -182,7 +187,7 @@ void loadCrazyflieContext() {
CMQuery contextCall;
contextCall.request.studentID = studentID;
ROS_INFO_STREAM("StudentID:" << studentID);
-
+
centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall)) {
@@ -254,16 +259,16 @@ int main(int argc, char* argv[]){
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
-
-
+
+
//ros::service::waitForService("/CentralManagerService/CentralManager");
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
-
+
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
ROS_INFO_STREAM("successfully subscribed to ViconData");
-
+
//ros::Publishers to advertise the control output
controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
@@ -275,7 +280,7 @@ int main(int argc, char* argv[]){
crazyflieEnabled = true;
usingSafeController = true;
loadSafeController();
-
+
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO_STREAM(package_path);