Commit 2cb81cfc authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Added Tuning Controller to the GUI and created the code, needs to be finished and tested

parent a8f81126
......@@ -270,6 +270,7 @@ add_executable(DemoControllerService src/nodes/DemoControllerService.cpp)
add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
......@@ -323,6 +324,7 @@ add_dependencies(DemoControllerService d_fall_pps_generate_messages_cpp ${cat
add_dependencies(StudentControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(MpcControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(RemoteControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TuningControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -364,6 +366,7 @@ else()
target_link_libraries(MpcControllerService ${catkin_LIBRARIES})
endif()
target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
......
......@@ -52,6 +52,7 @@
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
#define REMOTE_CONTROLLER 5
#define TUNING_CONTROLLER 6
// Commands for CrazyRadio
......@@ -72,6 +73,7 @@
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
#define CMD_USE_REMOTE_CONTROLLER 5
#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_CRAZYFLY_TAKE_OFF 11
#define CMD_CRAZYFLY_LAND 12
......@@ -93,12 +95,14 @@
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
#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
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
// Universal constants
#define PI 3.141592653589
......@@ -142,6 +146,7 @@ private slots:
void on_load_student_yaml_button_clicked();
void on_load_mpc_yaml_button_clicked();
void on_load_remote_yaml_button_clicked();
void on_load_tuning_yaml_button_clicked();
// # Enable controllers
void on_enable_safe_controller_clicked();
......@@ -149,6 +154,7 @@ private slots:
void on_enable_student_controller_clicked();
void on_enable_mpc_controller_clicked();
void on_enable_remote_controller_clicked();
void on_enable_tuning_controller_clicked();
......@@ -162,6 +168,15 @@ private slots:
void on_remote_activate_button_clicked();
void on_remote_deactivate_button_clicked();
// Buttons within the TUNING controller tab
void on_tuning_test_horizontal_button_clicked();
void on_tuning_test_vertical_button_clicked();
void on_tuning_test_heading_button_clicked();
void on_tuning_test_all_button_clicked();
void on_tuning_slider_horizontal_valueChanged(int value);
void on_tuning_slider_vertical_valueChanged(int value);
void on_tuning_slider_heading_valueChanged(int value);
private:
......@@ -179,6 +194,8 @@ private:
ros::Timer m_timer_yaml_file_for_student_controller;
ros::Timer m_timer_yaml_file_for_mpc_controller;
ros::Timer m_timer_yaml_file_for_remote_controller;
ros::Timer m_timer_yaml_file_for_tuning_controller;
int m_student_id;
CrazyflieContext m_context;
......@@ -210,6 +227,7 @@ private:
// > For the MPC Controller SETPOINTS
ros::Publisher mpcSetpointPublisher;
ros::Subscriber mpcSetpointSubscriber;
// > For the Remote Controller subscribe action
ros::Publisher remoteSubscribePublisher;
// > For the Remote Controller activate action
......@@ -219,6 +237,13 @@ private:
// > For the Remote Control setpoint
ros::Subscriber remoteControlSetpointSubscriber;
// > For the TUNING CONTROLLER "test" button publisher
ros::Publisher tuningActivateTestPublisher;
// > For the TUNING CONTOLLER "gain" sliders
ros::Publisher tuningHorizontalGainPublisher;
ros::Publisher tuningVerticalGainPublisher;
ros::Publisher tuningHeadingGainPublisher;
ros::Publisher PPSClientStudentCustomButtonPublisher;
......@@ -265,6 +290,7 @@ private:
void studentYamlFileTimerCallback(const ros::TimerEvent&);
void mpcYamlFileTimerCallback(const ros::TimerEvent&);
void remoteYamlFileTimerCallback(const ros::TimerEvent&);
void tuningYamlFileTimerCallback(const ros::TimerEvent&);
......@@ -290,6 +316,7 @@ private:
void highlightStudentControllerTab();
void highlightMpcControllerTab();
void highlightRemoteControllerTab();
void highlightTuningControllerTab();
bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
......
......@@ -89,6 +89,15 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
remoteControlSetpointSubscriber = nodeHandle.subscribe("RemoteControllerService/RemoteControlSetpoint", 1, &MainWindow::remoteControlSetpointCallback, this);;
// > For the TUNING CONTROLLER "test" button publisher
tuningActivateTestPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/ActivateTest", 1);
// > For the TUNING CONTOLLER "gain" sliders
tuningHorizontalGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/HorizontalGain", 1);
tuningVerticalGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/VerticalGain", 1);
tuningHeadingGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/HeadingGain", 1);
// subscribers
crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
......@@ -208,6 +217,7 @@ void MainWindow::highlightSafeControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightDemoControllerTab()
{
......@@ -216,6 +226,7 @@ void MainWindow::highlightDemoControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightStudentControllerTab()
{
......@@ -224,6 +235,7 @@ void MainWindow::highlightStudentControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightMpcControllerTab()
{
......@@ -232,6 +244,7 @@ void MainWindow::highlightMpcControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightRemoteControllerTab()
{
......@@ -240,6 +253,16 @@ void MainWindow::highlightRemoteControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
void MainWindow::highlightTuningControllerTab()
{
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::green);
}
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
......@@ -266,6 +289,8 @@ void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
break;
case REMOTE_CONTROLLER:
highlightRemoteControllerTab();
case TUNING_CONTROLLER:
highlightTuningControllerTab();
break;
default:
break;
......@@ -868,6 +893,36 @@ void MainWindow::remoteYamlFileTimerCallback(const ros::TimerEvent&)
void MainWindow::on_load_tuning_yaml_button_clicked()
{
// Set the "load tuning yaml" button to be disabled
ui->load_tuning_yaml_button->setEnabled(false);
// Send a message requesting the parameters from the YAML
// file to be reloaded for the tuning controller
std_msgs::Int32 msg;
msg.data = LOAD_YAML_TUNING_CONTROLLER_AGENT;
this->requestLoadControllerYamlPublisher.publish(msg);
ROS_INFO("[STUDENT GUI] Request load of tuning 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_tuning_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::tuningYamlFileTimerCallback, this, true);
}
void MainWindow::tuningYamlFileTimerCallback(const ros::TimerEvent&)
{
// Enble the "load tuning yaml" button again
ui->load_tuning_yaml_button->setEnabled(true);
}
void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
{
// Extract from the "msg" for which controller the YAML
......@@ -955,6 +1010,21 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
break;
case LOAD_YAML_TUNING_CONTROLLER_AGENT:
case LOAD_YAML_TUNING_CONTROLLER_COORDINATOR:
// Set the "load tuning yaml" button to be disabled
ui->load_tuning_yaml_button->setEnabled(false);
// 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.
m_timer_yaml_file_for_tuning_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::tuningYamlFileTimerCallback, this, true);
break;
default:
ROS_INFO("Unknown 'all controllers to load yaml' command, thus nothing will be disabled");
break;
......@@ -1001,6 +1071,13 @@ void MainWindow::on_enable_remote_controller_clicked()
this->PPSClientCommandPublisher.publish(msg);
}
void MainWindow::on_enable_tuning_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_TUNING_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
}
// # Custom command buttons
......@@ -1034,6 +1111,60 @@ void MainWindow::on_customButton_3_clicked()
Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
{
Setpoint corrected_setpoint;
corrected_setpoint = setpoint;
float x_size = context.localArea.xmax - context.localArea.xmin;
float y_size = context.localArea.ymax - context.localArea.ymin;
float z_size = context.localArea.zmax - context.localArea.zmin;
if(setpoint.x > x_size/2)
corrected_setpoint.x = x_size/2;
if(setpoint.y > y_size/2)
corrected_setpoint.y = y_size/2;
if(setpoint.z > z_size)
corrected_setpoint.z = z_size;
if(setpoint.x < -x_size/2)
corrected_setpoint.x = -x_size/2;
if(setpoint.y < -y_size/2)
corrected_setpoint.y = -y_size/2;
if(setpoint.z < 0)
corrected_setpoint.z = 0;
return corrected_setpoint;
}
bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
{
float x_size = context.localArea.xmax - context.localArea.xmin;
float y_size = context.localArea.ymax - context.localArea.ymin;
float z_size = context.localArea.zmax - context.localArea.zmin;
//position check
if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
ROS_INFO_STREAM("x outside safety box");
return false;
}
if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
ROS_INFO_STREAM("y outside safety box");
return false;
}
if((setpoint.z < 0) or (setpoint.z > z_size)) {
ROS_INFO_STREAM("z outside safety box");
return false;
}
return true;
}
// # Custom buttons for the REMOTE controller service
......@@ -1127,52 +1258,75 @@ void MainWindow::remoteControlSetpointCallback(const CrazyflieData& setpointData
// TUNING CONTROLLER TAB
void MainWindow::on_tuning_test_horizontal_button_clicked()
{
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
msg.data = 1;
// Publish the message
this->tuningActivateTestPublisher.publish(msg);
}
Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
void MainWindow::on_tuning_test_vertical_button_clicked()
{
Setpoint corrected_setpoint;
corrected_setpoint = setpoint;
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
msg.data = 2;
// Publish the message
this->tuningActivateTestPublisher.publish(msg);
}
float x_size = context.localArea.xmax - context.localArea.xmin;
float y_size = context.localArea.ymax - context.localArea.ymin;
float z_size = context.localArea.zmax - context.localArea.zmin;
void MainWindow::on_tuning_test_heading_button_clicked()
{
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
msg.data = 3;
// Publish the message
this->tuningActivateTestPublisher.publish(msg);
}
if(setpoint.x > x_size/2)
corrected_setpoint.x = x_size/2;
if(setpoint.y > y_size/2)
corrected_setpoint.y = y_size/2;
if(setpoint.z > z_size)
corrected_setpoint.z = z_size;
void MainWindow::on_tuning_test_all_button_clicked()
{
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
msg.data = 4;
// Publish the message
this->tuningActivateTestPublisher.publish(msg);
}
if(setpoint.x < -x_size/2)
corrected_setpoint.x = -x_size/2;
if(setpoint.y < -y_size/2)
corrected_setpoint.y = -y_size/2;
if(setpoint.z < 0)
corrected_setpoint.z = 0;
void MainWindow::on_tuning_slider_horizontal_valueChanged(int value)
{
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
msg.data = value;
// Publish the message
this->tuningHorizontalGainPublisher.publish(msg);
}
return corrected_setpoint;
void MainWindow::on_tuning_slider_vertical_valueChanged(int value)
{
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
msg.data = value;
// Publish the message
this->tuningVerticalGainPublisher.publish(msg);
}
bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
void MainWindow::on_tuning_slider_heading_valueChanged(int value)
{
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
msg.data = value;
// Publish the message
this->tuningHeadingGainPublisher.publish(msg);
}
float x_size = context.localArea.xmax - context.localArea.xmin;
float y_size = context.localArea.ymax - context.localArea.ymin;
float z_size = context.localArea.zmax - context.localArea.zmin;
//position check
if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
ROS_INFO_STREAM("x outside safety box");
return false;
}
if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
ROS_INFO_STREAM("y outside safety box");
return false;
}
if((setpoint.z < 0) or (setpoint.z > z_size)) {
ROS_INFO_STREAM("z outside safety box");
return false;
}
return true;
}
......@@ -88,6 +88,7 @@
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
#define REMOTE_CONTROLLER 5
#define TUNING_CONTROLLER 6
// The constants that "command" changes in the
// operation state of this agent
......@@ -96,6 +97,8 @@
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
#define CMD_USE_REMOTE_CONTROLLER 5
#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_CRAZYFLY_TAKE_OFF 11
#define CMD_CRAZYFLY_LAND 12
......@@ -153,6 +156,8 @@ ros::ServiceClient studentController;
ros::ServiceClient mpcController;
// The Remote controller specified in the ClientConfig.yaml
ros::ServiceClient remoteController;
// The Tuning controller specified in the ClientConfig.yaml
ros::ServiceClient tuningController;
//values for safteyCheck
......@@ -297,6 +302,7 @@ void loadDemoController();
void loadStudentController();
void loadMpcController();
void loadRemoteController();
void loadTuningController();
void sendMessageUsingController(int controller);
void setInstantController(int controller);
......
......@@ -40,12 +40,14 @@
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
#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
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
// For sending commands to the controller node informing
......@@ -60,9 +62,11 @@
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
#define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT 5
#define FETCH_YAML_TUNING_CONTROLLER_FROM_OWN_AGENT 6
#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
#define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
\ No newline at end of file
#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
#define FETCH_YAML_TUNING_CONTROLLER_FROM_COORDINATOR 16
\ No newline at end of file
// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
//
// 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/CustomButton.h"
#include "d_fall_pps/ViconSubscribeObjectName.h"
#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.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