Commit 58a3fd69 authored by roangel's avatar roangel
Browse files

Added some changes to try downstairs: Scale factor 1.2, take into account...

Added some changes to try downstairs: Scale factor 1.2, take into account occluded flag in controller, Z origin is now 0, table, not middle of box
parent 4381a8fa
...@@ -14,8 +14,8 @@ ...@@ -14,8 +14,8 @@
using namespace d_fall_pps; using namespace d_fall_pps;
#endif #endif
#define DRONE_HEIGHT 100 * FROM_MILIMETERS_TO_UNITS #define DRONE_HEIGHT 100 * FROM_MILIMETERS_TO_UNITS * 1.2
#define DRONE_WIDTH 100 * FROM_MILIMETERS_TO_UNITS #define DRONE_WIDTH 100 * FROM_MILIMETERS_TO_UNITS * 1.2
class crazyFly : public QGraphicsSvgItem class crazyFly : public QGraphicsSvgItem
{ {
......
...@@ -94,7 +94,7 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) { ...@@ -94,7 +94,7 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
return false; return false;
} }
} }
return true; return true;
} }
...@@ -102,7 +102,9 @@ void coordinatesToLocal(CrazyflieData& cf) { ...@@ -102,7 +102,9 @@ void coordinatesToLocal(CrazyflieData& cf) {
AreaBounds area = context.localArea; AreaBounds area = context.localArea;
float originX = (area.xmin + area.xmax) / 2.0; float originX = (area.xmin + area.xmax) / 2.0;
float originY = (area.ymin + area.ymax) / 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.x -= originX;
cf.y -= originY; cf.y -= originY;
...@@ -113,52 +115,55 @@ void coordinatesToLocal(CrazyflieData& cf) { ...@@ -113,52 +115,55 @@ void coordinatesToLocal(CrazyflieData& cf) {
void viconCallback(const ViconData& viconData) { void viconCallback(const ViconData& viconData) {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) { for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData global = *it; CrazyflieData global = *it;
if(global.crazyflieName == context.crazyflieName) { if(!global.occluded) //if it is not occluded, then proceed to compute the controller output
Controller controllerCall; {
if(global.crazyflieName == context.crazyflieName) {
CrazyflieData local = global; Controller controllerCall;
coordinatesToLocal(local);
controllerCall.request.ownCrazyflie = local; CrazyflieData local = global;
coordinatesToLocal(local);
if(crazyflieEnabled){ controllerCall.request.ownCrazyflie = local;
if(!usingSafeController) {
bool success = customController.call(controllerCall); if(crazyflieEnabled){
if(!usingSafeController) {
if(!success) { bool success = customController.call(controllerCall);
ROS_ERROR("Failed to call custom controller, switching to safe controller");
ROS_ERROR_STREAM("custom controller status: valid: " << customController.isValid() << ", exists: " << customController.exists()); if(!success) {
ROS_ERROR_STREAM("custom controller name: " << customController.getService()); ROS_ERROR("Failed to call custom controller, switching to safe controller");
usingSafeController = true; ROS_ERROR_STREAM("custom controller status: valid: " << customController.isValid() << ", exists: " << customController.exists());
} else if(!safetyCheck(global, controllerCall.response.controlOutput)) { ROS_ERROR_STREAM("custom controller name: " << customController.getService());
usingSafeController = true; usingSafeController = true;
ROS_INFO_STREAM("safety check failed, switching to safe controller"); } 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) { if(usingSafeController) {
ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists()); 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); //ROS_INFO_STREAM("safe controller active: " << usingSafeController);
bag.write("ViconData", ros::Time::now(), local); controlCommandPublisher.publish(controllerCall.response.controlOutput);
bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
bag.write("ViconData", ros::Time::now(), local);
} else { //crazyflie disabled bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode } else { //crazyflie disabled
controlCommandPublisher.publish(zeroOutput); ControlCommand zeroOutput = ControlCommand(); //everything set to zero
bag.write("ViconData", ros::Time::now(), local); zeroOutput.onboardControllerType = 2; //set to motor_mode
bag.write("ControlOutput", ros::Time::now(), zeroOutput); controlCommandPublisher.publish(zeroOutput);
} bag.write("ViconData", ros::Time::now(), local);
} bag.write("ControlOutput", ros::Time::now(), zeroOutput);
}
}
}
} }
} }
...@@ -182,7 +187,7 @@ void loadCrazyflieContext() { ...@@ -182,7 +187,7 @@ void loadCrazyflieContext() {
CMQuery contextCall; CMQuery contextCall;
contextCall.request.studentID = studentID; contextCall.request.studentID = studentID;
ROS_INFO_STREAM("StudentID:" << studentID); ROS_INFO_STREAM("StudentID:" << studentID);
centralManager.waitForExistence(ros::Duration(-1)); centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall)) { if(centralManager.call(contextCall)) {
...@@ -254,16 +259,16 @@ int main(int argc, char* argv[]){ ...@@ -254,16 +259,16 @@ int main(int argc, char* argv[]){
ros::init(argc, argv, "PPSClient"); ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle); loadParameters(nodeHandle);
//ros::service::waitForService("/CentralManagerService/CentralManager"); //ros::service::waitForService("/CentralManagerService/CentralManager");
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false); centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext(); loadCrazyflieContext();
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback); ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
ROS_INFO_STREAM("successfully subscribed to ViconData"); ROS_INFO_STREAM("successfully subscribed to ViconData");
//ros::Publishers to advertise the control output //ros::Publishers to advertise the control output
controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1); controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
...@@ -275,7 +280,7 @@ int main(int argc, char* argv[]){ ...@@ -275,7 +280,7 @@ int main(int argc, char* argv[]){
crazyflieEnabled = true; crazyflieEnabled = true;
usingSafeController = true; usingSafeController = true;
loadSafeController(); loadSafeController();
std::string package_path; std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/"; package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO_STREAM(package_path); ROS_INFO_STREAM(package_path);
......
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