Commit dfe49202 authored by roangel's avatar roangel
Browse files

highlight tab when selecting controller

parent 5e4abb53
......@@ -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 };
};
......
......@@ -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;
......
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
......@@ -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;
......
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