diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index a9dcd7e86fb7ac454a3d40abfce3df64f1db5777..dddbf25d765208586cdd3039d4e40606d41c21b2 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -13,6 +13,10 @@
#include "d_fall_pps/Setpoint.h"
+// tipes of controllers being used:
+#define SAFE_CONTROLLER 0
+#define CUSTOM_CONTROLLER 1
+
// commands for CrazyRadio
#define CMD_RECONNECT 0
#define CMD_DISCONNECT 1
@@ -111,6 +115,8 @@ private:
ros::Publisher customYAMLloadedPublisher;
ros::Publisher safeYAMLloadedPublisher;
+ ros::Subscriber controllerUsedSubscriber;
+
ros::ServiceClient centralManager;
// callbacks
@@ -122,6 +128,7 @@ private:
void DBChangedCallback(const std_msgs::Int32& msg);
void customYamlFileTimerCallback(const ros::TimerEvent&);
void safeYamlFileTimerCallback(const ros::TimerEvent&);
+ void controllerUsedChangedCallback(const std_msgs::Int32& msg);
float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage);
@@ -132,6 +139,8 @@ private:
void disableGUI();
void enableGUI();
+ void highlightSafeControllerTab();
+ void highlightCustomControllerTab();
const std::vector<float> m_cutoff_voltages {3.1966, 3.2711, 3.3061, 3.3229, 3.3423, 3.3592, 3.3694, 3.385, 3.4006, 3.4044, 3.4228, 3.4228, 3.4301, 3.4445, 3.4531, 3.4677, 3.4705, 3.4712, 3.4756, 3.483, 3.4944, 3.5008, 3.5008, 3.5084, 3.511, 3.5122, 3.5243, 3.5329, 3.5412, 3.5529, 3.5609, 3.5625, 3.5638, 3.5848, 3.6016, 3.6089, 3.6223, 3.628, 3.6299, 3.6436, 3.6649, 3.6878, 3.6983, 3.7171, 3.7231, 3.7464, 3.7664, 3.7938, 3.8008, 3.816, 3.8313, 3.8482, 3.866, 3.8857, 3.8984, 3.9159, 3.9302, 3.9691, 3.997, 4.14 };
};
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index 90417e6dd6eff95799949df68c2136128dc4247e..77ae9cfd02de2040d2c49946079c85478c5447b0 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -41,6 +41,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
+ controllerUsedSubscriber = nodeHandle.subscribe("PPSClient/controllerUsed", 1, &MainWindow::controllerUsedChangedCallback, this);
+
safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
@@ -105,12 +107,38 @@ void MainWindow::enableGUI()
ui->groupBox_general->setEnabled(true);
}
+void MainWindow::highlightSafeControllerTab()
+{
+ ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
+ ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
+}
+void MainWindow::highlightCustomControllerTab()
+{
+ ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
+}
+
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
loadCrazyflieContext();
ROS_INFO("context reloaded in student_GUI");
}
+void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
+{
+ switch(msg.data)
+ {
+ case SAFE_CONTROLLER:
+ highlightSafeControllerTab();
+ break;
+ case CUSTOM_CONTROLLER:
+ highlightCustomControllerTab();
+ break;
+ default:
+ break;
+ }
+}
+
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
{
m_safe_setpoint = newSetpoint;
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index f2336c54249b824af613e3e9c0c4b5fcf5fd2da1..54a5f7cbfe4c2951a9ee952c564da8784ecab111 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -1,3 +1,3 @@
5,PPS_CF02,0/8/2M,1,-0.401205,0.729533,-0.2,0.289141,1.25551,2
6,PPS_CF08,0/56/2M,2,-0.997887,-0.285942,-0.2,0.00653145,0.214912,2
-4,PPS_CF05,0/32/2M,0,-0.85884,0.248234,-0.2,-0.136071,0.812101,2
+4,PPS_CF05,0/32/2M,0,-1.02122,0.593954,-0.2,-0.298455,1.15782,2
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 890ff9214ee5e846c362cdab52b9d6880f016e56..5052a1c6241e4f82cb26b3b8fc26172a36493579 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -33,6 +33,10 @@
#include "d_fall_pps/ControlCommand.h"
+// tipes of controllers being used:
+#define SAFE_CONTROLLER 0
+#define CUSTOM_CONTROLLER 1
+
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2
#define CMD_CRAZYFLY_TAKE_OFF 3
@@ -80,7 +84,6 @@ float angleMargin;
Setpoint controller_setpoint;
-
// variables for linear trayectory
Setpoint current_safe_setpoint;
double distance;
@@ -117,7 +120,9 @@ int crazyradio_status;
CrazyflieContext context;
//wheter to use safe of custom controller
-bool usingSafeController;
+bool using_safe_controller;
+
+ros::Publisher controllerUsedPublisher;
std::string ros_namespace;
@@ -133,6 +138,28 @@ ros::Timer timer_takeoff;
ros::Timer timer_land;
+void useSafeController()
+{
+ using_safe_controller = true;
+ // send a message in topic for the studentGUI to read it
+ std_msgs::Int32 controller_used_msg;
+ controller_used_msg.data = SAFE_CONTROLLER;
+ controllerUsedPublisher.publish(controller_used_msg);
+}
+
+void useCustomController()
+{
+ using_safe_controller = false;
+ // send a message in topic for the studentGUI to read it
+ std_msgs::Int32 controller_used_msg;
+ controller_used_msg.data = CUSTOM_CONTROLLER;
+ controllerUsedPublisher.publish(controller_used_msg);
+}
+
+bool getUsingSafeController()
+{
+ return using_safe_controller;
+}
void loadSafeController() {
ros::NodeHandle nodeHandle("~");
@@ -247,7 +274,7 @@ void takeOffCF(CrazyflieData& current_local_coordinates) //local because the set
ROS_INFO("X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
// now, use safe controller to go to that setpoint
- usingSafeController = true;
+ useSafeController();
loadSafeController();
// when do we finish? after some time with delay?
@@ -266,7 +293,7 @@ void landCF(CrazyflieData& current_local_coordinates)
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
// now, use safe controller to go to that setpoint
- usingSafeController = true;
+ useSafeController();
loadSafeController();
setCurrentSafeSetpoint(setpoint_msg);
}
@@ -379,7 +406,7 @@ void viconCallback(const ViconData& viconData) {
{
if(!global.occluded) //if it is not occluded, then proceed to compute the controller output.
{
- if(!usingSafeController && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
+ if(!getUsingSafeController() && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
{
bool success = customController.call(controllerCall);
if(!success)
@@ -387,11 +414,11 @@ void viconCallback(const ViconData& viconData) {
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;
+ useSafeController();
}
else if(!safetyCheck(global, controllerCall.response.controlOutput))
{
- usingSafeController = true;
+ useSafeController();
ROS_INFO_STREAM("safety check failed, switching to safe controller");
}
}
@@ -430,7 +457,6 @@ void viconCallback(const ViconData& viconData) {
}
}
- //ROS_INFO_STREAM("safe controller active: " << usingSafeController);
controlCommandPublisher.publish(controllerCall.response.controlOutput);
@@ -543,13 +569,13 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
case CMD_USE_SAFE_CONTROLLER:
ROS_INFO("USE_SAFE_CONTROLLER Command received");
loadSafeController();
- usingSafeController = true;
+ useSafeController();
break;
case CMD_USE_CUSTOM_CONTROLLER:
ROS_INFO("USE_CUSTOM_CONTROLLER Command received");
loadCustomController();
- usingSafeController = false;
+ useSafeController();
break;
case CMD_CRAZYFLY_TAKE_OFF:
@@ -660,6 +686,8 @@ int main(int argc, char* argv[])
// this topic will publish flying state whenever it changes.
flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
+ controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
+
// crazy radio status
crazyradio_status = DISCONNECTED;
@@ -687,7 +715,7 @@ int main(int argc, char* argv[])
//start with safe controller
flying_state = STATE_MOTORS_OFF;
- usingSafeController = true;
+ useSafeController();
loadSafeController();
std::string package_path;