diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 09af57d22ad7089d8bf2bb13052ea6f20b9906fa..81579ef9f643443074a398f3c99e24aa8eccd704 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 93f79577d92fd390d0a9b049e8e7ca50e4ff1c3d..d96707d4ea9d913932c4364b16345c01e6fdfc43 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 7aad04c8c5296835b92b63aba5f5962a70719a4b..a4d11624d21eb22cceac82e97ee775aa07c8ba8c 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 c8a3a4a919864f445ce8e063c374039374a2d98b..68edb4ea2d980a4c0f4cf7acd3a85855e4d3128b 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 aafb53da1629859fcb401560f280724a929e951b..d71401287e10bae9b049e929ffa8231bb8def3cd 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 56aaf802665a67e9fc1634182b5cc2643b9ac9cb..147a43ba57c542b5a46b0d80665e2791498107e6 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 2890865534f0f48097abf5f06a36b1a127bc7f0c..135fab04895da4edee344a057c55f2513abea0f3 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 cfd33ca8fc32650c81e72946bc7657bb6d67a89a..58f17f9d672367d1e5233bfaf8cb6292a2341ac6 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 3389835206d622359a88917b67ad18df10db7b16..fd4d1d4fcf29c6b76782f79f803ec6f93bd7a988 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 0000000000000000000000000000000000000000..b88fa8e2d13a1ea256914875ee955ec4109e0429
--- /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 011ff0c73baa4186b05bb15e610f950c15b17d3a..75d5172928d0792a955debcbc01809dd8b22da0e 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 8eea640a9f5e8a92d778757f8e86be50b117124e..7d440b25ef09187e8e01410ea7d6affbf38cd48c 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 bb994eb31df45deb508c13f4107f60f93d0da668..2c97814356f996fc45087b00d2c4d6427fe6fb67 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 9198063bc304eeb821d802752c48b99e2f7402de..ff412908b37d14516ae13d70209dd218c95b2277 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 0000000000000000000000000000000000000000..3c208ca7a7f77c05e4570bee4e0c62a0eb02ae3b
--- /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 0000000000000000000000000000000000000000..fe37b42c2eec7e7ad03ef1c088d16d400ffe4be8
--- /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 825f8108ea949cc30ea95be52f461b86650753f1..917e7fa042e3b64ec9ed398adba161f55edd4eab 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 d738c68816520804ceaced1e82c753d3dd0e77c4..12ba738ba34f59d59ecb38116acdfc5ac1bd9043 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 0000000000000000000000000000000000000000..c3f841a95dd3a56131ad8f98aae09857281498db
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/LoadYamlFromFilename.srv
@@ -0,0 +1,3 @@
+StringWithHeader stringWithHeader
+---
+uint32 data