From ca3b48852ac9f143fb7d1fcc822b0021f65ce100 Mon Sep 17 00:00:00 2001
From: beuchatp <beuchatp@control.ee.ethz.ch>
Date: Tue, 18 Dec 2018 14:15:30 +0100
Subject: [PATCH] Setpoint changes now generalised through the GUI for the
Default controller. Needs more extensive testing. Next step is to get that
current controller state when the corrdinator is coordinating only one agent
---
pps_ws/src/d_fall_pps/CMakeLists.txt | 5 +
.../include/Constants_for_Qt_compile.h | 46 +-
.../flyingAgentGUI/include/controllertabs.h | 1 +
.../include/defaultcontrollertab.h | 11 +-
.../flyingAgentGUI/src/controllertabs.cpp | 30 +-
.../src/defaultcontrollertab.cpp | 204 +++-
.../src/enablecontrollerloadyamlbar.cpp | 13 +-
.../GUI_Qt/flyingAgentGUI/src/mainwindow.cpp | 14 +-
.../src/d_fall_pps/include/nodes/Constants.h | 32 +-
.../include/nodes/DefaultControllerService.h | 237 +++++
.../include/nodes/ParameterService.h | 9 +-
.../include/nodes/StudentControllerService.h | 5 +-
pps_ws/src/d_fall_pps/launch/Agent.launch | 9 +
.../src/d_fall_pps/msg/SetpointWithHeader.msg | 2 +-
.../d_fall_pps/param/DefaultController.yaml | 16 +
.../src/nodes/DefaultControllerService.cpp | 962 ++++++++++++++++++
.../d_fall_pps/src/nodes/ParameterService.cpp | 173 +---
.../src/nodes/StudentControllerService.cpp | 52 +-
.../d_fall_pps/srv/LoadYamlFromFilename.srv | 3 +
19 files changed, 1595 insertions(+), 229 deletions(-)
create mode 100644 pps_ws/src/d_fall_pps/include/nodes/DefaultControllerService.h
create mode 100644 pps_ws/src/d_fall_pps/param/DefaultController.yaml
create mode 100644 pps_ws/src/d_fall_pps/src/nodes/DefaultControllerService.cpp
create mode 100644 pps_ws/src/d_fall_pps/srv/LoadYamlFromFilename.srv
diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 09af57d2..81579ef9 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -230,6 +230,7 @@ add_service_files(
CMQuery.srv
CMUpdate.srv
CMCommand.srv
+ LoadYamlFromFilename.srv
)
## Generate actions in the 'action' folder
@@ -326,6 +327,8 @@ add_executable(PPSClient src/nodes/PPSClient.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp
src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(DefaultControllerService src/nodes/DefaultControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
add_executable(DemoControllerService src/nodes/DemoControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
@@ -412,6 +415,7 @@ endif()
add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(BatteryMonitor d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(DefaultControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DemoControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(StudentControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -463,6 +467,7 @@ endif()
target_link_libraries(PPSClient ${catkin_LIBRARIES})
target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
+target_link_libraries(DefaultControllerService ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
index 93f79577..d96707d4 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
@@ -49,6 +49,38 @@
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// The types, i.e., agent or coordinator
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 2
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
// Types PPS firmware
#define CF_COMMAND_TYPE_MOTORS 6
#define CF_COMMAND_TYPE_RATE 7
@@ -145,12 +177,16 @@
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
-// The types, i.e., agent or coordinator
-#define TYPE_INVALID -1
-#define TYPE_COORDINATOR 1
-#define TYPE_AGENT 2
-
+// For standard buttons in the GUI
+#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/controllertabs.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/controllertabs.h
index 7aad04c8..a4d11624 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/controllertabs.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/controllertabs.h
@@ -52,6 +52,7 @@ public slots:
signals:
+ void agentIDsToCoordinateChanged(QVector<int> agentIDs , bool shouldCoordinateAll);
void measuredPoseValueChanged(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
void poseDataUnavailableSignal();
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
index c8a3a4a9..68edb4ea 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
@@ -46,6 +46,7 @@ public:
public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
void poseDataUnavailableSlot();
@@ -90,12 +91,13 @@ private:
#ifdef CATKIN_MAKE
- // Variable for storing the default setpoint
- d_fall_pps::SetpointWithHeader m_defaultSetpoint;
-
// PUBLISHER
// > For requesting the setpoint to be changed
ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
#endif
@@ -104,6 +106,9 @@ private:
#ifdef CATKIN_MAKE
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const d_fall_pps::SetpointWithHeader& newSetpoint);
+
// Fill the header for a message
void fillSetpointMessageHeader( d_fall_pps::SetpointWithHeader & msg );
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
index aafb53da..d7140128 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
@@ -34,6 +34,17 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->default_controller_tab_widget , &DefaultControllerTab::poseDataUnavailableSlot
);
+
+ // CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
+ // WITH THE LIST OF AGENT IDs TO COORDINATE
+ // This is passed from this "controller tabs widget" to
+ // each of the controller tabs. The signal is simply
+ // "passed through"
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->default_controller_tab_widget , &DefaultControllerTab::setAgentIDsToCoordinate
+ );
+
@@ -148,23 +159,8 @@ void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& vicon
void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
{
-
- // Lock the mutex
- m_agentIDs_toCoordinate_mutex.lock();
- // Add the "coordinate all" flag
- m_shouldCoordinateAll = shouldCoordinateAll;
- // Clear the previous list of agent IDs
- m_vector_of_agentIDs_toCoordinate.clear();
- // Copy across the agent IDs, if necessary
- if (!shouldCoordinateAll)
- {
- for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
- {
- m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
- }
- }
- // Unlock the mutex
- m_agentIDs_toCoordinate_mutex.unlock();
+ // Simply pass on the signal to the tabs
+ emit agentIDsToCoordinateChanged( agentIDs , shouldCoordinateAll );
}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
index 56aaf802..147a43ba 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
@@ -36,15 +36,16 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
// CREATE A NODE HANDLE TO THIS GUI
ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
- // CREATE THE COMMAND PUBLISHER
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<d_fall_pps::SetpointWithHeader>("DefaultControllerService/RequestSetpointChange", 1);
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("DefaultControllerService/SetpointChanged", 1, &DefaultControllerTab::setpointChangedCallback, this);
+ }
- // Set the default setpoint
- m_defaultSetpoint.x = 0.0f;
- m_defaultSetpoint.y = 0.0f;
- m_defaultSetpoint.z = 0.5f;
- m_defaultSetpoint.yaw = 0.0f;
#endif
}
@@ -130,6 +131,83 @@ void DefaultControllerTab::poseDataUnavailableSlot()
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void DefaultControllerTab::setpointChangedCallback(const d_fall_pps::SetpointWithHeader& newSetpoint)
+{
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // EXTRACT THE SETPOINT
+ float x = newSetpoint.x;
+ float y = newSetpoint.y;
+ float z = newSetpoint.z;
+ float yaw = newSetpoint.yaw;
+
+ // UPDATE THE SETPOINT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 3));
+}
+#endif
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE QQQ U U EEEEE SSSS TTTTT N N EEEEE W W
+// R R E Q Q U U E S T NN N E W W
+// RRRR EEE Q Q U U EEE SSS T N N N EEE W W
+// R R E Q Q U U E S T N NN E W W W
+// R R EEEEE QQ Q UUU EEEEE SSSS T N N EEEEE W W
+//
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+// ----------------------------------------------------------------------------------
+
+
void DefaultControllerTab::publishSetpoint(float x, float y, float z, float yaw)
{
#ifdef CATKIN_MAKE
@@ -143,7 +221,7 @@ void DefaultControllerTab::publishSetpoint(float x, float y, float z, float yaw)
msg.x = x;
msg.y = y;
msg.z = z;
- msg.yaw = yaw;
+ msg.yaw = yaw * DEG2RAD;
// Publish the setpoint
this->requestSetpointChangePublisher.publish(msg);
@@ -224,8 +302,23 @@ void DefaultControllerTab::on_set_setpoint_button_clicked()
void DefaultControllerTab::on_default_setpoint_button_clicked()
{
#ifdef CATKIN_MAKE
- // Call the function to publish the setpoint
- publishSetpoint(m_defaultSetpoint.x, m_defaultSetpoint.y, m_defaultSetpoint.z, m_defaultSetpoint.yaw );
+ // Publish this as a blank setpoint with the
+ // "buttonID" field set appropriately
+
+ // Initialise the message as a local variable
+ d_fall_pps::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.buttonID = REQUEST_DEFAULT_SETPOINT_BUTTON_ID;
+
+ // Publish the default setpoint button press
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Published request for setpoint change to the default");
#endif
}
@@ -416,6 +509,99 @@ void DefaultControllerTab::on_yaw_increment_minus_button_clicked()
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void DefaultControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // // > Request the current flying state
+ // ros::ServiceClient getCurrentFlyingStateService = agent_base_nodeHandle.serviceClient<d_fall_pps::IntIntService>("PPSClient/getCurrentFlyingState", false);
+ // d_fall_pps::IntIntService getFlyingStateCall;
+ // getFlyingStateCall.request.data = 0;
+ // getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0));
+ // if(getCurrentFlyingStateService.call(getFlyingStateCall))
+ // {
+ // setFlyingState(getFlyingStateCall.response.data);
+ // }
+ // else
+ // {
+ // setFlyingState(STATE_UNAVAILABLE);
+ // }
+
+ // // > Request the current status of the crazy radio
+ // ros::ServiceClient getCurrentCrazyRadioStateService = agent_base_nodeHandle.serviceClient<d_fall_pps::IntIntService>("CrazyRadio/getCurrentCrazyRadioStatus", false);
+ // d_fall_pps::IntIntService getCrazyRadioCall;
+ // getCrazyRadioCall.request.data = 0;
+ // getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(2.0));
+ // if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall))
+ // {
+ // setCrazyRadioStatus(getCrazyRadioCall.response.data);
+ // }
+ // else
+ // {
+ // setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+ // }
+
+
+ // SUBSCRIBERS
+ // > For receiving message that the setpoint was changed
+ setpointChangedSubscriber = agent_base_nodeHandle.subscribe("DefaultControllerService/SetpointChanged", 1, &DefaultControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Set information back to the default
+ ui->lineEdit_setpoint_current_x->setText("xx.xx");
+ ui->lineEdit_setpoint_current_y->setText("xx.xx");
+ ui->lineEdit_setpoint_current_z->setText("xx.xx");
+ ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
+
+ }
+#endif
+}
+
+
+
+
+
+
// ----------------------------------------------------------------------------------
// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
// MM MM S G G H H E A A D D E R R
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
index 28908655..135fab04 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
@@ -137,7 +137,18 @@ void EnableControllerLoadYamlBar::on_load_yaml_student_button_clicked()
void EnableControllerLoadYamlBar::on_load_yaml_default_button_clicked()
{
-
+ #ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ d_fall_pps::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "DefaultController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Default Controller YAML was clicked.");
+#endif
}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
index cfd33ca8..58f17f9d 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
@@ -95,21 +95,25 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
// CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
// WITH THE LIST OF AGENT IDs TO COORDINATE
- // Connect the "should coordinate value changed" signal to
- // the respective slot
+ // This is passed from the "coordinator" to the elements
+ // in the main part of the GUI, namely to the:
+ // > "top banner",
+ // > "connect,start,stop bar",
+ // > "enable controller, load yaml bar", and
+ // > "controller tabs widget".
QObject::connect(
ui->customWidget_coordinator , &Coordinator::agentIDsToCoordinateChanged ,
- ui->customWidget_connectStartStopBar , &ConnectStartStopBar::setAgentIDsToCoordinate
+ ui->customWidget_topBanner , &TopBanner::setAgentIDsToCoordinate
);
QObject::connect(
ui->customWidget_coordinator , &Coordinator::agentIDsToCoordinateChanged ,
- ui->customWidget_enableControllerLoadYamlBar , &EnableControllerLoadYamlBar::setAgentIDsToCoordinate
+ ui->customWidget_connectStartStopBar , &ConnectStartStopBar::setAgentIDsToCoordinate
);
QObject::connect(
ui->customWidget_coordinator , &Coordinator::agentIDsToCoordinateChanged ,
- ui->customWidget_topBanner , &TopBanner::setAgentIDsToCoordinate
+ ui->customWidget_enableControllerLoadYamlBar , &EnableControllerLoadYamlBar::setAgentIDsToCoordinate
);
QObject::connect(
diff --git a/pps_ws/src/d_fall_pps/include/nodes/Constants.h b/pps_ws/src/d_fall_pps/include/nodes/Constants.h
index 33898352..fd4d1d4f 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/Constants.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/Constants.h
@@ -53,6 +53,25 @@
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// The types, i.e., agent or coordinator
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 2
+
+
+
+
+
// ----------------------------------------------------------------------------------
//
//
@@ -192,11 +211,16 @@
-// The types, i.e., agent or coordinator
-#define TYPE_INVALID -1
-#define TYPE_COORDINATOR 1
-#define TYPE_AGENT 2
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+// For standard buttons in the GUI
+#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
diff --git a/pps_ws/src/d_fall_pps/include/nodes/DefaultControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/DefaultControllerService.h
new file mode 100644
index 00000000..b88fa8e2
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/DefaultControllerService.h
@@ -0,0 +1,237 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// The fall-back 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>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "d_fall_pps/IntWithHeader.h"
+#include "d_fall_pps/StringWithHeader.h"
+#include "d_fall_pps/SetpointWithHeader.h"
+#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 the DFALL service types
+#include "d_fall_pps/LoadYamlFromFilename.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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.
+
+// NOTE: manz constants are already defined in the "Constant.h" header file
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+
+// The mass of the crazyflie, in [grams]
+float yaml_cf_mass_in_grams = 25.0;
+
+// Coefficients of the 16-bit command to thrust conversion
+//std::vector<float> yaml_motorPoly(3);
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+// Frequency at which the controller is running
+float yaml_control_frequency = 200.0;
+
+// The min and max for saturating 16 bit thrust commands
+float yaml_command_sixteenbit_min = 1000;
+float yaml_command_sixteenbit_max = 65000;
+
+// The default setpoint, the ordering is (x,y,z,yaw),
+// with unit [meters,meters,meters,radians]
+std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+
+
+
+// The weight of the Crazyflie in Newtons, i.e., mg
+float m_cf_weight_in_newtons = 0.0;
+
+// The location error of the Crazyflie at the "previous" time step
+float m_previous_stateErrorInertial[9];
+
+std::vector<float> m_setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
+
+
+// The LQR Controller parameters for "LQR_RATE_MODE"
+std::vector<float> m_gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
+std::vector<float> m_gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
+std::vector<float> m_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
+std::vector<float> m_gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
+
+
+// ROS Publisher for debugging variables
+ros::Publisher m_debugPublisher;
+
+// ROS Publisher for inform the network about
+// changes to the setpoin
+ros::Publisher m_setpointChangedPublisher;
+
+
+
+// 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
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButton& commandReceived);
+
+
+// > For the LOADING of YAML PARAMETERS
+void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
+void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
index 011ff0c7..75d51729 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
@@ -58,6 +58,9 @@
//#include "d_fall_pps/FloatWithHeader.h"
#include "d_fall_pps/StringWithHeader.h"
+// Include the DFALL service types
+#include "d_fall_pps/LoadYamlFromFilename.h"
+
// Include the shared definitions
#include "nodes/Constants.h"
@@ -101,6 +104,10 @@ int m_ID = 0;
// The namespace into which this parameter service loads yaml parameters
std::string m_base_namespace;
+// Publisher for passing a service request onto the
+// loadinging function
+ros::Publisher requestLoadYamlFilenamePublisher;
+
@@ -119,7 +126,7 @@ std::string m_base_namespace;
// P R R OOO T OOO T Y P EEEEE SSSS
// ----------------------------------------------------------------------------------
-void requestLoadControllerYamlCallback(const std_msgs::Int32& msg);
+bool requestLoadYamlFilenameCallbackServiceCallback(LoadYamlFromFilename::Request &request, LoadYamlFromFilename::Response &response);
void requestLoadYamlFilenameCallback(const StringWithHeader& yamlFilenameToLoad);
diff --git a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
index 8eea640a..7d440b25 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
@@ -55,7 +55,7 @@
#include "std_msgs/Float32.h"
#include <std_msgs/String.h>
-//the generated structs from the msg-files have to be included
+// Include the DFALL message types
#include "d_fall_pps/IntWithHeader.h"
#include "d_fall_pps/StringWithHeader.h"
#include "d_fall_pps/ViconData.h"
@@ -65,6 +65,9 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
+// Include the DFALL service types
+#include "d_fall_pps/LoadYamlFromFilename.h"
+
// Include the shared definitions
#include "nodes/Constants.h"
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index bb994eb3..2c978143 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -47,6 +47,15 @@
>
</node>
+ <!-- DEFAULT CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "DefaultControllerService"
+ output = "screen"
+ type = "DefaultControllerService"
+ >
+ </node>
+
<!-- SAFE CONTROLLER -->
<node
pkg = "d_fall_pps"
diff --git a/pps_ws/src/d_fall_pps/msg/SetpointWithHeader.msg b/pps_ws/src/d_fall_pps/msg/SetpointWithHeader.msg
index 9198063b..ff412908 100644
--- a/pps_ws/src/d_fall_pps/msg/SetpointWithHeader.msg
+++ b/pps_ws/src/d_fall_pps/msg/SetpointWithHeader.msg
@@ -5,4 +5,4 @@ float64 yaw
bool isChecked
uint32 buttonID
bool shouldCheckForAgentID
-uint8[] agentIDs
\ No newline at end of file
+uint32[] agentIDs
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/param/DefaultController.yaml b/pps_ws/src/d_fall_pps/param/DefaultController.yaml
new file mode 100644
index 00000000..3c208ca7
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/DefaultController.yaml
@@ -0,0 +1,16 @@
+# The mass of the crazyflie, in [grams]
+mass : 28
+
+# Frequency of the controller, in [hertz]
+control_frequency : 200
+
+# Quadratic motor regression equation (a0, a1, a2)
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# The min and max for saturating 16 bit thrust commands
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 65000
+
+# The default setpoint, the ordering is (x,y,z,yaw),
+# with unit [meters,meters,meters,radians]
+default_setpoint : [0.0, 0.0, 0.4, 0.0]
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/nodes/DefaultControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/DefaultControllerService.cpp
new file mode 100644
index 00000000..fe37b42c
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/nodes/DefaultControllerService.cpp
@@ -0,0 +1,962 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// The fall-back controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/DefaultControllerService.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 "DefaultController" 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 in the header file, they are:
+// - CF_COMMAND_TYPE_MOTORS
+// - CF_COMMAND_TYPE_RATE
+// - CF_COMMAND_TYPE_ANGLE
+//
+// > For completeing the class exercises it is only required to use
+// the CF_COMMAND_TYPE_RATE option.
+//
+// > For the CF_COMMAND_TYPE_RATE optoin:
+// 1) 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.
+// 2) 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" found in the firmware).
+// 3) 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, the 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 loop controller, add all your control
+ // computation here, or you may find it convienient to create functions
+ // to keep you code cleaner
+
+
+ // Define a local array to fill in with the state error
+ float stateErrorInertial[9];
+
+ // Fill in the (x,y,z) position error
+ stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
+ stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
+ stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
+
+ // Compute an estimate of the velocity
+ // > As simply the derivative between the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+
+ // Fill in the roll and pitch angle measurements directly
+ stateErrorInertial[6] = request.ownCrazyflie.roll;
+ stateErrorInertial[7] = request.ownCrazyflie.pitch;
+
+ // 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 = request.ownCrazyflie.yaw - m_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
+ stateErrorInertial[8] = yawError;
+
+
+ // 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
+ float stateErrorBody[9];
+ convertIntoBodyFrame(stateErrorInertial, stateErrorBody, request.ownCrazyflie.yaw);
+
+
+ // SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
+ // > as we have already used previous error we can now update it update it
+ for(int i = 0; i < 9; ++i)
+ {
+ m_previous_stateErrorInertial[i] = stateErrorInertial[i];
+ }
+
+
+
+
+ // YAW CONTROLLER
+
+ // Perform the "-Kx" LQR computation for the yaw rate
+ // to respond with
+ float yawRate_forResponse = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ yawRate_forResponse -= m_gainMatrixYawRate[i] * stateErrorBody[i];
+ }
+ // Put the computed yaw rate into the "response" variable
+ response.controlOutput.yaw = yawRate_forResponse;
+
+
+
+
+ // ALITUDE CONTROLLER (i.e., z-controller)
+
+ // Perform the "-Kx" LQR computation for the thrust adjustment
+ // to use for computing the response with
+ float thrustAdjustment = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ thrustAdjustment -= m_gainMatrixThrust[i] * stateErrorBody[i];
+ }
+
+ // Add the feed-forward thrust before putting in the response
+ float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
+ float thrust_per_motor = thrustAdjustment + feed_forward_thrust_per_motor;
+
+ // > NOTE: the function "computeMotorPolyBackward" converts the
+ // input argument from Newtons to the 16-bit command
+ // expected by the Crazyflie.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrust_per_motor);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrust_per_motor);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_per_motor);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_per_motor);
+
+
+ // BODY FRAME X CONTROLLER
+
+ // Perform the "-Kx" LQR computation for the pitch rate
+ // to respoond with
+ float pitchRate_forResponse = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ pitchRate_forResponse -= m_gainMatrixPitchRate[i] * stateErrorBody[i];
+ }
+ // Put the computed pitch rate into the "response" variable
+ response.controlOutput.pitch = pitchRate_forResponse;
+
+
+
+
+ // BODY FRAME Y CONTROLLER
+
+ // Instantiate the local variable for the roll rate that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+
+
+ // Perform the "-Kx" LQR computation for the roll rate
+ // to respoond with
+ float rollRate_forResponse = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ rollRate_forResponse -= m_gainMatrixRollRate[i] * stateErrorBody[i];
+ }
+ // Put the computed roll rate into the "response" variable
+ response.controlOutput.roll = rollRate_forResponse;
+
+
+
+ // PREPARE AND RETURN THE VARIABLE "response"
+
+ /*choosing the Crazyflie onBoard controller type.
+ it can either be Motor, Rate or Angle based */
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+
+
+
+ // ***********************************************************
+ // 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"
+ m_debugPublisher.publish(debugMsg);
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+
+ // 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-coordinates: " << request.ownCrazyflie.x);
+ // ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
+ // ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
+ // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
+ // ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
+ // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
+ // ROS_INFO_STREAM("Delta t: " << 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:" << yaml_motorPoly[0]);
+ // ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
+ // ROS_INFO_STREAM("motorPoly 2:" << yaml_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);
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+ }
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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
+//
+// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
+{
+
+ 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];
+}
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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 NEED TO BE edited for successful completion of
+// the exercise
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-but command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd_16bit < yaml_command_sixteenbit_min)
+ {
+ cmd_16bit = 0;
+ }
+ else if (cmd_16bit > yaml_command_sixteenbit_max)
+ {
+ cmd_16bit = yaml_command_sixteenbit_max;
+ }
+
+ // Return the result
+ return cmd_16bit;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Check if the request if for the default setpoint
+ if (newSetpoint.buttonID == REQUEST_DEFAULT_SETPOINT_BUTTON_ID)
+ {
+ setNewSetpoint(
+ yaml_default_setpoint[0],
+ yaml_default_setpoint[1],
+ yaml_default_setpoint[2],
+ yaml_default_setpoint[3]
+ );
+ }
+ else
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw
+ );
+ }
+ }
+}
+
+
+
+void setNewSetpoint(float x, float y, float z, float yaw)
+{
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = x;
+ m_setpoint[1] = y;
+ m_setpoint[2] = z;
+ m_setpoint[3] = yaw;
+
+ // Publish the change so that the netwrok is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC U U SSSS TTTTT OOO M M
+// C U U S T O O MM MM
+// C U U SSS T O O M M M
+// C U U S T O O M M
+// CCCC UUU SSSS T OOO M M
+//
+// CCCC OOO M M M M A N N DDDD
+// C O O MM MM MM MM A A NN N D D
+// C O O M M M M M M A A N N N D D
+// C O O M M M M AAAAA N NN D D
+// CCCC OOO M M M M A A N N DDDD
+// ----------------------------------------------------------------------------------
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButton& commandReceived)
+{
+ // Extract the data from the message
+ int custom_button_index = commandReceived.button_index;
+ float custom_command_code = commandReceived.command_code;
+
+ // Switch between the button pressed
+ switch(custom_button_index)
+ {
+
+ // > FOR CUSTOM BUTTON 1
+ case 1:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO("[DEFAULT CONTROLLER] Button 1 received in controller.");
+ // Code here to respond to custom button 1
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 2
+ case 2:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO("[DEFAULT CONTROLLER] Button 2 received in controller.");
+ // Code here to respond to custom button 2
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 3
+ case 3:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] Button 3 received in controller, with command code:" << custom_command_code );
+ // Code here to respond to custom button 3
+
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[DEMO CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.command_code = " << custom_command_code );
+ break;
+ }
+ }
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ofthe exercise
+void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[DEFAULT CONTROLLER] Now fetching the DefaultController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[DEFAULT CONTROLLER] Now fetching the DefaultController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[DEFAULT CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchDefaultControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+// This function CAN BE edited for successful completion of the
+// exercise, and the use of parameters fetched from the YAML file
+// is highly recommended to make tuning of your controller easier
+// and quicker.
+void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // DefaultController.yaml
+
+ // Add the "DefaultController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "DefaultController");
+
+
+
+ // GET THE PARAMETERS:
+
+ // The mass of the crazyflie, in [grams]
+ yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
+
+ // > The frequency at which the "computeControlOutput" function
+ // is being called, as determined by the frequency at which
+ // the Motion Caption (Vicon) system provides pose data, i.e.,
+ // measurement of (x,y,z) position and (roll,pitch,yaw) attitude.
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit
+ // motor command to thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+ // The min and max for saturating 16 bit thrust commands
+ yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ yaml_command_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
+
+ // The default setpoint, the ordering is (x,y,z,yaw),
+ // with unit [meters,meters,meters,radians]
+ getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+
+
+
+ // > DEBUGGING: Print out one of the parameters that was loaded to
+ // debug if the fetching of parameters worked correctly
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] DEBUGGING: the fetched DefaultController/mass = " << yaml_cf_mass_in_grams);
+
+
+
+ // PROCESS THE PARAMTERS
+
+ // > Compute the feed-forward force that we need to counteract
+ // gravity (i.e., mg) in units of [Newtons]
+ m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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, "DefaultControllerService");
+
+ // 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 "DefaultControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "PPSClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "PPSClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/PPSClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[DEFAULT CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "DefaultController", 1, isReadyDefaultControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("DefaultController", 1, isReadyDefaultControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "DefaultController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyDefaultControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[DEFAULT CONTROLLER] The request load yaml file service call failed.");
+ }
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "newSetpointRequestSubscriber" to
+ // be a "ros::Subscriber" type variable that subscribes to the
+ // "NewSetpointRequest" topic and calls the class function
+ // "newSetpointRequestCallback" each time a messaged is received on
+ // this topic and the message is passed as an input argument to the
+ // callback function. This subscriber will mainly receive messages
+ // from the "flying agent GUI" when the setpoint is changed by
+ // the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("DefaultControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Instantiate the class variable "m_setpointChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current setpoint for this controller.
+ m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "DefaultController". 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("DefaultController", calculateControlOutput);
+
+ // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "GUIButton" topic and calls the class
+ // function "customCommandReceivedCallback" each time a messaged is received on this topic
+ // and the message received is passed as an input argument to the callback function.
+ ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);
+
+ // 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("[DEFAULT 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;
+}
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 825f8108..917e7fa0 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -74,170 +74,17 @@
-/*
-void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
+bool requestLoadYamlFilenameCallbackServiceCallback(LoadYamlFromFilename::Request &request, LoadYamlFromFilename::Response &response)
{
- // Extract from the "msg" for which controller the YAML
- // parameters should be loaded
- int controller_to_load_yaml = msg.data;
+ // Put the flying state into the response variable
+ requestLoadYamlFilenamePublisher.publish(request.stringWithHeader);
- ROS_INFO_STREAM("[PARAMETER SERVICE] Received the message to load YAML parameters from file into cache");
+ // Put success into the response
+ response.data = 1;
-
- // Instantiate a local varaible to confirm that something can actually be loaded
- // from a YAML file
- bool isValidToAttemptLoad = true;
-
- // Instantiate a local variable for the string that will be passed to the "system"
- std::string cmd;
-
- // Get the abolute path to "d_fall_pps"
- std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
-
- // Switch between loading for the different controllers
- // ----------------------------------------
- // FOR THE SAFE CONTROLLER
- if (
- ((controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
- ||
- ((controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
- )
- {
- // Re-load the parameters of the safe controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_base_namespace + "/SafeController";
- }
- // ----------------------------------------
- // FOR THE DEMO CONTROLLER
- else if (
- ((controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
- ||
- ((controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
- )
- {
- // Re-load the parameters of the demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/DemoController.yaml " + m_base_namespace + "/DemoController";
- }
- // ----------------------------------------
- // FOR THE STUDENT CONTROLLER
- else if (
- ((controller_to_load_yaml==LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
- ||
- ((controller_to_load_yaml==LOAD_YAML_STUDENT_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
- )
- {
- // Re-load the parameters of the demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/StudentController.yaml " + m_base_namespace + "/StudentController";
- }
- // ----------------------------------------
- // FOR THE MPC CONTROLLER
- else if (
- ((controller_to_load_yaml==LOAD_YAML_MPC_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
- ||
- ((controller_to_load_yaml==LOAD_YAML_MPC_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
- )
- {
- // 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) && (m_type==TYPE_COORDINATOR))
- ||
- ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_AGENT) && (m_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";
- }
- // ----------------------------------------
- // FOR THE TUNING CONTROLLER
- else if (
- ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
- ||
- ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
- )
- {
- // Re-load the parameters of the demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/TuningController.yaml " + m_base_namespace + "/TuningController";
- }
- // ----------------------------------------
- // FOR THE PICKER CONTROLLER
- else if (
- ((controller_to_load_yaml==LOAD_YAML_PICKER_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
- ||
- ((controller_to_load_yaml==LOAD_YAML_PICKER_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
- )
- {
- // Re-load the parameters of the demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/PickerController.yaml " + m_base_namespace + "/PickerController";
- }
- else
- {
- // Let the user know that the command was not recognised
- ROS_INFO_STREAM("[PARAMETER SERVICE] > Nothing to load for this parameter service with.");
- ROS_INFO_STREAM("[PARAMETER SERVICE] > The message received has 'controller_to_load_yaml' = " << controller_to_load_yaml);
- ROS_INFO_STREAM("[PARAMETER SERVICE] > And the type of this Parameter Service is 'm_type' = " << m_type);
- // Set the boolean that prevents the fetch message from being sent
- isValidToAttemptLoad = false;
- }
-
-
- // Only bother with ttempting to loaded the .yaml file, and subseuently send the "ready for fetch"
- // message if something can actually be loaded from a YAML file
- if (isValidToAttemptLoad)
- {
- // Let the user know what is about to happen
- ROS_INFO_STREAM("[PARAMETER SERVICE] > The following path will be used for locating the .yaml file: " << d_fall_pps_path << " The comand line string sent to the 'system' is: " << cmd );
-
- // Re-load the parameters by pass the command line string via a "system" call
- // > i.e., this replicates pasting this string in a new terminal window and pressing enter
- system(cmd.c_str());
-
- // Pause breifly to ensure that the yaml file is fully loaded
- ros::Duration(0.5).sleep();
-
- // Instantiate a local varaible to confirm that something was actually loaded from
- // a YAML file
- bool isReadyForFetch = true;
-
- // Instantiate a local variable for the fetch message
- std_msgs::Int32 fetch_msg;
- // Fill in the data of the fetch message
- fetch_msg.data = controller_to_load_yaml;
- // Fill in the data of the fetch message
- // switch(controller_to_load_yaml)
- // {
- // case (LOAD_YAML_SAFE_CONTROLLER_COORDINATOR):
- // fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR;
- // break;
- // case (LOAD_YAML_DEMO_CONTROLLER_COORDINATOR):
- // fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR;
- // break;
- // case (LOAD_YAML_SAFE_CONTROLLER_AGENT):
- // fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT;
- // break;
- // case (LOAD_YAML_DEMO_CONTROLLER_AGENT):
- // fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT;
- // break;
- // default:
- // // Let the user know that the command was not recognised
- // ROS_INFO("Unknown 'controller to load yaml' command, thus a 'ready to fetch' message will NOT be published.");
- // // Set the boolean that prevents the fetch message from being sent
- // isReadyForFetch = false;
- // break;
- // }
- // Send a message that the YAML parameter have been loaded and hence are
- // ready to be fetched (i.e., using getparam())
- if (isReadyForFetch)
- {
- controllerYamlReadyForFetchPublisher.publish(fetch_msg);
- }
- }
+ // Return
+ return true;
}
-*/
-
-
void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_load_with_header)
{
@@ -487,6 +334,12 @@ int main(int argc, char* argv[])
// Subscribe to the messages that request loading a yaml file
ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 1, requestLoadYamlFilenameCallback);
+ // Publisher for publishing "internally" to the subscriber above
+ requestLoadYamlFilenamePublisher = nodeHandle.advertise <StringWithHeader>("requestLoadYamlFilename", 1);
+
+ // Advertise the service for requests to load a yaml file
+ ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
+
// Inform the user the this node is ready
ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
// Spin as a single-thread node
diff --git a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
index d738c688..12ba738b 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -846,30 +846,38 @@ int main(int argc, char* argv[]) {
// FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
- // The yaml files for the controllers are not added to "Parameter
- // Service" as part of launching.
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
// The process for loading the yaml parameters is to send a
- // message containing the filename of the *.yaml file, and
- // then a message will be received on the above subscribers
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
// when the paramters are ready.
-
- // Create a publisher for request the yaml load
- // > created as a local variable
- // > note importantly that the final argument is "true", this
- // enables "latching" on the connection. When a connection is
- // latched, the last message published is saved and
- // automatically sent to any future subscribers that connect.
- ros::Publisher requestLoadYamlFilenamePublisher = nodeHandle_to_own_agent_parameter_service.advertise<StringWithHeader>("requestLoadYamlFilename", 1, true);
- // Create a local variable for the message
- StringWithHeader yaml_filename_msg;
- // Specify the data
- yaml_filename_msg.data = "StudentController";
- // Set for whom this applies to
- yaml_filename_msg.shouldCheckForID = false;
- // Sleep to make the publisher known to ROS (in seconds)
- //ros::Duration(1.0).sleep();
- // Send the message
- requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "StudentController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyStudentControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[STUDENT CONTROLLER] The request load yaml file service call failed.");
+ }
diff --git a/pps_ws/src/d_fall_pps/srv/LoadYamlFromFilename.srv b/pps_ws/src/d_fall_pps/srv/LoadYamlFromFilename.srv
new file mode 100644
index 00000000..c3f841a9
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/LoadYamlFromFilename.srv
@@ -0,0 +1,3 @@
+StringWithHeader stringWithHeader
+---
+uint32 data
--
GitLab