diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 69a8a5696c461f7d99ce7226d9d34ffbd27fdcaf..ea93a9fdffd97cc5736e86fb8e93052040542245 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -157,6 +157,7 @@ add_message_files(
#----------------------------------------------------------------------
DebugMsg.msg
CustomButton.msg
+ ViconSubscribeObjectName.msg
)
## Generate services in the 'srv' folder
@@ -268,6 +269,7 @@ add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
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(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
@@ -320,6 +322,7 @@ add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${cat
add_dependencies(DemoControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
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(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -360,6 +363,7 @@ if(Eigen3_FOUND)
else()
target_link_libraries(MpcControllerService ${catkin_LIBRARIES})
endif()
+target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
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 22eeb488518a048b71cb1b289d27f00a3dbdc007..e8480b3a54822766677e0baef0763626425bc38d 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
@@ -43,6 +43,7 @@
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ViconSubscribeObjectName.h"
// Types of controllers being used:
@@ -50,6 +51,7 @@
#define DEMO_CONTROLLER 2
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
+#define REMOTE_CONTROLLER 5
// Commands for CrazyRadio
@@ -69,6 +71,7 @@
#define CMD_USE_DEMO_CONTROLLER 2
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
+#define CMD_USE_REMOTE_CONTROLLER 5
#define CMD_CRAZYFLY_TAKE_OFF 11
#define CMD_CRAZYFLY_LAND 12
@@ -89,11 +92,13 @@
#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
+#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
#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
// Universal constants
#define PI 3.141592653589
@@ -136,12 +141,14 @@ private slots:
void on_load_demo_yaml_button_clicked();
void on_load_student_yaml_button_clicked();
void on_load_mpc_yaml_button_clicked();
+ void on_load_remote_yaml_button_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_enable_remote_controller_clicked();
@@ -149,6 +156,13 @@ private slots:
void on_customButton_2_clicked();
void on_customButton_3_clicked();
+ // Buttons within the REMOTE controller tab
+ void on_remote_subscribe_button_clicked();
+ void on_remote_unsubscribe_button_clicked();
+ void on_remote_activate_button_clicked();
+ void on_remote_deactivate_button_clicked();
+
+
private:
Ui::MainWindow *ui;
@@ -164,6 +178,7 @@ private:
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;
+ ros::Timer m_timer_yaml_file_for_remote_controller;
int m_student_id;
CrazyflieContext m_context;
@@ -185,16 +200,24 @@ private:
ros::Publisher controllerSetpointPublisher;
ros::Subscriber safeSetpointSubscriber;
- // SUBSCRIBERS AND PUBLISHERS FOR THE SETPOINTS:
- // > For the Demo Controller
+ // SUBSCRIBERS AND PUBLISHERS:
+ // > For the Demo Controller SETPOINTS
ros::Publisher demoSetpointPublisher;
ros::Subscriber demoSetpointSubscriber;
- // > For the Student Controller
+ // > For the Student Controller SETPOINTS
ros::Publisher studentSetpointPublisher;
ros::Subscriber studentSetpointSubscriber;
- // > For the MPC Controller
+ // > 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
+ ros::Publisher remoteActivatePublisher;
+ // > For the Remote Controller data
+ ros::Subscriber remoteDataSubscriber;
+ // > For the Remote Control setpoint
+ ros::Subscriber remoteControlSetpointSubscriber;
@@ -229,6 +252,11 @@ private:
void studentSetpointCallback(const Setpoint& newSetpoint);
void mpcSetpointCallback(const Setpoint& newSetpoint);
+
+ void remoteDataCallback(const CrazyflieData& objectData);
+ void remoteControlSetpointCallback(const CrazyflieData& setpointData);
+
+
void DBChangedCallback(const std_msgs::Int32& msg);
// # Load Yaml when acting as the GUI for an Agent
@@ -236,6 +264,7 @@ private:
void demoYamlFileTimerCallback(const ros::TimerEvent&);
void studentYamlFileTimerCallback(const ros::TimerEvent&);
void mpcYamlFileTimerCallback(const ros::TimerEvent&);
+ void remoteYamlFileTimerCallback(const ros::TimerEvent&);
@@ -260,6 +289,7 @@ private:
void highlightDemoControllerTab();
void highlightStudentControllerTab();
void highlightMpcControllerTab();
+ void highlightRemoteControllerTab();
bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
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 fbfeee725a03a2ce4f7b148db8fbbe87181fc165..dcb30009b620cb1614a2e229c7e75f7d4d060a34 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
@@ -59,7 +59,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
setCrazyRadioStatus(DISCONNECTED);
m_ros_namespace = ros::this_node::getNamespace();
- ROS_INFO("Student GUI node namespace: %s", m_ros_namespace.c_str());
+ ROS_INFO("[Student GUI] node namespace: %s", m_ros_namespace.c_str());
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
@@ -67,17 +67,28 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ros::NodeHandle nodeHandle(m_ros_namespace);
- // SUBSCRIBERS AND PUBLISHERS FOR THE SETPOINTS:
- // > For the Demo Controller
+ // SUBSCRIBERS AND PUBLISHERS:
+ // > For the Demo Controller SETPOINTS
demoSetpointPublisher = nodeHandle.advertise<Setpoint>("DemoControllerService/Setpoint", 1);
demoSetpointSubscriber = nodeHandle.subscribe("DemoControllerService/Setpoint", 1, &MainWindow::demoSetpointCallback, this);
- // > For the Student Controller
+ // > For the Student Controller SETPOINTS
studentSetpointPublisher = nodeHandle.advertise<Setpoint>("StudentControllerService/Setpoint", 1);
studentSetpointSubscriber = nodeHandle.subscribe("StudentControllerService/Setpoint", 1, &MainWindow::studentSetpointCallback, this);
- // > For the MPC Controller
+ // > For the MPC Controller SETPOINTS
mpcSetpointPublisher = nodeHandle.advertise<Setpoint>("MpcControllerService/Setpoint", 1);
mpcSetpointSubscriber = nodeHandle.subscribe("MpcControllerService/Setpoint", 1, &MainWindow::mpcSetpointCallback, this);
+
+ // > For the Remote Controller subscribe action
+ remoteSubscribePublisher = nodeHandle.advertise<ViconSubscribeObjectName>("RemoteControllerService/ViconSubscribeObjectName", 1);
+ // > For the Remote Controller activate action
+ remoteActivatePublisher = nodeHandle.advertise<std_msgs::Int32>("RemoteControllerService/Activate", 1);
+ // > For the Remote Controller data
+ remoteDataSubscriber = nodeHandle.subscribe("RemoteControllerService/RemoteData", 1, &MainWindow::remoteDataCallback, this);;
+ // > For the Remote Controller data
+ remoteControlSetpointSubscriber = nodeHandle.subscribe("RemoteControllerService/RemoteControlSetpoint", 1, &MainWindow::remoteControlSetpointCallback, this);;
+
+
// subscribers
crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
@@ -196,6 +207,7 @@ void MainWindow::highlightSafeControllerTab()
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);
}
void MainWindow::highlightDemoControllerTab()
{
@@ -203,6 +215,7 @@ void MainWindow::highlightDemoControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
}
void MainWindow::highlightStudentControllerTab()
{
@@ -210,6 +223,7 @@ void MainWindow::highlightStudentControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
}
void MainWindow::highlightMpcControllerTab()
{
@@ -217,6 +231,15 @@ void MainWindow::highlightMpcControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::green);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+}
+void MainWindow::highlightRemoteControllerTab()
+{
+ 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::green);
}
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
@@ -241,6 +264,9 @@ void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
case MPC_CONTROLLER:
highlightMpcControllerTab();
break;
+ case REMOTE_CONTROLLER:
+ highlightRemoteControllerTab();
+ break;
default:
break;
}
@@ -811,6 +837,37 @@ void MainWindow::mpcYamlFileTimerCallback(const ros::TimerEvent&)
+
+void MainWindow::on_load_remote_yaml_button_clicked()
+{
+ // Set the "load remote yaml" button to be disabled
+ ui->load_remote_yaml_button->setEnabled(false);
+
+ // Send a message requesting the parameters from the YAML
+ // file to be reloaded for the remote controller
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_REMOTE_CONTROLLER_AGENT;
+ this->requestLoadControllerYamlPublisher.publish(msg);
+ ROS_INFO("[STUDENT GUI] Request load of remote 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_remote_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::remoteYamlFileTimerCallback, this, true);
+}
+
+void MainWindow::remoteYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load remote yaml" button again
+ ui->load_remote_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
@@ -883,6 +940,21 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
break;
+ case LOAD_YAML_REMOTE_CONTROLLER_AGENT:
+ case LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR:
+ // Set the "load remote yaml" button to be disabled
+ ui->load_remote_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_remote_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::remoteYamlFileTimerCallback, this, true);
+
+ break;
+
default:
ROS_INFO("Unknown 'all controllers to load yaml' command, thus nothing will be disabled");
break;
@@ -922,6 +994,13 @@ void MainWindow::on_enable_mpc_controller_clicked()
this->PPSClientCommandPublisher.publish(msg);
}
+void MainWindow::on_enable_remote_controller_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_REMOTE_CONTROLLER;
+ this->PPSClientCommandPublisher.publish(msg);
+}
+
// # Custom command buttons
@@ -953,6 +1032,102 @@ void MainWindow::on_customButton_3_clicked()
ROS_INFO("Custom button 3 pressed in GUI");
}
+
+
+
+
+// # Custom buttons for the REMOTE controller service
+void MainWindow::on_remote_subscribe_button_clicked()
+{
+ // Initialise the message
+ ViconSubscribeObjectName msg;
+ // Set the subscribe flag
+ msg.shouldSubscribe = true;
+ // Set the object name
+ msg.objectName = (ui->remote_object_name->text()).toUtf8().constData();
+ // Publish the message
+ this->remoteSubscribePublisher.publish(msg);
+}
+
+void MainWindow::on_remote_unsubscribe_button_clicked()
+{
+ // Initialise the message
+ ViconSubscribeObjectName msg;
+ // Set the subscribe flag
+ msg.shouldSubscribe = false;
+ // Set the object name
+ msg.objectName = (ui->remote_object_name->text()).toUtf8().constData();
+ // Publish the message
+ this->remoteSubscribePublisher.publish(msg);
+}
+
+void MainWindow::on_remote_activate_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 1;
+ // Publish the message
+ this->remoteActivatePublisher.publish(msg);
+}
+
+void MainWindow::on_remote_deactivate_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 0;
+ // Publish the message
+ this->remoteActivatePublisher.publish(msg);
+}
+
+void MainWindow::remoteDataCallback(const CrazyflieData& objectData)
+{
+ // Check if the object is occluded
+ if (objectData.occluded)
+ {
+ // Set the column heading label to have a red background
+ // > IMPORTANT: Set the background auto fill property to true
+ ui->remote_data_label->setAutoFillBackground(true);
+ // > Get the pallette currently set for the label
+ QPalette pal = ui->remote_roll_label->palette();
+ // > Set the palette property that will change the background
+ pal.setColor(QPalette::Window, QColor(Qt::red));
+ // > Update the palette for the label
+ ui->remote_data_label->setPalette(pal);
+ }
+ else
+ {
+ // Put the roll, pitch, yaw, and z data into the appropriate fields
+ ui->remote_data_roll ->setText(QString::number( objectData.roll * RAD2DEG, 'f', 1));
+ ui->remote_data_pitch->setText(QString::number( objectData.pitch * RAD2DEG, 'f', 1));
+ ui->remote_data_yaw ->setText(QString::number( objectData.yaw * RAD2DEG, 'f', 1));
+ ui->remote_data_z ->setText(QString::number( objectData.z, 'f', 2));
+
+ // Set the column heading label to have a "normal" background
+ // > IMPORTANT: Set the background auto fill property to true
+ ui->remote_data_label->setAutoFillBackground(false);
+ // > Get the pallette currently set for the roll label
+ QPalette pal = ui->remote_roll_label->palette();
+ // > Update the palette for the column heading label
+ ui->remote_data_label->setPalette(pal);
+ }
+}
+
+void MainWindow::remoteControlSetpointCallback(const CrazyflieData& setpointData)
+{
+ ui->remote_setpoint_roll ->setText(QString::number( setpointData.roll * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_pitch->setText(QString::number( setpointData.pitch * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_yaw ->setText(QString::number( setpointData.yaw * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_z ->setText(QString::number( setpointData.z, 'f', 2));
+}
+
+
+
+
+
+
+
Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
{
Setpoint corrected_setpoint;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index 4df498052a5c319512b7eac1b64e61563a25cc83..7513dcb809cac1e95ef34c7b436380b82ca8954c 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
@@ -369,7 +369,7 @@
</font>
</property>
<property name="currentIndex">
- <number>0</number>
+ <number>4</number>
</property>
<property name="usesScrollButtons">
<bool>true</bool>
@@ -2378,6 +2378,439 @@
</item>
</layout>
</widget>
+ <widget class="QWidget" name="tab_remote">
+ <attribute name="title">
+ <string>Remote</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_14">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_10">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>6</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QLineEdit" name="remote_object_name">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLineEdit" name="remote_data_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="remote_unsubscribe_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>UN-subscribe</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="2">
+ <widget class="QLineEdit" name="remote_setpoint_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="remote_subscribe_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Subscribe</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QLineEdit" name="remote_data_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="2">
+ <widget class="QLineEdit" name="remote_setpoint_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QLineEdit" name="remote_data_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="Line" name="line_12">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="Line" name="line_10">
+ <property name="frameShadow">
+ <enum>QFrame::Sunken</enum>
+ </property>
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="remote_setpoint_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="remote_data_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="Line" name="line_11">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="remote_setpoint_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLabel" name="remote_data_label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Remote</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLabel" name="label_38">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="remote_roll_label">
+ <property name="text">
+ <string>Roll [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="remote_pitch_label">
+ <property name="text">
+ <string>Pitch [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QLabel" name="remote_yaw_label">
+ <property name="text">
+ <string>Yaw [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <widget class="QLabel" name="remote_z_label">
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="1">
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QPushButton" name="remote_activate_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Activate</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="remote_deactivate_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>DE-activate</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="verticalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
</widget>
</item>
<item>
@@ -2401,6 +2834,36 @@
<property name="bottomMargin">
<number>6</number>
</property>
+ <item row="7" column="2">
+ <widget class="QPushButton" name="load_remote_yaml_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Remote</string>
+ </property>
+ </widget>
+ </item>
<item row="5" column="2">
<widget class="QPushButton" name="load_student_yaml_button">
<property name="sizePolicy">
@@ -2538,7 +3001,7 @@
</property>
</widget>
</item>
- <item row="7" column="0">
+ <item row="8" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
@@ -2653,22 +3116,22 @@
</property>
</widget>
</item>
- <item row="4" column="1">
- <widget class="Line" name="line_4">
+ <item row="1" column="1">
+ <widget class="Line" name="line_6">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
- <item row="7" column="1">
- <widget class="Line" name="line_5">
+ <item row="4" column="1">
+ <widget class="Line" name="line_4">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="Line" name="line_6">
+ <item row="8" column="1">
+ <widget class="Line" name="line_5">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
@@ -2729,6 +3192,36 @@
</property>
</widget>
</item>
+ <item row="7" column="0">
+ <widget class="QPushButton" name="enable_remote_controller">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Remote</string>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
<item>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.1400dcd b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.1400dcd
new file mode 100644
index 0000000000000000000000000000000000000000..8e1edbbfa62e3ca2e0c55ceaefa53f5f983a47e2
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.1400dcd
@@ -0,0 +1,271 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE QtCreatorProject>
+<!-- Written by QtCreator 3.5.1, 2017-10-18T15:20:06. -->
+<qtcreator>
+ <data>
+ <variable>EnvironmentId</variable>
+ <value type="QByteArray">{1400dcd4-82c6-466c-a808-34f7a3d4fe21}</value>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.ActiveTarget</variable>
+ <value type="int">0</value>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.EditorSettings</variable>
+ <valuemap type="QVariantMap">
+ <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
+ <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
+ <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
+ <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
+ <value type="QString" key="language">Cpp</value>
+ <valuemap type="QVariantMap" key="value">
+ <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
+ </valuemap>
+ </valuemap>
+ <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
+ <value type="QString" key="language">QmlJS</value>
+ <valuemap type="QVariantMap" key="value">
+ <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
+ </valuemap>
+ </valuemap>
+ <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
+ <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
+ <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
+ <value type="int" key="EditorConfiguration.IndentSize">4</value>
+ <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
+ <value type="int" key="EditorConfiguration.MarginColumn">80</value>
+ <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
+ <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
+ <value type="int" key="EditorConfiguration.PaddingMode">1</value>
+ <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
+ <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
+ <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
+ <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
+ <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
+ <value type="int" key="EditorConfiguration.TabSize">8</value>
+ <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
+ <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
+ <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
+ <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
+ <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
+ <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
+ </valuemap>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.PluginSettings</variable>
+ <valuemap type="QVariantMap"/>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.Target.0</variable>
+ <valuemap type="QVariantMap">
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{051621a5-413a-4a38-907c-a6d036ac454e}</value>
+ <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
+ <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
+ <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
+ <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop-Debug</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibraryAuto">true</value>
+ <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+ <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+ <value type="QString">-w</value>
+ <value type="QString">-r</value>
+ </valuelist>
+ <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+ <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+ <value type="QString">-w</value>
+ <value type="QString">-r</value>
+ </valuelist>
+ <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+ <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+ <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Debug</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+ <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">2</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
+ <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop-Release</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibraryAuto">true</value>
+ <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+ <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+ <value type="QString">-w</value>
+ <value type="QString">-r</value>
+ </valuelist>
+ <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+ <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+ <value type="QString">-w</value>
+ <value type="QString">-r</value>
+ </valuelist>
+ <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+ <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+ <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+ <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">2</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+ <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
+ <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
+ <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
+ <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
+ <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
+ <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
+ <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
+ <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
+ <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+ <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
+ <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
+ <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
+ <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
+ <value type="int">0</value>
+ <value type="int">1</value>
+ <value type="int">2</value>
+ <value type="int">3</value>
+ <value type="int">4</value>
+ <value type="int">5</value>
+ <value type="int">6</value>
+ <value type="int">7</value>
+ <value type="int">8</value>
+ <value type="int">9</value>
+ <value type="int">10</value>
+ <value type="int">11</value>
+ <value type="int">12</value>
+ <value type="int">13</value>
+ <value type="int">14</value>
+ </valuelist>
+ <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+ <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">studentGUI</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro</value>
+ <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
+ <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">studentGUI.pro</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseTerminal">false</value>
+ <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
+ <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
+ <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
+ <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+ <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
+ <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
+ <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
+ </valuemap>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.TargetCount</variable>
+ <value type="int">1</value>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
+ <value type="int">18</value>
+ </data>
+ <data>
+ <variable>Version</variable>
+ <value type="int">18</value>
+ </data>
+</qtcreator>
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
index f2f870cf40bb426c7abaf41136f5965d6fe36d41..3a37faf9d1bdafe6e65e9d559b1c4c13696ca09e 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -87,6 +87,7 @@
#define DEMO_CONTROLLER 2
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
+#define REMOTE_CONTROLLER 5
// The constants that "command" changes in the
// operation state of this agent
@@ -94,6 +95,7 @@
#define CMD_USE_DEMO_CONTROLLER 2
#define CMD_USE_STUDENT_CONTROLLER 3
#define CMD_USE_MPC_CONTROLLER 4
+#define CMD_USE_REMOTE_CONTROLLER 5
#define CMD_CRAZYFLY_TAKE_OFF 11
#define CMD_CRAZYFLY_LAND 12
@@ -145,10 +147,12 @@ int agentID;
ros::ServiceClient safeController;
// The Demo controller specified in the ClientConfig.yaml
ros::ServiceClient demoController;
-// The Demo controller specified in the ClientConfig.yaml
+// The Student controller specified in the ClientConfig.yaml
ros::ServiceClient studentController;
-// The Demo controller specified in the ClientConfig.yaml
+// The MPC controller specified in the ClientConfig.yaml
ros::ServiceClient mpcController;
+// The Remote controller specified in the ClientConfig.yaml
+ros::ServiceClient remoteController;
//values for safteyCheck
@@ -264,6 +268,8 @@ void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
+void viconCallback(const ViconData& viconData);
+
// > For the LOADING of YAML PARAMETERS
void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
@@ -290,6 +296,7 @@ void loadSafeController();
void loadDemoController();
void loadStudentController();
void loadMpcController();
+void loadRemoteController();
void sendMessageUsingController(int controller);
void setInstantController(int controller);
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
index 17d3b1dc069c0f6af8ba61f8a2d9de2143ca7e6b..7edcebdd65bd87a0b7253d5b9850fb584035f36a 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
@@ -39,11 +39,13 @@
#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
+#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
#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
// For sending commands to the controller node informing
@@ -57,8 +59,10 @@
#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
#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_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
\ No newline at end of file
+#define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
+#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..21eaae66e4b8d490e04203f0352cf53461ca6792
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
@@ -0,0 +1,479 @@
+// 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
+
+// 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 CF_COMMAND_TYPE_MOTOR 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 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_MODE_MOTOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands per motor thrusts
+//
+// LQR_MODE_ACTUATOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands actuators of total force and bodz torques
+//
+// LQR_MODE_RATE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_MODE_ANGLE LQR controller based on the state vector:
+// [position,velocity]
+//
+//#define CONTROLLER_MODE_LQR_MOTOR 1
+//#define CONTROLLER_MODE_LQR_ACTUATOR 2
+#define CONTROLLER_MODE_LQR_RATE 3 // (DEFAULT)
+#define CONTROLLER_MODE_LQR_ANGLE 4
+#define CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED 5
+//#define CONTROLLER_MODE_ANGLE_RESPONSE_TEST 6
+
+
+// These constants define the method used for estimating the Inertial
+// frame state.
+// All methods are run at all times, this flag indicates which estimate
+// is passed onto the controller.
+// The following is a short description about each mode:
+//
+// ESTIMATOR_METHOD_FINITE_DIFFERENCE
+// Takes the poisition and angles directly as measured,
+// and estimates the velocities as a finite different to the
+// previous measurement
+//
+// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
+// Uses a 2nd order random walk estimator independently for
+// each of (x,y,z,roll,pitch,yaw)
+//
+// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
+// Uses the model of the quad-rotor and the previous inputs
+//
+#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
+#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
+#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
+
+// 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
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+// ******************************************************************************* //
+// VARIABLES SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+// ROS Subscriber for the Vicon data of the remote
+ros::Subscriber viconSubscriber;
+
+// ROS Publisher for the Vicon data of the remote
+ros::Publisher remote_xyz_rpy_publisher;
+
+// ROS Publisher for the Setpoint for the remote controller
+ros::Publisher remote_control_setpoint_publisher;
+
+// Vicon object name of the Remote to follow
+std::string viconObjectName_forRemote ("empty");
+std::string default_viconObjectName_forRemote ("DFALL_REMOTE01");
+
+// Boolean for whether the Remote's state should be published as a message
+bool shouldPublishRemote_xyz_rpy = false;
+
+// Boolean for whether the Remote's state should be display in the terminal window
+bool shouldDisplayRemote_xyz_rpy = false;
+
+// Boolean for whether the Remote control mode is active
+bool isActive_remoteControlMode = false;
+
+// The setpoint from the remote
+float setpointFromRemote_roll = 0.0;
+float setpointFromRemote_pitch = 0.0;
+float setpointFromRemote_yaw = 0.0;
+float setpointFromRemote_z = 0.0;
+
+// The z-height of the remote when "Activate" is pressed
+float z_of_remote_previous_measurement = 0.0;
+float z_when_remote_activated = 0.0;
+
+// Roll and pitch limit (in degrees for angles)
+float remoteControlLimit_roll_degrees = 15.0;
+float remoteControlLimit_pitch_degrees = 15.0;
+float remoteControlLimit_roll = 0.262;
+float remoteControlLimit_pitch = 0.262;
+
+// Factor by which to reduce the remote control input
+float remoteConrtolSetpointFactor_roll = 1.0;
+float remoteConrtolSetpointFactor_pitch = 1.0;
+float remoteConrtolSetpointFactor_yaw = 1.0;
+float remoteConrtolSetpointFactor_z = 1.0;
+
+// LQR Gain matrix for tracking the angle commands from the Crazyflie
+std::vector<float> gainMatrixRollRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixPitchRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixYawRate_forRemoteControl (3,0.0);
+
+// ******************************************************************************* //
+
+
+
+// VARIABLES FOR SOME "ALMOST CONSTANTS"
+// > Mass of the Crazyflie quad-rotor, in [grams]
+float cf_mass;
+// > Coefficients of the 16-bit command to thrust conversion
+std::vector<float> motorPoly(3);
+// The weight of the Crazyflie in [Newtons], i.e., mg
+float gravity_force;
+// One quarter of the "gravity_force"
+float gravity_force_quarter;
+
+
+
+
+// VARIABLES FOR THE CONTROLLER
+
+// Frequency at which the controller is running
+float vicon_frequency;
+
+// Frequency at which the controller is running
+float control_frequency;
+
+
+// > The setpoints for (x,y,z) position and yaw angle, in that order
+float setpoint[4] = {0.0,0.0,0.4,0.0};
+
+
+
+
+// The controller type to run in the "calculateControlOutput" function
+int controller_mode = CONTROLLER_MODE_LQR_RATE;
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
+std::vector<float> gainMatrixRollRate (9,0.0);
+std::vector<float> gainMatrixPitchRate (9,0.0);
+std::vector<float> gainMatrixYawRate (9,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
+std::vector<float> gainMatrixThrust_SixStateVector (6,0.0);
+std::vector<float> gainMatrixRollAngle (6,0.0);
+std::vector<float> gainMatrixPitchAngle (6,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
+std::vector<float> gainMatrixThrust_SixStateVector_50Hz (6,0.0);
+std::vector<float> gainMatrixRollAngle_50Hz (6,0.0);
+std::vector<float> gainMatrixPitchAngle_50Hz (6,0.0);
+
+std::vector<float> gainMatrixRollRate_Nested (3,0.0);
+std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
+std::vector<float> gainMatrixYawRate_Nested (3,0.0);
+
+int lqr_angleRateNested_counter = 4;
+float lqr_angleRateNested_prev_thrustAdjustment = 0.0;
+float lqr_angleRateNested_prev_rollAngle = 0.0;
+float lqr_angleRateNested_prev_pitchAngle = 0.0;
+float lqr_angleRateNested_prev_yawAngle = 0.0;
+
+
+// The 16-bit command limits
+float cmd_sixteenbit_min;
+float cmd_sixteenbit_max;
+
+
+// VARIABLES FOR THE ESTIMATOR
+
+// Frequency at which the controller is running
+float estimator_frequency;
+
+// > A flag for which estimator to use:
+int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+// > The current state interial estimate,
+// for use by the controller
+float current_stateInertialEstimate[12];
+
+// > The measurement of the Crazyflie at the "current" time step,
+// to avoid confusion
+float current_xzy_rpy_measurement[6];
+
+// > The measurement of the Crazyflie at the "previous" time step,
+// used for computing finite difference velocities
+float previous_xzy_rpy_measurement[6];
+
+// > The full 12 state estimate maintained by the finite
+// difference state estimator
+float stateInterialEstimate_viaFiniteDifference[12];
+
+// > The full 12 state estimate maintained by the point mass
+// kalman filter state estimator
+float stateInterialEstimate_viaPointMassKalmanFilter[12];
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
+std::vector<float> PMKF_Kinf_for_positions (2,0.0);
+// > For the (roll,pitch,yaw) angles
+std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
+std::vector<float> PMKF_Kinf_for_angles (2,0.0);
+
+
+
+// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+
+// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
+ros::Publisher debugPublisher;
+
+
+// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
+
+// Boolean whether to execute the convert into body frame function
+bool shouldPerformConvertIntoBodyFrame = false;
+
+
+// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool shouldDisplayDebugInfo = false;
+
+
+// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
+// POSITION
+
+// The ID of this agent, i.e., the ID of this compute
+int my_agentID = 0;
+
+
+
+
+// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
+// The "CrazyflieData" type used for the "request" variable is a
+// structure as defined in the file "CrazyflieData.msg" which has the following
+// properties:
+// string crazyflieName The name given to the Crazyflie in the Vicon software
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+// > The function that is called to "start" all estimation and control computations
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// > The various functions that implement an LQR controller
+void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforActuators( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+
+// ESTIMATOR COMPUTATIONS
+void performEstimatorUpdate_forStateInterial(Controller::Request &request);
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+// PUBLISHING OF THE DEBUG MESSAGE
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
+
+
+// TRANSFORMATION FROM INTERIAL ESTIMATE TO BODY FRAME ERROR
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12]);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// SETPOINT CHANGE CALLBACK
+void setpointCallback(const Setpoint& newSetpoint);
+
+
+
+// LOAD PARAMETERS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
+
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+
+
+
+// ******************************************************************************* //
+// FUNCTIONS SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+void calculateControlOutput_forRemoteControl(float stateInertial[12], Controller::Request &request, Controller::Response &response);
+
+// CALLBACK WITH INFOMATION ABOUT WHICH REMOTE TO SUBSCRIBE TO
+void viconSubscribeObjectNameCallback(const ViconSubscribeObjectName& msg);
+
+// VICON DATA CALLBACK
+void viconCallback(const ViconData& viconData);
+
+// ACTIVATE REMOTE CONTROL MODE MESSAGE CALLBACK
+void shouldActivateCallback(const std_msgs::Int32& msg);
+
+// ******************************************************************************* //
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index 17b545b5f3bc809d6a7498c0540ec9176a214075..ecc4b1befe20e535f957ae86a51b0871fe3ec7f9 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -65,6 +65,15 @@
>
</node>
+ <!-- REMOTE CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "RemoteControllerService"
+ output = "screen"
+ type = "RemoteControllerService"
+ >
+ </node>
+
<!-- PARAMETER SERVICE -->
<node
pkg = "d_fall_pps"
@@ -99,6 +108,11 @@
file = "$(find d_fall_pps)/param/MpcController.yaml"
ns = "MpcController"
/>
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/RemoteController.yaml"
+ ns = "RemoteController"
+ />
</node>
<!-- AGENT GUI (aka. the "student GUI") -->
diff --git a/pps_ws/src/d_fall_pps/msg/ViconSubscribeObjectName.msg b/pps_ws/src/d_fall_pps/msg/ViconSubscribeObjectName.msg
new file mode 100644
index 0000000000000000000000000000000000000000..fbe1b0746c9daf3af001e7870357e6a87f4198bb
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/ViconSubscribeObjectName.msg
@@ -0,0 +1,2 @@
+string objectName
+bool shouldSubscribe
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index f9836bdeb665f257a0109405f3a8fe3d0a669356..73d16618f0566630facb936297788c32a286f929 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,7 +1,8 @@
-#safeController: "SafeControllerService/RateController"
-#demoController: "DemoControllerService/DemoController"
-#studentController: "StudentControllerService/StudentController"
-#mpcController: "MpcControllerService/MpcController"
+safeController: "SafeControllerService/RateController"
+demoController: "DemoControllerService/DemoController"
+studentController: "StudentControllerService/StudentController"
+mpcController: "MpcControllerService/MpcController"
+remoteController: "RemoteControllerService/RemoteController"
strictSafety: false
angleMargin: 0.6
diff --git a/pps_ws/src/d_fall_pps/param/DemoController.yaml b/pps_ws/src/d_fall_pps/param/DemoController.yaml
index 853d908346f75c394b581ce5fb6dcc9e8357e133..9705236c9845010f5bbcf33d9ea3d4f08c55d0a9 100644
--- a/pps_ws/src/d_fall_pps/param/DemoController.yaml
+++ b/pps_ws/src/d_fall_pps/param/DemoController.yaml
@@ -48,7 +48,7 @@ shouldDisplayDebugInfo : false
# - Swaps between pitch set-points to test angle set-point response time
# i.e., this controller test the assumption that "the inner loop is infinitely fast"
#
-controller_mode : 6
+controller_mode : 3
# A flag for which estimator to use, defined as:
diff --git a/pps_ws/src/d_fall_pps/param/RemoteController.yaml b/pps_ws/src/d_fall_pps/param/RemoteController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7653cf734f2a018f877353bdb9acb2a0d5aa1dd4
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/RemoteController.yaml
@@ -0,0 +1,121 @@
+# ***************************************************************************** #
+# PARAMETERS SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+# Vicon object name of the Remote to follow
+default_viconObjectName_forRemote : 'DFALL_REMOTE01'
+
+# Boolean for whether the Remote's state should be published as a message
+shouldPublishRemote_xyz_rpy : True
+
+# Boolean for whether the Remote's state should be display in the terminal window
+shouldDisplayRemote_xyz_rpy : False
+
+# Roll and pitch limit (in degrees for angles)
+remoteControlLimit_roll_degrees : 15
+remoteControlLimit_pitch_degrees : 15
+
+# Factor by which to reduce the remote control input
+remoteConrtolSetpointFactor_roll : 0.5
+remoteConrtolSetpointFactor_pitch : 0.5
+remoteConrtolSetpointFactor_yaw : 1.0
+remoteConrtolSetpointFactor_z : 1.0
+
+# LQR Gain matrix for tracking the angle commands from the Crazyflie
+gainMatrixRollRate_forRemoteControl : [ 5.00, 0.00, 0.00]
+gainMatrixPitchRate_forRemoteControl : [ 0.00, 5.00, 0.00]
+gainMatrixYawRate_forRemoteControl : [ 0.00, 0.00, 2.80]
+
+# ***************************************************************************** #
+
+
+
+# Mass of the crazyflie
+mass : 29
+
+# Frequency of the controller, in hertz
+vicon_frequency : 200
+control_frequency : 200
+
+# Quadratic motor regression equation (a0, a1, a2)
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# Boolean for whether to execute the convert into body frame function
+shouldPerformConvertIntoBodyFrame : true
+
+# Boolean indiciating whether the "Debug Message" of this agent should be published or not
+shouldPublishDebugMessage : false
+
+# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+shouldDisplayDebugInfo : false
+
+
+# A flag for which controller mode to use, defined as:
+# 1 CONTROLLER_MODE_LQR_MOTOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands per motor thrusts
+# 2 CONTROLLER_MODE_LQR_ACTUATOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands actuators of total force and bodz torques
+# 3 CONTROLLER_MODE_LQR_RATE
+# - LQR controller based on the state vector: [position,velocity,angles]
+# 4 CONTROLLER_MODE_LQR_ANGLE
+# - LQR controller based on the state vector: [position,velocity]
+# 5 CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED
+# - LQR Nested angle and rate controller
+# 6 CONTROLLER_MODE_ANGLE_RESPONSE_TEST
+# - Swaps between pitch set-points to test angle set-point response time
+# i.e., this controller test the assumption that "the inner loop is infinitely fast"
+#
+controller_mode : 3
+
+
+# A flag for which estimator to use, defined as:
+# 1 - Finite Different Method,
+# Takes the poisition and angles directly as measured,
+# and estimates the velocities as a finite different to the
+# previous measurement
+# 2 - Point Mass Per Dimension Method
+# Uses a 2nd order random walk estimator independently for
+# each of (x,y,z,roll,pitch,yaw)
+# 3 - Quad-rotor Model Based Method
+# Uses the model of the quad-rotor and the previous inputs
+estimator_method : 1
+
+
+# The LQR Controller parameters for "mode = 3"
+gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
+gainMatrixRollRate : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00]
+gainMatrixPitchRate : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00]
+gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
+
+# The LQR Controller parameters for "mode = 4"
+gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25]
+gainMatrixRollAngle : [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
+gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
+
+# The LQR Controller parameters for "mode = 5"
+gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
+gainMatrixRollAngle_50Hz : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
+gainMatrixPitchAngle_50Hz : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
+
+gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
+gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
+gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
+
+
+# The max and minimum thrust for a 16-bit command
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 60000
+
+
+# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+# > For the (x,y,z) position
+PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
+PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
+PMKF_Kinf_for_positions : [ 0.3277,12.9648]
+
+
+# > For the (roll,pitch,yaw) angles
+PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
+PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
+PMKF_Kinf_for_angles : [ 0.3046,11.0342]
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
index abccda99215e7f121533f18c9bc925eb1b42c77c..e14630c8370ff649fa194c33f2ec846ae9b39684 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -206,7 +206,8 @@ void goToControllerSetpoint()
//is called when new data from Vicon arrives
-void viconCallback(const ViconData& viconData) {
+void viconCallback(const ViconData& viconData)
+{
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData global = *it;
@@ -297,6 +298,9 @@ void viconCallback(const ViconData& viconData) {
case MPC_CONTROLLER:
success = mpcController.call(controllerCall);
break;
+ case REMOTE_CONTROLLER:
+ success = remoteController.call(controllerCall);
+ break;
default:
ROS_ERROR("[PPS CLIENT] the current controller was not recognised");
break;
@@ -448,6 +452,11 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
setControllerUsed(MPC_CONTROLLER);
break;
+ case CMD_USE_REMOTE_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_REMOTE_CONTROLLER Command received");
+ setControllerUsed(REMOTE_CONTROLLER);
+ break;
+
case CMD_CRAZYFLY_TAKE_OFF:
if(flying_state == STATE_MOTORS_OFF)
{
@@ -808,6 +817,21 @@ void loadMpcController()
ROS_INFO_STREAM("[PPS CLIENT] Loaded mpc controller " << mpcController.getService());
}
+void loadRemoteController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string remoteControllerName;
+ if(!nodeHandle.getParam("remoteController", remoteControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get remote controller name");
+ return;
+ }
+
+ remoteController = ros::service::createClient<Controller>(remoteControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded remote controller " << remoteController.getService());
+}
+
void sendMessageUsingController(int controller)
{
// send a message in topic for the studentGUI to read it
@@ -834,6 +858,9 @@ void setInstantController(int controller) //for right now, temporal use
case MPC_CONTROLLER:
loadMpcController();
break;
+ case REMOTE_CONTROLLER:
+ loadRemoteController();
+ break;
default:
break;
}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
index 1e5ba7c3cb0524ffb552a8bbce87c3aac32c3169..f3faa81ade9e6632ab46fa4fca10ab11f18a0efa 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -139,6 +139,17 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// Re-load the parameters of the demo controller:
cmd = "rosparam load " + d_fall_pps_path + "/param/MpcController.yaml " + m_base_namespace + "/MpcController";
}
+ // ----------------------------------------
+ // FOR THE REMOTE CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
+ {
+ // Re-load the parameters of the demo controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/RemoteController.yaml " + m_base_namespace + "/RemoteController";
+ }
else
{
// Let the user know that the command was not recognised
diff --git a/pps_ws/src/d_fall_pps/src/nodes/RemoteControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/RemoteControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7fa4a2d19872f21da4187ffe81fdb3758f30864
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/nodes/RemoteControllerService.cpp
@@ -0,0 +1,1658 @@
+// 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
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/RemoteControllerService.h"
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// OOO U U TTTTT EEEEE RRRR
+// O O U U T E R R
+// O O U U T EEE RRRR
+// O O U U T E R R
+// OOO UUU T EEEEE R R
+//
+// CCCC OOO N N TTTTT RRRR OOO L L OOO OOO PPPP
+// C O O NN N T R R O O L L O O O O P P
+// C O O N N N T RRRR O O L L O O O O PPPP
+// C O O N NN T R R O O L L O O O O P
+// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
+// ----------------------------------------------------------------------------------
+
+// This function is the callback that is linked to the "CustomController" service that
+// is advertised in the main function. This must have arguments that match the
+// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
+// folder)
+//
+// The arument "request" is a structure provided to this service with the following two
+// properties:
+// request.ownCrazyflie
+// This property is itself a structure of type "CrazyflieData", which is defined in the
+// file "CrazyflieData.msg", and has the following properties
+// string crazyflieName
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// The values in these properties are directly the measurement taken by the Vicon
+// motion capture system of the Crazyflie that is to be controlled by this service
+//
+// request.otherCrazyflies
+// This property is an array of "CrazyflieData" structures, what allows access to the
+// Vicon measurements of other Crazyflies.
+//
+// The argument "response" is a structure that is expected to be filled in by this
+// service by this function, it has only the following property
+// response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is defined in
+// the file "ControlCommand.msg", and has the following properties:
+// float32 roll The command sent to the Crazyflie for the body frame x-axis
+// float32 pitch The command sent to the Crazyflie for the body frame y-axis
+// float32 yaw The command sent to the Crazyflie for the body frame z-axis
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
+// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
+// > The allowed values for "onboardControllerType" are in the "Defines" section at the
+// top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
+// > In the PPS exercise we will only use the RATE_MODE.
+// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested from the
+// PID controllers running in the Crazyflie 2.0 firmware.
+// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
+// are the baseline motor commands requested from the Crazyflie, with the adjustment
+// for body rates being added on top of this in the firmware (i.e., as per the code
+// of the "distribute_power" function provided in exercise sheet 2).
+// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
+// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
+// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
+// thrust). To assist, teh following is an ASCII art of this convention:
+//
+// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
+//
+// > This is a top view,
+// > M1 to M4 stand for Motor 1 to Motor 4,
+// > "CW" indicates that the motor rotates Clockwise,
+// > "CCW" indicates that the motor rotates Counter-Clockwise,
+// > By right-hand axis convention, the positive z-direction points our of the screen,
+// > This being a "top view" means tha the direction of positive thrust produced
+// by the propellers is also out of the screen.
+//
+// ____ ____
+// / \ / \
+// (CW) | M4 | x | M1 | (CCW)
+// \____/\ ^ /\____/
+// \ \ | / /
+// \ \ | / /
+// \ \______ | ______/ /
+// \ | /
+// | | |
+// y <-------------o |
+// | |
+// / _______________ \
+// / / \ \
+// / / \ \
+// ____/ / \ \____
+// / \/ \/ \
+// (CCW) | M3 | | M2 | (CW)
+// \____/ \____/
+//
+//
+//
+// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+
+
+
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+
+ // THIS IS THE START OF THE "OUTER" CONTROL LOOP
+ // > i.e., this is the control loop run on this laptop
+ // > this function is called at the frequency specified
+ // > this function performs all estimation and control
+
+
+ // PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
+ // > After this function is complete the class variable
+ // "current_stateInertialEstimate" is updated and ready
+ // to be used for subsequent controller copmutations
+ performEstimatorUpdate_forStateInterial(request);
+
+
+
+
+ if (isActive_remoteControlMode)
+ {
+ // CARRY OUT THE CONTROLLER COMPUTATIONS
+ calculateControlOutput_forRemoteControl(current_stateInertialEstimate,request,response);
+
+ }
+ else
+ {
+ // CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
+ // THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
+ // > Define a local array to fill in with the body frame error
+ float current_bodyFrameError[12];
+ // > Call the function to perform the conversion
+ convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+
+
+
+ // CARRY OUT THE CONTROLLER COMPUTATIONS
+ calculateControlOutput_viaLQR(current_bodyFrameError,request,response);
+ }
+
+
+ // PUBLISH THE DEBUG MESSAGE (if required)
+ if (shouldPublishDebugMessage)
+ {
+ construct_and_publish_debug_message(request,response);
+ }
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE M M OOO TTTTT EEEEE
+// R R E MM MM O O T E
+// RRRR EEE M M M O O T EEE
+// R R E M M O O T E
+// R R EEEEE M M OOO T EEEEE
+//
+// CCCC OOO N N TTTTT RRRR OOO L
+// C O O NN N T R R O O L
+// C O O N N N T RRRR O O L
+// C O O N NN T R R O O L
+// CCCC OOO N N T R R OOO LLLLL
+// ----------------------------------------------------------------------------------
+
+
+void calculateControlOutput_forRemoteControl(float stateInertial[12], Controller::Request &request, Controller::Response &response)
+{
+
+ // PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // > total thrust adjustment
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+ float thrustAdjustment_total = 0;
+
+ // Create the angle error to use for the inner controller
+ float temp_stateAngleError[3] = {
+ stateInertial[6] - setpointFromRemote_roll,
+ stateInertial[7] - setpointFromRemote_pitch,
+ stateInertial[8] - setpointFromRemote_yaw
+ };
+
+ // Create the z-error
+ float temp_stateZError = stateInertial[2] - setpointFromRemote_z;
+
+ // Perform the "-Kx" LQR computation for the rates:
+ for(int i = 0; i < 3; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate_forRemoteControl[i] * temp_stateAngleError[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate_forRemoteControl[i] * temp_stateAngleError[i];
+ // BODY FRAME Z CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate_forRemoteControl[i] * temp_stateAngleError[i];
+ }
+
+ // Perform the "-Kx" LQR computation for the thrust
+ thrustAdjustment_total -= temp_stateZError * gainMatrixThrust_SixStateVector[2];
+ thrustAdjustment_total -= stateInertial[5] * gainMatrixThrust_SixStateVector[5];
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ float thrustAdjustment = thrustAdjustment_total / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ }
+
+}
+
+
+
+
+
+void viconSubscribeObjectNameCallback(const ViconSubscribeObjectName& msg)
+{
+ if (msg.shouldSubscribe)
+ {
+ // Get the object name into the class variable
+ viconObjectName_forRemote = msg.objectName;
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Keeps 10 messages because otherwise ViconDataPublisher would override the data immediately
+ viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 10, viconCallback);
+ ROS_INFO_STREAM("[REMOTE CONTORLLER] successfully subscribed to ViconData");
+ }
+ else
+ {
+ // Unsubscribe from the Vicon data
+ viconSubscriber.shutdown();
+ }
+}
+
+
+
+
+
+//is called when new data from Vicon arrives
+void viconCallback(const ViconData& viconData)
+{
+ for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
+ {
+ CrazyflieData objectData_in_globalFrame = *it;
+
+ if(objectData_in_globalFrame.crazyflieName == viconObjectName_forRemote)
+ {
+
+ if (shouldPublishRemote_xyz_rpy)
+ {
+ // Publish the "CrayzflieData" for this object
+ remote_xyz_rpy_publisher.publish(objectData_in_globalFrame);
+ }
+
+ if (shouldDisplayRemote_xyz_rpy)
+ {
+ // Dislpaz the "CrayzflieData" for this object
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] Remote [z,r,p,y] = [ " << objectData_in_globalFrame.z << " , " << objectData_in_globalFrame.roll << " , " << objectData_in_globalFrame.pitch << " , " << objectData_in_globalFrame.yaw << " ]" );
+ }
+
+ // Update the remote set-point is not occluded
+ if(!objectData_in_globalFrame.occluded)
+ {
+
+ // Update the z height variable that is used when activating
+ z_of_remote_previous_measurement = objectData_in_globalFrame.z;
+
+ if (isActive_remoteControlMode)
+ {
+ // Update the setpoint used for the "Remote Controller"
+ setpointFromRemote_roll = objectData_in_globalFrame.roll * remoteConrtolSetpointFactor_roll;
+ setpointFromRemote_pitch = objectData_in_globalFrame.pitch * remoteConrtolSetpointFactor_pitch;
+ setpointFromRemote_yaw = objectData_in_globalFrame.yaw * remoteConrtolSetpointFactor_yaw;
+ setpointFromRemote_z = (objectData_in_globalFrame.z - z_when_remote_activated) * remoteConrtolSetpointFactor_z + setpoint[2];
+
+ // Clip the roll angle to its limit
+ if (setpointFromRemote_roll>remoteControlLimit_roll)
+ {
+ setpointFromRemote_roll = remoteControlLimit_roll;
+ }
+ else if (setpointFromRemote_roll<(-remoteControlLimit_roll))
+ {
+ setpointFromRemote_roll = -remoteControlLimit_roll;
+ }
+ // Clip the pitch angle to its limit
+ if (setpointFromRemote_pitch>remoteControlLimit_pitch)
+ {
+ setpointFromRemote_pitch = remoteControlLimit_pitch;
+ }
+ else if (setpointFromRemote_pitch<(-remoteControlLimit_pitch))
+ {
+ setpointFromRemote_pitch = -remoteControlLimit_pitch;
+ }
+
+ // Publish the updated setpoint
+ CrazyflieData setpointToPublish;
+ setpointToPublish.roll = setpointFromRemote_roll;
+ setpointToPublish.pitch = setpointFromRemote_pitch;
+ setpointToPublish.yaw = setpointFromRemote_yaw;
+ setpointToPublish.z = setpointFromRemote_z;
+
+ remote_control_setpoint_publisher.publish(setpointToPublish);
+ }
+ else
+ {
+ // Update the yaw setpoint for the "de-activated" controller
+ // > this ensures a smooth transition from "de-activated" to "activated"
+ setpoint[3] = objectData_in_globalFrame.yaw * remoteConrtolSetpointFactor_yaw;
+ }
+
+ }
+ }
+ }
+}
+
+
+void shouldActivateCallback(const std_msgs::Int32& msg)
+{
+ if (msg.data)
+ {
+ ROS_INFO("[REMOTE CONTROLLER] Received message to ACTIVATE remote control mode.");
+ isActive_remoteControlMode = true;
+
+ z_when_remote_activated = z_of_remote_previous_measurement;
+ }
+ else
+ {
+ ROS_INFO("[REMOTE CONTROLLER] Received message to DE-ACTIVATE remote control mode.");
+ isActive_remoteControlMode = false;
+
+ setpointFromRemote_roll = 0.0;
+ setpointFromRemote_pitch = 0.0;
+ setpointFromRemote_yaw = 0.0;
+ setpointFromRemote_z = 0.0;
+
+ z_when_remote_activated = 0.0;
+ }
+}
+
+
+
+
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
+// E S T I MM MM A A T I O O NN N
+// EEE SSS T I M M M A A T I O O N N N
+// E S T I M M AAAAA T I O O N NN
+// EEEEE SSSS T III M M A A T III OOO N N
+// ------------------------------------------------------------------------------
+void performEstimatorUpdate_forStateInterial(Controller::Request &request)
+{
+
+ // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+ // > for (x,y,z) position
+ current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+
+
+ // RUN THE FINITE DIFFERENCE ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+
+
+ // RUN THE POINT MASS KALMAN FILTER ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ // Estimator based on Point Mass Kalman Filter
+ case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ }
+
+
+ // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+ // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
+ // > for (x,y,z) position
+ previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
+ previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
+ previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
+ previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
+ previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ stateInterialEstimate_viaFiniteDifference[0] = current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaFiniteDifference[1] = current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaFiniteDifference[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ stateInterialEstimate_viaFiniteDifference[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ stateInterialEstimate_viaFiniteDifference[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+{
+ // PERFORM THE KALMAN FILTER UPDATE STEP
+ // > First take a copy of the estimator state
+ float temp_PMKFstate[12];
+ for(int i = 0; i < 12; ++i)
+ {
+ temp_PMKFstate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaPointMassKalmanFilter[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
+ stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
+
+ // > roll position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaPointMassKalmanFilter[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
+ // > pitch position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaPointMassKalmanFilter[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
+ // > yaw position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
+ stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L QQQ RRRR
+// L Q Q R R
+// L Q Q RRRR
+// L Q Q R R
+// LLLLL QQ Q R R
+// ----------------------------------------------------------------------------------
+
+void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // SWITCH BETWEEN THE DIFFERENT LQR CONTROLLER MODES:
+ switch (controller_mode)
+ {
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_RATE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity]
+ case CONTROLLER_MODE_LQR_ANGLE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAngles(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAnglesRatesNested(stateErrorBody,request,response);
+ break;
+ }
+
+ default:
+ {
+ // Display that the "controller_mode" was not recognised
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'controller_mode' is not recognised.");
+ // Set everything in the response to zero
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
+ response.controlOutput.motorCmd1 = 0;
+ response.controlOutput.motorCmd2 = 0;
+ response.controlOutput.motorCmd3 = 0;
+ response.controlOutput.motorCmd4 = 0;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ break;
+ }
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 9; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ // BODY FRAME YAW CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+ if (shouldDisplayDebugInfo)
+ {
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
+
+ // An example of "printing out" the control actions computed.
+ ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
+ ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
+ ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
+ ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+
+ // An example of "printing out" the per motor 16-bit command computed.
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollAngle_forResponse = 0;
+ float pitchAngle_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
+ }
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollAngle_forResponse;
+ response.controlOutput.pitch = pitchAngle_forResponse;
+ response.controlOutput.yaw = setpoint[3];
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+}
+
+
+
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE NESTED "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Increment the counter variable
+ lqr_angleRateNested_counter++;
+
+ if (lqr_angleRateNested_counter > 4)
+ {
+ //ROS_INFO("Outer called");
+
+ // Reset the counter to 1
+ lqr_angleRateNested_counter = 1;
+
+ // PERFORM THE OUTER "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Reset the class variable to zero for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > body frame yaw angle,
+ // > total thrust adjustment.
+ // These will be requested from the "inner-loop" LQR controller below
+ lqr_angleRateNested_prev_rollAngle = 0;
+ lqr_angleRateNested_prev_pitchAngle = 0;
+ lqr_angleRateNested_prev_thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
+ }
+
+ // BODY FRAME Z CONTROLLER
+ //lqr_angleRateNested_prev_yawAngle = setpoint[3];
+ lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
+
+
+ }
+
+ //ROS_INFO("Inner called");
+
+ // PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+
+ // Create the angle error to use for the inner controller
+ float temp_stateAngleError[3] = {
+ stateErrorBody[6] - lqr_angleRateNested_prev_rollAngle,
+ stateErrorBody[7] - lqr_angleRateNested_prev_pitchAngle,
+ lqr_angleRateNested_prev_yawAngle
+ };
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 4; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME Z CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response)
+{
+ // ***********************************************************
+ // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
+ // D D E B B U U G MM MM S G
+ // D D EEE BBBB U U G M M M SSS G
+ // D D E B B U U G G M M S G G
+ // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
+
+ // DEBUGGING CODE:
+ // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
+ // By fill this message with data and publishing it you can display the data in
+ // real time using rpt plots. Instructions for using rqt plots can be found on
+ // the wiki of the D-FaLL-System repository
+
+ // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
+ // (located in the "msg" folder) to see the full structure of this message.
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ debugMsg.vicon_x = request.ownCrazyflie.x;
+ debugMsg.vicon_y = request.ownCrazyflie.y;
+ debugMsg.vicon_z = request.ownCrazyflie.z;
+ debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // Fill in the debugging message with any other data you would like to display
+ // in real time. For example, it might be useful to display the thrust
+ // adjustment computed by the z-altitude controller.
+ // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
+ // type "float64" that you can fill in with data you would like to plot in
+ // real-time.
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ // Publish the "debugMsg"
+ debugPublisher.publish(debugMsg);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
+// R R O O T A A T E I NN N T O O
+// RRRR O O T A A T EEE I N N N T O O
+// R R O O T AAAAA T E I N NN T O O
+// R R OOO T A A T EEEEE III N N T OOO
+//
+// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
+// B B O O D D Y Y F R R A A MM MM E
+// BBBB O O D D Y FFF RRRR A A M M M EEE
+// B B O O D D Y F R R AAAAA M M E
+// BBBB OOO DDDD Y F R R A A M M EEEEE
+// ------------------------------------------------------------------------------
+
+// The arguments for this function are as follows:
+// stateInertial
+// This is an array of length 9 with the estimates the error of of the following values
+// relative to the sepcifed setpoint:
+// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
+// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
+// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
+//
+// stateBody
+// This is an empty array of length 9, this function should fill in all elements of this
+// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
+// position and (x,y) velocities are rotated into the body frame.
+//
+// yaw_measured
+// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
+// the Vicon motion capture system
+//
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured)
+{
+ if (shouldPerformConvertIntoBodyFrame)
+ {
+ float sinYaw = sin(yaw_measured);
+ float cosYaw = cos(yaw_measured);
+
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0] * cosYaw + stateInertial[1] * sinYaw;
+ stateBody[1] = stateInertial[1] * cosYaw - stateInertial[0] * sinYaw;
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3] * cosYaw + stateInertial[4] * sinYaw;
+ stateBody[4] = stateInertial[4] * cosYaw - stateInertial[3] * sinYaw;
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+ else
+ {
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0];
+ stateBody[1] = stateInertial[1];
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3];
+ stateBody[4] = stateInertial[4];
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+}
+
+
+
+
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12])
+{
+ // Store the current YAW in a local variable
+ float temp_stateInertial_yaw = stateInertial[8];
+
+ // Adjust the INERTIAL (x,y,z) position for the setpoint
+ stateInertial[0] = stateInertial[0] - setpoint[0];
+ stateInertial[1] = stateInertial[1] - setpoint[1];
+ stateInertial[2] = stateInertial[2] - setpoint[2];
+
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > First, get the yaw error into a local variable
+ float yawError = stateInertial[8] - setpoint[3];
+ // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // > Third, put the "yawError" into the "stateError" variable
+ stateInertial[8] = yawError;
+
+
+ if (yawError>(PI/6))
+ {
+ yawError = (PI/6);
+ }
+ else if (yawError<(-PI/6))
+ {
+ yawError = (-PI/6);
+ }
+
+ // CONVERSION INTO BODY FRAME
+ // Conver the state erorr from the Inertial frame into the Body frame
+ // > Note: the function "convertIntoBodyFrame" is implemented in this file
+ // and by default does not perform any conversion. The equations to convert
+ // the state error into the body frame should be implemented in that function
+ // for successful completion of the PPS exercise
+ convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-bit command signal that generates the "thrust" force
+ float cmd = (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd < cmd_sixteenbit_min)
+ {
+ cmd = 0;
+ }
+ else if (cmd > cmd_sixteenbit_max)
+ {
+ cmd = cmd_sixteenbit_max;
+ }
+
+ return cmd;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// NN N E W W S E T P P O O I NN N T
+// N N N EEE W W SSS EEE T PPPP O O I N N N T
+// N NN E W W W S E T P O O I N NN T
+// N N EEEEE W W SSSS EEEEE T P OOO III N N T
+//
+// GGG U U III CCCC A L L BBBB A CCCC K K
+// G G U U I C A A L L B B A A C K K
+// G U U I C A A L L BBBB A A C KKK
+// G G U U I C AAAAA L L B B AAAAA C K K
+// GGGG UUU III CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void setpointCallback(const Setpoint& newSetpoint)
+{
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+{
+ // Extract from the "msg" for which controller the and from where to fetch the YAML
+ // parameters
+ int controller_to_fetch_yaml = msg.data;
+
+ // Switch between fetching for the different controllers and from different locations
+ switch(controller_to_fetch_yaml)
+ {
+
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[REMOTE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[REMOTE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not relevant
+ //ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters fetched from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the CustomController.yaml file
+
+ // Add the "CustomController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_RemoteController(nodeHandle, "RemoteController");
+
+
+ // ******************************************************************************* //
+ // PARAMETERS SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+ // Vicon object name of the Remote to follow
+ default_viconObjectName_forRemote = getParameterString(nodeHandle_for_RemoteController, "default_viconObjectName_forRemote");
+
+ // Boolean for whether the Remote's state should be published as a message
+ shouldPublishRemote_xyz_rpy = getParameterBool(nodeHandle_for_RemoteController, "shouldPublishRemote_xyz_rpy");
+
+ // Boolean for whether the Remote's state should be display in the terminal window
+ shouldDisplayRemote_xyz_rpy = getParameterBool(nodeHandle_for_RemoteController, "shouldDisplayRemote_xyz_rpy");
+
+ // Roll and pitch limit (in degrees for angles)
+ remoteControlLimit_roll_degrees = getParameterFloat(nodeHandle_for_RemoteController , "remoteControlLimit_roll_degrees");
+ remoteControlLimit_pitch_degrees = getParameterFloat(nodeHandle_for_RemoteController , "remoteControlLimit_pitch_degrees");
+
+ // Factor by which to reduce the remote control input
+ remoteConrtolSetpointFactor_roll = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_roll");
+ remoteConrtolSetpointFactor_pitch = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_pitch");
+ remoteConrtolSetpointFactor_yaw = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_yaw");
+ remoteConrtolSetpointFactor_z = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_z");
+
+ // LQR Gain matrix for tracking the angle commands from the Crazyflie
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate_forRemoteControl", gainMatrixRollRate_forRemoteControl, 3);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate_forRemoteControl", gainMatrixPitchRate_forRemoteControl, 3);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate_forRemoteControl", gainMatrixYawRate_forRemoteControl, 3);
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/default_viconObjectName_forRemote = " << default_viconObjectName_forRemote);
+
+ // ******************************************************************************* //
+
+
+
+ // > The mass of the crazyflie
+ cf_mass = getParameterFloat(nodeHandle_for_RemoteController , "mass");
+
+ // Display one of the YAML parameters to debug if it is working correctly
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ vicon_frequency = getParameterFloat(nodeHandle_for_RemoteController, "vicon_frequency");
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ control_frequency = getParameterFloat(nodeHandle_for_RemoteController, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_RemoteController, "motorPoly", motorPoly, 3);
+
+ // > The boolean for whether to execute the convert into body frame function
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_RemoteController, "shouldPerformConvertIntoBodyFrame");
+
+ // Boolean indiciating whether the "Debug Message" of this agent should be published or not
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_RemoteController, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_RemoteController, "shouldDisplayDebugInfo");
+
+
+ // THE CONTROLLER GAINS:
+
+ // A flag for which controller to use:
+ controller_mode = getParameterInt( nodeHandle_for_RemoteController , "controller_mode" );
+
+ // A flag for which estimator to use:
+ estimator_method = getParameterInt( nodeHandle_for_RemoteController , "estimator_method" );
+
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE"
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+
+
+ // 16-BIT COMMAND LIMITS
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_RemoteController, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_RemoteController, "command_sixteenbit_max");
+
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ // > For the (roll,pitch,yaw) angles
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ //ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/mass = " << cf_mass);
+
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters loaded from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void processFetchedParameters()
+{
+ // Compute the feed-forward force that we need to counteract gravity (i.e., mg)
+ // > in units of [Newtons]
+ gravity_force = cf_mass * 9.81/(1000);
+ gravity_force_quarter = cf_mass * 9.81/(1000*4);
+
+ // Set that the estimator frequency is the same as the control frequency
+ estimator_frequency = vicon_frequency;
+
+
+ // Roll and pitch limit (in degrees for angles)
+ remoteControlLimit_roll = remoteControlLimit_roll_degrees * PI / 180.0;
+ remoteControlLimit_pitch = remoteControlLimit_pitch_degrees * PI / 180.0;
+
+
+ // Use the Remote name if the variable is currently empty
+ if (viconObjectName_forRemote == "empty")
+ {
+ viconObjectName_forRemote = default_viconObjectName_forRemote;
+ }
+
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: after processing the viconObjectName_forRemote = " << viconObjectName_forRemote);
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
+// G E T P P A A R R A A MM MM ( )
+// G EEE T PPPP A A RRRR A A M M M ( )
+// G G E T P AAAAA R R AAAAA M M ( )
+// GGGG EEEEE T P A A R R A A M M ( )
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
+{
+ std::string val;
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "RemoteControllerService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this "StudentControllerService" node
+ // > This should be something like: "/dfall/agentXXX"
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+ // Get the agent ID as the "ROS_NAMESPACE" this computer.
+ // NOTES:
+ // > If you look at the "Student.launch" file in the "launch" folder, you will see
+ // the following line of code:
+ // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "studentID" to the "PPSClient"
+ // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
+ // to the "PPSClient" node within which this controller service is nested.
+ // Get the handle to the "PPSClient" node
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+ // Get the value of the "studentID" parameter into the instance variable "my_agentID"
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
+ {
+ // Throw an error if the student ID parameter could not be obtained
+ ROS_ERROR("[REMOTE CONTROLLER] Failed to get agentID from PPSClient");
+ }
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+
+ // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
+ // namespace string for the parameter service that is running on the machine of this
+ // agent
+ namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
+
+ // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
+ // for the parameter service that is running on the coordinate machine
+ // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
+ // is very important because it specifies that the name is global
+ namespace_to_coordinator_parameter_service = "/ParameterService";
+
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
+ //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+
+
+ // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // *********************************************************************************
+
+
+
+
+ //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
+ //ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 10, viconCallback);
+ //ROS_INFO_STREAM("[REMOTE CONTORLLER] successfully subscribed to ViconData");
+
+ // Subscribe to the message that triggers Vicon subscribe/unsubscribe actions
+ ros::Subscriber viconSubscribeObjectNameSubscriber = nodeHandle.subscribe("ViconSubscribeObjectName", 1, viconSubscribeObjectNameCallback);
+
+ // Advertise the message topic of the Remote (y,roll,pitch,yaw)
+ remote_xyz_rpy_publisher = nodeHandle.advertise<CrazyflieData>("RemoteData", 1);
+
+ // Subscribe to the message that triggers activates/deactivates remote control mode
+ ros::Subscriber shouldActivateSubscriber = nodeHandle.subscribe("Activate", 1, shouldActivateCallback);
+
+ // Advertise the message topic of the Remote (y,roll,pitch,yaw)
+ remote_control_setpoint_publisher = nodeHandle.advertise<CrazyflieData>("RemoteControlSetpoint", 1);
+
+
+
+
+ // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
+ // advertises under the name "DebugTopic" and is a message with the structure
+ // defined in the file "DebugMsg.msg" (located in the "msg" folder).
+ debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "Setpoint" topic and calls the class function
+ // "setpointCallback" each time a messaged is received on this topic and the message
+ // is passed as an input argument to the "setpointCallback" class function.
+ //ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "CustomController". This service has
+ // the input-output behaviour defined in the "Controller.srv" file (located in the
+ // "srv" folder). This service, when called, is provided with the most recent
+ // measurement of the Crazyflie and is expected to respond with the control action
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("RemoteController", calculateControlOutput);
+
+ // Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
+ // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
+ // "using namespace d_fall_pps;"
+ // in the "DEFINES" section at the top of this file.
+ ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+
+ // Print out some information to the user.
+ ROS_INFO("[REMOTE CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}