Commit 4534dc5b authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Updated student GUI code for the new enable and load yaml buttons

parent 326018be
...@@ -46,8 +46,11 @@ ...@@ -46,8 +46,11 @@
// Types of controllers being used: // Types of controllers being used:
#define SAFE_CONTROLLER 0 #define SAFE_CONTROLLER 0
#define CUSTOM_CONTROLLER 1 #define DEMO_CONTROLLER 1
#define STUDENT_CONTROLLER 2
#define MPC_CONTROLLER 3
// Commands for CrazyRadio // Commands for CrazyRadio
#define CMD_RECONNECT 0 #define CMD_RECONNECT 0
...@@ -62,11 +65,14 @@ ...@@ -62,11 +65,14 @@
// operation state of this agent. These "commands" // operation state of this agent. These "commands"
// are sent from this GUI node to the "PPSClient" // are sent from this GUI node to the "PPSClient"
// node where the command is enacted // node where the command is enacted
#define CMD_USE_SAFE_CONTROLLER 1 #define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2 #define CMD_USE_DEMO_CONTROLLER 2
#define CMD_CRAZYFLY_TAKE_OFF 3 #define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_CRAZYFLY_LAND 4 #define CMD_USE_MPC_CONTROLLER 4
#define CMD_CRAZYFLY_MOTORS_OFF 5
#define CMD_CRAZYFLY_TAKE_OFF 11
#define CMD_CRAZYFLY_LAND 12
#define CMD_CRAZYFLY_MOTORS_OFF 13
// Flying States // Flying States
#define STATE_MOTORS_OFF 1 #define STATE_MOTORS_OFF 1
...@@ -80,9 +86,14 @@ ...@@ -80,9 +86,14 @@
// For which controller parameters to load // For which controller parameters to load
#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1 #define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
#define LOAD_YAML_CUSTOM_CONTROLLER_AGENT 2 #define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 3 #define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR 4 #define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
// Universal constants // Universal constants
#define PI 3.141592653589 #define PI 3.141592653589
...@@ -104,29 +115,39 @@ public: ...@@ -104,29 +115,39 @@ public:
private slots: private slots:
void updateNewViconData(const ptrToMessage& p_msg); void updateNewViconData(const ptrToMessage& p_msg);
// # RF Crazyradio Connect Disconnect
void on_RF_Connect_button_clicked(); void on_RF_Connect_button_clicked();
void on_RF_disconnect_button_clicked();
// # Take off, lanf, motors off
void on_take_off_button_clicked(); void on_take_off_button_clicked();
void on_land_button_clicked(); void on_land_button_clicked();
void on_motors_OFF_button_clicked(); void on_motors_OFF_button_clicked();
// # Setpoint
void on_set_setpoint_button_clicked(); void on_set_setpoint_button_clicked();
void on_set_setpoint_button_2_clicked(); void on_set_setpoint_button_2_clicked();
void on_RF_disconnect_button_clicked(); // # Load Yaml when acting as the GUI for an Agent
void on_load_custom_yaml_button_clicked();
void on_load_safe_yaml_button_clicked(); void on_load_safe_yaml_button_clicked();
void on_load_demo_yaml_button_clicked();
void on_load_student_yaml_button_clicked();
void on_load_mpc_yaml_button_clicked();
void on_en_custom_controller_clicked(); // # Enable controllers
void on_enable_safe_controller_clicked();
void on_enable_demo_controller_clicked();
void on_enable_student_controller_clicked();
void on_enable_mpc_controller_clicked();
void on_en_safe_controller_clicked();
void on_customButton_1_clicked(); void on_customButton_1_clicked();
void on_customButton_2_clicked(); void on_customButton_2_clicked();
void on_customButton_3_clicked(); void on_customButton_3_clicked();
private: private:
Ui::MainWindow *ui; Ui::MainWindow *ui;
...@@ -138,7 +159,9 @@ private: ...@@ -138,7 +159,9 @@ private:
std::string m_ros_namespace; std::string m_ros_namespace;
ros::Timer m_timer_yaml_file_for_safe_controller; ros::Timer m_timer_yaml_file_for_safe_controller;
ros::Timer m_timer_yaml_file_for_custom_controlller; ros::Timer m_timer_yaml_file_for_demo_controller;
ros::Timer m_timer_yaml_file_for_student_controller;
ros::Timer m_timer_yaml_file_for_mpc_controller;
int m_student_id; int m_student_id;
CrazyflieContext m_context; CrazyflieContext m_context;
...@@ -189,8 +212,15 @@ private: ...@@ -189,8 +212,15 @@ private:
void safeSetpointCallback(const Setpoint& newSetpoint); void safeSetpointCallback(const Setpoint& newSetpoint);
void customSetpointCallback(const Setpoint& newSetpoint); void customSetpointCallback(const Setpoint& newSetpoint);
void DBChangedCallback(const std_msgs::Int32& msg); void DBChangedCallback(const std_msgs::Int32& msg);
void customYamlFileTimerCallback(const ros::TimerEvent&);
// # Load Yaml when acting as the GUI for an Agent
void safeYamlFileTimerCallback(const ros::TimerEvent&); void safeYamlFileTimerCallback(const ros::TimerEvent&);
void demoYamlFileTimerCallback(const ros::TimerEvent&);
void studentYamlFileTimerCallback(const ros::TimerEvent&);
void mpcYamlFileTimerCallback(const ros::TimerEvent&);
void requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg); void requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg);
void controllerUsedChangedCallback(const std_msgs::Int32& msg); void controllerUsedChangedCallback(const std_msgs::Int32& msg);
void batteryStateChangedCallback(const std_msgs::Int32& msg); void batteryStateChangedCallback(const std_msgs::Int32& msg);
......
...@@ -195,7 +195,7 @@ void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg) ...@@ -195,7 +195,7 @@ void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
case SAFE_CONTROLLER: case SAFE_CONTROLLER:
highlightSafeControllerTab(); highlightSafeControllerTab();
break; break;
case CUSTOM_CONTROLLER: case DEMO_CONTROLLER:
highlightCustomControllerTab(); highlightCustomControllerTab();
break; break;
default: default:
...@@ -432,6 +432,10 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne ...@@ -432,6 +432,10 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne
} }
} }
// ----------------------------------------------------------------------------------
// # RF Crazyradio Connect Disconnect
void MainWindow::on_RF_Connect_button_clicked() void MainWindow::on_RF_Connect_button_clicked()
{ {
std_msgs::Int32 msg; std_msgs::Int32 msg;
...@@ -440,6 +444,18 @@ void MainWindow::on_RF_Connect_button_clicked() ...@@ -440,6 +444,18 @@ void MainWindow::on_RF_Connect_button_clicked()
ROS_INFO("command reconnect published"); ROS_INFO("command reconnect published");
} }
void MainWindow::on_RF_disconnect_button_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_DISCONNECT;
this->crazyRadioCommandPublisher.publish(msg);
ROS_INFO("command disconnect published");
}
// ----------------------------------------------------------------------------------
// # Take off, lanf, motors off
void MainWindow::on_take_off_button_clicked() void MainWindow::on_take_off_button_clicked()
{ {
std_msgs::Int32 msg; std_msgs::Int32 msg;
...@@ -461,6 +477,10 @@ void MainWindow::on_motors_OFF_button_clicked() ...@@ -461,6 +477,10 @@ void MainWindow::on_motors_OFF_button_clicked()
this->PPSClientCommandPublisher.publish(msg); this->PPSClientCommandPublisher.publish(msg);
} }
// ----------------------------------------------------------------------------------
// # Setpoint
void MainWindow::on_set_setpoint_button_clicked() void MainWindow::on_set_setpoint_button_clicked()
{ {
Setpoint msg_setpoint; Setpoint msg_setpoint;
...@@ -537,17 +557,11 @@ void MainWindow::on_set_setpoint_button_2_clicked() ...@@ -537,17 +557,11 @@ void MainWindow::on_set_setpoint_button_2_clicked()
ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw); ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
} }
void MainWindow::on_RF_disconnect_button_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_DISCONNECT;
this->crazyRadioCommandPublisher.publish(msg);
ROS_INFO("command disconnect published");
}
// ----------------------------------------------------------------------------------
// # Load Yaml when acting as the GUI for an Agent
void MainWindow::on_load_safe_yaml_button_clicked() void MainWindow::on_load_safe_yaml_button_clicked()
{ {
// Set the "load safe yaml" button to be disabled // Set the "load safe yaml" button to be disabled
...@@ -578,19 +592,47 @@ void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&) ...@@ -578,19 +592,47 @@ void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
void MainWindow::on_load_demo_yaml_button_clicked()
{
// Set the "load demo yaml" button to be disabled
ui->load_demo_yaml_button->setEnabled(false);
// Send a message requesting the parameters from the YAML
// file to be reloaded for the demo controller
std_msgs::Int32 msg;
msg.data = LOAD_YAML_DEMO_CONTROLLER_AGENT;
this->requestLoadControllerYamlPublisher.publish(msg);
ROS_INFO("Request load of demo controller YAML published");
// Start a timer which will enable the button in its callback
// > This is required because the agent node waits some time between
// re-loading the values from the YAML file and then assigning then
// to the local variable of the agent.
// > Thus we use this timer to prevent the user from clicking the
// button in the GUI repeatedly.
ros::NodeHandle nodeHandle("~");
m_timer_yaml_file_for_demo_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::demoYamlFileTimerCallback, this, true);
}
void MainWindow::demoYamlFileTimerCallback(const ros::TimerEvent&)
{
// Enble the "load demo yaml" button again
ui->load_demo_yaml_button->setEnabled(true);
}
void MainWindow::on_load_custom_yaml_button_clicked() void MainWindow::on_load_student_yaml_button_clicked()
{ {
// Set the "load custom yaml" button to be disabled // Set the "load student yaml" button to be disabled
ui->load_custom_yaml_button->setEnabled(false); ui->load_student_yaml_button->setEnabled(false);
// Send a message requesting the parameters from the YAML // Send a message requesting the parameters from the YAML
// file to be reloaded for the custom controller // file to be reloaded for the student controller
std_msgs::Int32 msg; std_msgs::Int32 msg;
msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT; msg.data = LOAD_YAML_STUDENT_CONTROLLER_AGENT;
this->requestLoadControllerYamlPublisher.publish(msg); this->requestLoadControllerYamlPublisher.publish(msg);
ROS_INFO("Request load of custom controller YAML published"); ROS_INFO("Request load of student controller YAML published");
// Start a timer which will enable the button in its callback // Start a timer which will enable the button in its callback
// > This is required because the agent node waits some time between // > This is required because the agent node waits some time between
...@@ -599,17 +641,45 @@ void MainWindow::on_load_custom_yaml_button_clicked() ...@@ -599,17 +641,45 @@ void MainWindow::on_load_custom_yaml_button_clicked()
// > Thus we use this timer to prevent the user from clicking the // > Thus we use this timer to prevent the user from clicking the
// button in the GUI repeatedly. // button in the GUI repeatedly.
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
m_timer_yaml_file_for_custom_controlller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::customYamlFileTimerCallback, this, true); m_timer_yaml_file_for_student_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::studentYamlFileTimerCallback, this, true);
} }
void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&) void MainWindow::studentYamlFileTimerCallback(const ros::TimerEvent&)
{ {
// Enble the "load custom yaml" button again // Enble the "load student yaml" button again
ui->load_custom_yaml_button->setEnabled(true); ui->load_student_yaml_button->setEnabled(true);
} }
void MainWindow::on_load_mpc_yaml_button_clicked()
{
// Set the "load mpc yaml" button to be disabled
ui->load_mpc_yaml_button->setEnabled(false);
// Send a message requesting the parameters from the YAML
// file to be reloaded for the mpc controller
std_msgs::Int32 msg;
msg.data = LOAD_YAML_MPC_CONTROLLER_AGENT;
this->requestLoadControllerYamlPublisher.publish(msg);
ROS_INFO("Request load of mpc controller YAML published");
// Start a timer which will enable the button in its callback
// > This is required because the agent node waits some time between
// re-loading the values from the YAML file and then assigning then
// to the local variable of the agent.
// > Thus we use this timer to prevent the user from clicking the
// button in the GUI repeatedly.
ros::NodeHandle nodeHandle("~");
m_timer_yaml_file_for_mpc_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::mpcYamlFileTimerCallback, this, true);
}
void MainWindow::mpcYamlFileTimerCallback(const ros::TimerEvent&)
{
// Enble the "load mpc yaml" button again
ui->load_mpc_yaml_button->setEnabled(true);
}
void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg) void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
...@@ -639,10 +709,10 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs:: ...@@ -639,10 +709,10 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
break; break;
case LOAD_YAML_CUSTOM_CONTROLLER_AGENT: case LOAD_YAML_DEMO_CONTROLLER_AGENT:
case LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR: case LOAD_YAML_DEMO_CONTROLLER_COORDINATOR:
// Set the "load custom yaml" button to be disabled // Set the "load custom yaml" button to be disabled
ui->load_custom_yaml_button->setEnabled(false); ui->load_demo_yaml_button->setEnabled(false);
// Start a timer which will enable the button in its callback // Start a timer which will enable the button in its callback
// > This is required because the agent node waits some time between // > This is required because the agent node waits some time between
...@@ -650,7 +720,7 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs:: ...@@ -650,7 +720,7 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
// to the local variable of the agent. // to the local variable of the agent.
// > Thus we use this timer to prevent the user from clicking the // > Thus we use this timer to prevent the user from clicking the
// button in the GUI repeatedly. // button in the GUI repeatedly.
m_timer_yaml_file_for_custom_controlller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::customYamlFileTimerCallback, this, true); m_timer_yaml_file_for_demo_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::demoYamlFileTimerCallback, this, true);
break; break;
...@@ -663,21 +733,39 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs:: ...@@ -663,21 +733,39 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
void MainWindow::on_en_custom_controller_clicked()
// # Enable controllers
void MainWindow::on_enable_safe_controller_clicked()
{ {
std_msgs::Int32 msg; std_msgs::Int32 msg;
msg.data = CMD_USE_CUSTOM_CONTROLLER; msg.data = CMD_USE_SAFE_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg); this->PPSClientCommandPublisher.publish(msg);
} }
void MainWindow::on_enable_demo_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_DEMO_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
}
void MainWindow::on_en_safe_controller_clicked() void MainWindow::on_enable_student_controller_clicked()
{ {
std_msgs::Int32 msg; std_msgs::Int32 msg;
msg.data = CMD_USE_SAFE_CONTROLLER; msg.data = CMD_USE_STUDENT_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg); this->PPSClientCommandPublisher.publish(msg);
} }
void MainWindow::on_enable_mpc_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_MPC_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
}
// # Custom command buttons
void MainWindow::on_customButton_1_clicked() void MainWindow::on_customButton_1_clicked()
{ {
CustomButton msg_custom_button; CustomButton msg_custom_button;
......
...@@ -85,11 +85,14 @@ ...@@ -85,11 +85,14 @@
// The constants that "command" changes in the // The constants that "command" changes in the
// operation state of this agent // operation state of this agent
#define CMD_USE_SAFE_CONTROLLER 1 #define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_DEMO_CONTROLLER 2 #define CMD_USE_DEMO_CONTROLLER 2
#define CMD_CRAZYFLY_TAKE_OFF 3 #define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_CRAZYFLY_LAND 4 #define CMD_USE_MPC_CONTROLLER 4
#define CMD_CRAZYFLY_MOTORS_OFF 5
#define CMD_CRAZYFLY_TAKE_OFF 11
#define CMD_CRAZYFLY_LAND 12
#define CMD_CRAZYFLY_MOTORS_OFF 13
// Flying states // Flying states
#define STATE_MOTORS_OFF 1 #define STATE_MOTORS_OFF 1
......
// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Place for students to implement their controller
//
// ----------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------
// III N N CCCC L U U DDDD EEEEE SSSS
// I NN N C L U U D D E S
// I N N N C L U U D D EEE SSS
// I N NN C L U U D D E S
// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
// ----------------------------------------------------------------------------------
// These various headers need to be included so that this controller service can be
// connected with the D-FaLL system.
//some useful libraries
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <ros/package.h>
//the generated structs from the msg-files have to be included
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomControllerYAML.h"
#include <std_msgs/Int32.h>
// ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS
// D D E F I NN N E S
// D D EEE FFF I N N N EEE SSS
// D D E F I N NN E S
// DDDD EEEEE F III N N EEEEE SSSS
// ----------------------------------------------------------------------------------
// These constants are defined to make the code more readable and adaptable.
// Universal constants
#define PI 3.1415926535
// These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the
// firmware running on the Crazyflie 2.0.
// The following is a short description about each mode:
// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors
// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angular rates from the PID rate
// controllers implemented in the Crazyflie 2.0 firmware.
// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angles from the PID attitude
// controllers implemented in the Crazyflie 2.0 firmware.
#define MOTOR_MODE 6
#define RATE_MODE 7
#define ANGLE_MODE 8
// These constants define the controller used for computing the response in the
// "calculateControlOutput" function
// The following is a short description about each mode:
// LQR_RATE_MODE LQR controller based on the state vector:
// [position,velocity,angles]
//
// LQR_ANGLE_MODE LQR controller based on the state vector:
// [position,velocity]
//
#define LQR_RATE_MODE 1 // (DEFAULT)
#define LQR_ANGLE_MODE 2
// Constants for feching the yaml paramters
#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2