To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

...
 
Commits (9)
/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
......@@ -118,6 +118,8 @@ set(SRC_HDRS_QOBJECT_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_INC}/safecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/studentcontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/deepccontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/rampccontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/loggingtab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/topbanner.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/tuningcontrollertab.h
)
......@@ -134,6 +136,8 @@ qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/safecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/studentcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/deepccontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/rampccontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/loggingtab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/topbanner.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/tuningcontrollertab.ui
)
......@@ -219,6 +223,7 @@ add_service_files(
CMCommand.srv
LoadYamlFromFilename.srv
GetSetpointService.srv
LoggingService.srv
)
## Generate actions in the 'action' folder
......@@ -358,6 +363,10 @@ add_executable(PickerControllerService src/nodes/PickerControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(DeepcControllerService src/nodes/DeepcControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(RampcControllerService src/nodes/RampcControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(Logging src/nodes/Logging.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(TestMotorsControllerService src/nodes/TestMotorsControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
......@@ -402,6 +411,8 @@ set(FLYING_AGENT_GUI_CPP_SOURCES # compilation of sources
${FLYING_AGENT_GUI_LIB_PATH_SRC}/safecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/studentcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/deepccontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/rampccontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/loggingtab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/topbanner.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/tuningcontrollertab.cpp
)
......@@ -434,7 +445,9 @@ add_dependencies(MpcControllerService dfall_pkg_generate_messages_cpp ${cat
add_dependencies(RemoteControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TuningControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PickerControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DeepcControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DeepcControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(RampcControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(Logging dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TestMotorsControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -488,6 +501,9 @@ target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
target_link_libraries(PickerControllerService ${catkin_LIBRARIES})
target_link_libraries(DeepcControllerService ${catkin_LIBRARIES} ${GUROBI_LIBRARIES} ${OSQP_LIBRARY})
target_link_libraries(RampcControllerService ${catkin_LIBRARIES} ${GUROBI_LIBRARIES} ${OSQP_LIBRARY})
target_link_libraries(Logging ${catkin_LIBRARIES} ${GUROBI_LIBRARIES} ${OSQP_LIBRARY})
target_link_libraries(TestMotorsControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
......
......@@ -30,6 +30,8 @@ SOURCES += src/main.cpp\
src/pickercontrollertab.cpp \
src/deepccontrollertab.cpp \
src/tuningcontrollertab.cpp \
src/loggingtab.cpp \
src/rampccontrollertab.cpp
HEADERS += include/mainwindow.h \
......@@ -46,6 +48,8 @@ HEADERS += include/mainwindow.h \
include/deepccontrollertab.h \
include/tuningcontrollertab.h \
include/Constants_for_Qt_compile.h \
include/loggingtab.h \
include/rampccontrollertab.h
......@@ -62,6 +66,8 @@ FORMS += forms/mainwindow.ui \
forms/pickercontrollertab.ui \
forms/deepccontrollertab.ui \
forms/tuningcontrollertab.ui \
forms/loggingtab.ui \
forms/rampccontrollertab.ui
RESOURCES += \
......
......@@ -28,7 +28,7 @@
<item row="0" column="0">
<widget class="QTabWidget" name="controller_tabs_widget">
<property name="currentIndex">
<number>4</number>
<number>5</number>
</property>
<property name="movable">
<bool>true</bool>
......@@ -83,6 +83,29 @@
</item>
</layout>
</widget>
<widget class="QWidget" name="rampc_tab">
<attribute name="title">
<string>RAMPC</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_8">
<item row="0" column="0">
<widget class="RampcControllerTab" name="rampc_controller_tab_widget" native="true"/>
</item>
</layout>
</widget>
<widget class="QWidget" name="logging_tab">
<property name="accessibleName">
<string>Logging</string>
</property>
<attribute name="title">
<string>Logging</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_3">
<item row="0" column="0">
<widget class="LoggingTab" name="logging_tab_widget" native="true"/>
</item>
</layout>
</widget>
</widget>
</item>
</layout>
......@@ -118,6 +141,18 @@
<header>deepccontrollertab.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>LoggingTab</class>
<extends>QWidget</extends>
<header>loggingtab.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>RampcControllerTab</class>
<extends>QWidget</extends>
<header location="global">rampccontrollertab.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources/>
<connections/>
......
......@@ -30,37 +30,6 @@
<property name="spacing">
<number>12</number>
</property>
<item row="0" column="2">
<widget class="QPushButton" name="enable_student_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>60</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string>Student</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="load_yaml_label">
<property name="sizePolicy">
......@@ -95,7 +64,7 @@
</property>
</widget>
</item>
<item row="0" column="6">
<item row="0" column="7">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
......@@ -164,6 +133,37 @@
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QPushButton" name="enable_student_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>60</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string>Student</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QPushButton" name="load_yaml_student_button">
<property name="sizePolicy">
......@@ -385,6 +385,56 @@
</property>
</widget>
</item>
<item row="0" column="6">
<widget class="QPushButton" name="enable_rampc_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>60</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="text">
<string>RAMPC</string>
</property>
</widget>
</item>
<item row="1" column="6">
<widget class="QPushButton" name="load_yaml_rampc_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>60</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="text">
<string>RAMPC</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
......
......@@ -190,7 +190,7 @@
<x>0</x>
<y>0</y>
<width>1862</width>
<height>40</height>
<height>26</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
......
......@@ -115,6 +115,7 @@
#define TUNING_CONTROLLER 6
#define PICKER_CONTROLLER 7
#define DEEPC_CONTROLLER 8
#define RAMPC_CONTROLLER 9
#define TESTMOTORS_CONTROLLER 21
......@@ -128,6 +129,7 @@
#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_USE_PICKER_CONTROLLER 7
#define CMD_USE_DEEPC_CONTROLLER 8
#define CMD_USE_RAMPC_CONTROLLER 9
#define CMD_CRAZYFLY_TAKE_OFF 11
......
......@@ -71,7 +71,7 @@
#else
// Include the shared definitions
//#include "include/Constants_for_Qt_compile.h"
#include "include/Constants_for_Qt_compile.h"
#endif
......@@ -94,6 +94,8 @@ public:
void showHideController_picker_changed();
void showHideController_tuning_changed();
void showHideController_deepc_changed();
void showHideController_rampc_changed();
void showHideController_logging_changed();
public slots:
......@@ -107,6 +109,9 @@ signals:
void poseDataUnavailableSignal();
private slots:
void on_logging_tab_widget_destroyed();
private:
Ui::ControllerTabs *ui;
......
......@@ -93,6 +93,8 @@ public:
void showHideController_picker_changed();
void showHideController_tuning_changed();
void showHideController_deepc_changed();
void showHideController_rampc_changed();
//void showHideController_logging_changed();
void testMotors_triggered();
......@@ -109,6 +111,8 @@ private slots:
void on_enable_picker_button_clicked();
void on_enable_tuning_button_clicked();
void on_enable_deepc_button_clicked();
void on_enable_rampc_button_clicked();
//void on_enable_logging_button_clicked();
// LOAD YAML BUTTONS ON-CLICK CALLBACK
void on_load_yaml_default_button_clicked();
......@@ -116,6 +120,8 @@ private slots:
void on_load_yaml_picker_button_clicked();
void on_load_yaml_tuning_button_clicked();
void on_load_yaml_deepc_button_clicked();
void on_load_yaml_rampc_button_clicked();
//void on_load_yaml_logging_button_clicked();
private:
......
// Copyright (C) 2019, 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:
// The GUI for Logging
//
// ----------------------------------------------------------------------------------
#ifndef LOGGINGTAB_H
#define LOGGINGTAB_H
#include <QWidget>
#include <QMutex>
#include <QVector>
#include <QLineEdit>
#include <QTextStream>
#ifdef CATKIN_MAKE
#include <ros/ros.h>
#include <ros/network.h>
#include <ros/package.h>
// Include the standard message types
//#include "std_msgs/Int32.h"
//#include "std_msgs/Float32.h"
//#include <std_msgs/String.h>
// Include the DFALL message types
//#include "dfall_pkg/IntWithHeader.h"
#include "dfall_pkg/SetpointWithHeader.h"
#include "dfall_pkg/CustomButtonWithHeader.h"
// Include the DFALL service types
#include "dfall_pkg/GetSetpointService.h"
// Include the shared definitions
#include "nodes/Constants.h"
#include "nodes/DeepcControllerConstants.h"
// SPECIFY THE PACKAGE NAMESPACE
//using namespace dfall_pkg;
#else
// Include the shared definitions
#include "include/Constants_for_Qt_compile.h"
#endif
namespace Ui {
class LoggingTab;
}
class LoggingTab : public QWidget
{
Q_OBJECT
public:
explicit LoggingTab(QWidget *parent = 0);
~LoggingTab();
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
void poseDataUnavailableSlot();
private slots:
/*
void on_lineEdit_setpoint_new_x_returnPressed();
void on_lineEdit_setpoint_new_y_returnPressed();
void on_lineEdit_setpoint_new_z_returnPressed();
void on_lineEdit_setpoint_new_yaw_returnPressed();
void on_set_setpoint_button_clicked();
void on_default_setpoint_button_clicked();
*/
void on_custom_button_1_clicked();
//void on_custom_button_2_clicked();
//void on_custom_button_3_clicked();
//void on_custom_button_4_clicked();
//void on_custom_button_5_clicked();
private:
Ui::LoggingTab *ui;
// --------------------------------------------------- //
// PRIVATE VARIABLES
// The type of this node, i.e., agent or a coordinator,
// specified as a parameter in the "*.launch" file
int m_type = 0;
// The ID of this node
int m_ID;
// For coordinating multiple agents
std::vector<int> m_vector_of_agentIDs_toCoordinate;
bool m_shouldCoordinateAll = true;
QMutex m_agentIDs_toCoordinate_mutex;
// Mutex for the current state label
QMutex m_label_current_state_mutex;
#ifdef CATKIN_MAKE
// PUBLISHER
// > For requesting the setpoint to be changed
ros::Publisher requestSetpointChangePublisher;
// SUBSCRIBER
// > For being notified when the setpoint is changed
ros::Subscriber setpointChangedSubscriber;
// PUBLISHER
// > For notifying that a custom button is pressed
ros::Publisher customButtonPublisher;
#endif
// --------------------------------------------------- //
// PRIVATE FUNCTIONS
#ifdef CATKIN_MAKE
// For receiving message that the setpoint was changed
void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
// Publish a message when a custom button is pressed
void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
// Fill the header for a message
void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
void fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg );
// Get the paramters that specify the type and ID
bool getTypeAndIDParameters();
#endif
void publishSetpoint(float x, float y, float z, float yaw_degrees);
};
#endif // LOGGINGTAB_H
......@@ -132,6 +132,8 @@ private slots:
void on_action_showHideController_picker_changed();
void on_action_showHideController_tuning_changed();
void on_action_showHideController_deepc_changed();
void on_action_showHideController_rampc_changed();
void on_action_showHideController_logging_changed();
void on_action_testMotors_triggered();
......
// Copyright (C) 2019, 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:
// The GUI for a RAMPC Controller for students build from
//
// ----------------------------------------------------------------------------------
#ifndef RAMPCCONTROLLERTAB_H
#define RAMPCCONTROLLERTAB_H
#include <QWidget>
#include <QMutex>
#include <QVector>
#include <QLineEdit>
#include <QTextStream>
#ifdef CATKIN_MAKE
#include <ros/ros.h>
#include <ros/network.h>
#include <ros/package.h>
// Include the standard message types
//#include "std_msgs/Int32.h"
//#include "std_msgs/Float32.h"
//#include <std_msgs/String.h>
// Include the DFALL message types
//#include "dfall_pkg/IntWithHeader.h"
#include "dfall_pkg/SetpointWithHeader.h"
#include "dfall_pkg/CustomButtonWithHeader.h"
// Include the DFALL service types
#include "dfall_pkg/GetSetpointService.h"
// Include the shared definitions
#include "nodes/Constants.h"
#include "nodes/RampcControllerConstants.h"
// SPECIFY THE PACKAGE NAMESPACE
//using namespace dfall_pkg;
#else
// Include the shared definitions
#include "include/Constants_for_Qt_compile.h"
#endif
namespace Ui {
class RampcControllerTab;
}
class RampcControllerTab : public QWidget
{
Q_OBJECT
public:
explicit RampcControllerTab(QWidget *parent = 0);
~RampcControllerTab();
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
void poseDataUnavailableSlot();
private slots:
void on_lineEdit_setpoint_new_x_returnPressed();
void on_lineEdit_setpoint_new_y_returnPressed();
void on_lineEdit_setpoint_new_z_returnPressed();
void on_lineEdit_setpoint_new_yaw_returnPressed();
void on_set_setpoint_button_clicked();
void on_default_setpoint_button_clicked();
void on_custom_button_1_clicked();
void on_custom_button_2_clicked();
void on_custom_button_3_clicked();
void on_custom_button_4_clicked();
void on_custom_button_5_clicked();
private:
Ui::RampcControllerTab *ui;
// --------------------------------------------------- //
// PRIVATE VARIABLES
// The type of this node, i.e., agent or a coordinator,
// specified as a parameter in the "*.launch" file
int m_type = 0;
// The ID of this node
int m_ID;
// For coordinating multiple agents
std::vector<int> m_vector_of_agentIDs_toCoordinate;
bool m_shouldCoordinateAll = true;
QMutex m_agentIDs_toCoordinate_mutex;
// Mutex for the current state label
QMutex m_label_current_state_mutex;
#ifdef CATKIN_MAKE
// PUBLISHER
// > For requesting the setpoint to be changed
ros::Publisher requestSetpointChangePublisher;
// SUBSCRIBER
// > For being notified when the setpoint is changed
ros::Subscriber setpointChangedSubscriber;
// PUBLISHER
// > For notifying that a custom button is pressed
ros::Publisher customButtonPublisher;
#endif
// --------------------------------------------------- //
// PRIVATE FUNCTIONS
#ifdef CATKIN_MAKE
// For receiving message that the setpoint was changed
void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
// Publish a message when a custom button is pressed
void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
// Fill the header for a message
void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
void fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg );
// Get the paramters that specify the type and ID
bool getTypeAndIDParameters();
#endif
void publishSetpoint(float x, float y, float z, float yaw_degrees);
};
#endif // RAMPCCONTROLLERTAB_H
......@@ -82,6 +82,16 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->deepc_controller_tab_widget , &DeepcControllerTab::setMeasuredPose
);
QObject::connect(
this , &ControllerTabs::measuredPoseValueChanged ,
ui->rampc_controller_tab_widget , &RampcControllerTab::setMeasuredPose
);
QObject::connect(
this , &ControllerTabs::measuredPoseValueChanged ,
ui->logging_tab_widget , &LoggingTab::setMeasuredPose
);
// CONNECT THE "MEASUREMENTS UNAVAILABLE" SIGNAL TO
......@@ -111,6 +121,15 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->deepc_controller_tab_widget , &DeepcControllerTab::poseDataUnavailableSlot
);
QObject::connect(
this , &ControllerTabs::poseDataUnavailableSignal ,
ui->rampc_controller_tab_widget , &RampcControllerTab::poseDataUnavailableSlot
);
QObject::connect(
this , &ControllerTabs::poseDataUnavailableSignal ,
ui->logging_tab_widget , &LoggingTab::poseDataUnavailableSlot
);
// CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
// WITH THE LIST OF AGENT IDs TO COORDINATE
......@@ -142,7 +161,16 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->deepc_controller_tab_widget , &DeepcControllerTab::setAgentIDsToCoordinate
);
QObject::connect(
this , &ControllerTabs::agentIDsToCoordinateChanged ,
ui->rampc_controller_tab_widget , &RampcControllerTab::setAgentIDsToCoordinate
);
QObject::connect(
this , &ControllerTabs::agentIDsToCoordinateChanged ,
ui->logging_tab_widget , &LoggingTab::setAgentIDsToCoordinate
);
......@@ -252,8 +280,15 @@ void ControllerTabs::showHideController_deepc_changed()
showHideController_toggle("Deepc",ui->deepc_tab);
}
void ControllerTabs::showHideController_rampc_changed()
{
showHideController_toggle("Rampc",ui->rampc_tab);
}
void ControllerTabs::showHideController_logging_changed()
{
showHideController_toggle("Logging",ui->logging_tab);
}
......@@ -437,6 +472,13 @@ void ControllerTabs::setControllerEnabled(int new_controller)
setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->deepc_tab );
break;
}
case RAMPC_CONTROLLER:
{
setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->rampc_tab );
break;
}
default:
{
//ui->controller_enabled_label->setText("Unknown");
......@@ -453,6 +495,8 @@ void ControllerTabs::setAllTabLabelsToNormalColouring()
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->picker_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->tuning_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->deepc_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->rampc_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->logging_tab );
}
void ControllerTabs::setTextColourOfTabLabel(QColor color , QWidget * tab_widget)
......@@ -645,3 +689,8 @@ bool ControllerTabs::getTypeAndIDParameters()
return return_was_successful;
}
#endif
void ControllerTabs::on_logging_tab_widget_destroyed()
{
}
......@@ -113,8 +113,18 @@ void EnableControllerLoadYamlBar::showHideController_deepc_changed()
ui->enable_deepc_button ->setHidden( !(ui->enable_deepc_button->isHidden()) );
ui->load_yaml_deepc_button->setHidden( !(ui->load_yaml_deepc_button->isHidden()) );
}
void EnableControllerLoadYamlBar::showHideController_rampc_changed()
{
ui->enable_rampc_button ->setHidden( !(ui->enable_rampc_button->isHidden()) );
ui->load_yaml_rampc_button->setHidden( !(ui->load_yaml_rampc_button->isHidden()) );
}
/*
void EnableControllerLoadYamlBar::showHideController_logging_changed()
{
ui->enable_logging_button ->setHidden( !(ui->enable_logging_button->isHidden()) );
ui->load_yaml_logging_button->setHidden( !(ui->load_yaml_logging_button->isHidden()) );
}
*/
// TEST MOTORS
......@@ -189,6 +199,16 @@ void EnableControllerLoadYamlBar::on_enable_deepc_button_clicked()
#endif
}
void EnableControllerLoadYamlBar::on_enable_rampc_button_clicked()
{
#ifdef CATKIN_MAKE
dfall_pkg::IntWithHeader msg;
fillIntMessageHeader(msg);
msg.data = CMD_USE_RAMPC_CONTROLLER;
this->commandPublisher.publish(msg);
ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable RAMPC Controller");
#endif
}
......@@ -277,7 +297,21 @@ void EnableControllerLoadYamlBar::on_load_yaml_deepc_button_clicked()
#endif
}
void EnableControllerLoadYamlBar::on_load_yaml_rampc_button_clicked()
{
#ifdef CATKIN_MAKE
// Create a local variable for the message
dfall_pkg::StringWithHeader yaml_filename_msg;
// Set for whom this applies to
fillStringMessageHeader(yaml_filename_msg);
// Specify the data
yaml_filename_msg.data = "RampcController";
// Send the message
m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
// Inform the user that the menu item was selected
ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load RAMPC Controller YAML was clicked.");
#endif
}
......
......@@ -249,7 +249,19 @@ void MainWindow::on_action_showHideController_deepc_changed()
ui->customWidget_enableControllerLoadYamlBar->showHideController_deepc_changed();
ui->customWidget_controller_tabs->showHideController_deepc_changed();
}
void MainWindow::on_action_showHideController_rampc_changed()
{
// Notify the UI elements of this change
//ui->customWidget_enableControllerLoadYamlBar->showHideController_rampc_changed();
ui->customWidget_controller_tabs->showHideController_rampc_changed();
}
void MainWindow::on_action_showHideController_logging_changed()
{
// Notify the UI elements of this change
//ui->customWidget_enableControllerLoadYamlBar->showHideController_logging_changed();
ui->customWidget_controller_tabs->showHideController_logging_changed();
}
void MainWindow::on_action_testMotors_triggered()
{
......
......@@ -128,6 +128,7 @@
#define TUNING_CONTROLLER 6
#define PICKER_CONTROLLER 7
#define DEEPC_CONTROLLER 8
#define RAMPC_CONTROLLER 9
#define TESTMOTORS_CONTROLLER 21
......@@ -141,6 +142,7 @@
#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_USE_PICKER_CONTROLLER 7
#define CMD_USE_DEEPC_CONTROLLER 8
#define CMD_USE_RAMPC_CONTROLLER 9
#define CMD_CRAZYFLY_TAKE_OFF 11
......
......@@ -64,12 +64,12 @@
#include "dfall_pkg/Controller.h"
#include "dfall_pkg/CMQuery.h"
#include "dfall_pkg/IntIntService.h"
#include "dfall_pkg/LoggingService.h"
// Include the shared definitions
#include "nodes/Constants.h"
#include "nodes/DefaultControllerConstants.h"
#include "nodes/DeepcControllerConstants.h"
#include "nodes/RampcControllerConstants.h"
// Include other classes
#include "classes/GetParamtersAndNamespaces.h"
......@@ -174,6 +174,10 @@ ros::ServiceClient m_defaultController_requestManoeuvre;
// to perform a {take-off,landing} maneouvre
ros::ServiceClient m_deepcController_requestManoeuvre;
// > Service Clients for requesting the Deepc controller
// to perform a {take-off,landing} maneouvre
ros::ServiceClient m_rampcController_requestManoeuvre;
// > Timer that fire when the {take-off,landing} is complete
ros::Timer m_timer_takeoff_complete;
ros::Timer m_timer_land_complete;
......@@ -212,11 +216,14 @@ ros::ServiceClient m_tuningController;
ros::ServiceClient m_pickerController;
// The Deepc controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_deepcController;
// The RAMPC controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_rampcController;
// The Test Mtoros controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_testMotorsController;
// The logging service
ros::ServiceClient m_logging;
......@@ -329,6 +336,8 @@ int getControllerNominallySelected();
void flyingStateOrControllerRequestCallback(const IntWithHeader & commandMsg);
// The callback that logging was requested
//void loggingRequestCallback(LoggingService::Request &request, LoggingService::Response &response);
// THE CALLBACK THAT THE CRAZY RADIO STATUS CHANGED
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
......@@ -343,7 +352,7 @@ void emergencyStopReceivedCallback(const IntWithHeader & msg);
// THE SERVICE CALLBACK REQUESTING THE CURRENT FLYING STATE
bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIntService::Response &response);
void loggingRequestCallback(const std_msgs::Int32 &msg);
// FOR THE BATTERY STATE CALLBACK
void batteryMonitorStateChangedCallback(std_msgs::Int32 msg);
......@@ -365,6 +374,8 @@ void createControllerServiceClientFromParameterName( std::string paramter_name ,
// > For creating an IntInt service client from the
// name of a YAML paramter
void createIntIntServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
void createLoggingServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
// > Callback for the timer so that all services servers
// exists before we try to create the clients
void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&);
......
This diff is collapsed.
// Copyright (C) 2019, 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:
// Constants that are used across the Rampc Controller
// Service and the Flying Agent Client
//
// ----------------------------------------------------------------------------------
// TO REQUEST MANOEUVRES
#define RAMPC_CONTROLLER_REQUEST_TAKEOFF 1
#define RAMPC_CONTROLLER_REQUEST_LANDING 2
// TO NOTIFY THAT MANOEUVRES ARE COMPLETED
#define RAMPC_CONTROLLER_LANDING_COMPLETE 2
// TO IDENITFY THE STATE OF THE RAMPC CONTROLLER
#define RAMPC_CONTROLLER_STATE_LQR 11
#define RAMPC_CONTROLLER_STATE_EXCITATION_LQR 12
#define RAMPC_CONTROLLER_STATE_RAMPC 13
#define RAMPC_CONTROLLER_STATE_EXCITATION_RAMPC 14
#define RAMPC_CONTROLLER_STATE_STANDBY 99
// > The sequence of states for a LANDING manoeuvre
#define RAMPC_CONTROLLER_STATE_LANDING_MOVE_DOWN 21
#define RAMPC_CONTROLLER_STATE_LANDING_SPIN_MOTORS 22
// OPTIMIZATION SOLVERS
#define RAMPC_CONTROLLER_SOLVER_GUROBI 0
#define RAMPC_CONTROLLER_SOLVER_OSQP 1
#define RAMPC_CONTROLLER_SOLVER_MPC 2
\ No newline at end of file
This diff is collapsed.
......@@ -189,7 +189,7 @@ float m_previous_stateErrorInertial[9];
// with units [meters,meters,meters,radians]
std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
/*
// The LQR Controller parameters for "LQR_RATE_MODE"
std::vector<float> m_gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
std::vector<float> m_gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
......@@ -197,6 +197,20 @@ std::vector<float> m_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.
std::vector<float> m_gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
std::vector<float> m_gainMatrixThrust1 = { -0.0068, 0.0036, 0.2968, -0.0068, 0.0036, 0.0873, -0.0142, -0.0267, -0.0508, -0.0027, -0.0051, -0.0173};
std::vector<float> m_gainMatrixThrust2 = { 0.0066, 0.0021, 0.2968, 0.0065, 0.0020, 0.0873, -0.0080, 0.0256, 0.0503, -0.0015, 0.0049, 0.0172};
std::vector<float> m_gainMatrixThrust3 = { 0.0036, 0.0021, 0.2968, 0.0036, -0.0023, 0.0873, 0.0091, 0.0139, -0.0492, 0.0017, 0.0027, -0.0168};
std::vector<float> m_gainMatrixThrust4 = { -0.0033, -0.0034, 0.2968, -0.0033, -0.0034, 0.0873, 0.0131, -0.0128, 0.0497, 0.0025, -0.0025, 0.0170};
*/
std::vector<float> m_gainMatrixThrust1 = { -0.0140, 0.0075, 0.2968, -0.0109, 0.0058, 0.0873, -0.0186, -0.0350, -0.0328, -0.0030, -0.0056, -0.0155};
std::vector<float> m_gainMatrixThrust2 = { 0.0134, 0.0042, 0.2968, 0.0105, 0.0033, 0.0873, -0.0105, 0.0335, 0.0325, -0.0017, 0.0054, 0.0153};
std::vector<float> m_gainMatrixThrust3 = { 0.0074, -0.0048, 0.2968, 0.0057, -0.0037, 0.0873, 0.0120, 0.0183, -0.0317, 0.0019, 0.0029, -0.0150};
std::vector<float> m_gainMatrixThrust4 = { -0.0068, -0.0069, 0.2968, -0.0053, -0.0054, 0.0873, 0.0172, -0.0169, 0.0320, 0.0028, -0.0027, 0.0151};
// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;
......@@ -234,7 +248,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
// INTO AN (x,y) BODY FRAME ERROR
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured);
// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);
......
......@@ -101,6 +101,24 @@
>
</node>
<!-- RAMPC CONTROLLER -->
<node
pkg = "dfall_pkg"
name = "RampcControllerService"
output = "screen"
type = "RampcControllerService"
>
</node>
<!-- LOGGING -->
<node
pkg = "dfall_pkg"
name = "Logging"
output = "screen"
type = "Logging"
>
</node>
<!-- TEST MOTORS CONTROLLER -->
<node
pkg = "dfall_pkg"
......@@ -154,6 +172,11 @@
file = "$(find dfall_pkg)/param/DeepcController.yaml"
ns = "DeepcController"
/>
<rosparam
command = "load"
file = "$(find dfall_pkg)/param/RampcController.yaml"
ns = "RampcController"
/>
</node>
......
......@@ -145,10 +145,10 @@ grb_presolve_at_setup : true
# This implements a 'Figure 8' in x and y and a sine in z
# Figure 8 amplitude, in m
figure_8_amplitude : 1
figure_8_amplitude : 1 #1
# Figure 8 frequency, in Hz
figure_8_frequency : 0.2
figure_8_frequency : 0.2 #0.2
# z sine amplitude, in m
z_sine_amplitude : 0.3
z_sine_amplitude : 0.3 #0.3
# z sine frequency, in Hz
z_sine_frequency : 0.1
\ No newline at end of file
z_sine_frequency : 0.1 #0.1
\ No newline at end of file
......@@ -5,6 +5,8 @@ studentController: "StudentControllerService/StudentController"
tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
deepcController: "DeepcControllerService/DeepcController"
rampcController: "RampcControllerService/RampcController"
logging: "Logging/LoggingService"
testMotorsController: "TestMotorsControllerService/TestMotorsController"
......@@ -15,6 +17,7 @@ defaultController_requestManoeuvre: "DefaultControllerService/RequestManoeuvre"
# The name of the service advertised by the Default
# Controller for requests manoeuvres to be performed
deepcController_requestManoeuvre: "DeepcControllerService/RequestManoeuvre"
rampcController_requestManoeuvre: "RampcControllerService/RequestManoeuvre"
# Flag for whether to use angle for switching
# to the Default Controller
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
CrazyflieData ownCrazyflie
ControlCommand controlOutput
---