From 8fe7e732cafca95ed2ff8adf8de2810ee6a68247 Mon Sep 17 00:00:00 2001
From: beuchatp <beuchatp@control.ee.ethz.ch>
Date: Wed, 19 Dec 2018 19:05:14 +0100
Subject: [PATCH] Tested to be working and added highlighting the tab label for
 the controller that is operating. Next step is to add tabs for the remote and
 picker controllers, add service for getting the currently operating
 controller, and switch and PPSClient from the safe controller to the default
 controller.

---
 .../flyingAgentGUI/include/controllertabs.h   |  17 +-
 .../flyingAgentGUI/src/controllertabs.cpp     | 171 +++++++++++++++++-
 .../src/defaultcontrollertab.cpp              |  22 ++-
 .../src/studentcontrollertab.cpp              |  48 ++---
 .../src/nodes/DefaultControllerService.cpp    |  10 +-
 .../src/nodes/StudentControllerService.cpp    |  22 ++-
 6 files changed, 256 insertions(+), 34 deletions(-)

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 8cf774a9..6ada530a 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
@@ -11,7 +11,7 @@
 #include <ros/package.h>
 
 // Include the standard message types
-//#include "std_msgs/Int32.h"
+#include "std_msgs/Int32.h"
 //#include "std_msgs/Float32.h"
 //#include <std_msgs/String.h>
 
@@ -90,6 +90,11 @@ private:
     // Flag for whether pose data should be emitted, this is
     // to save looking through the data when it is unnecessary
     bool m_should_search_pose_data_for_object_name = false;
+    QMutex m_should_search_pose_data_for_object_name_mutex;
+
+    // The color for normal and highlighted tabs
+    QColor m_tab_text_colour_normal;
+    QColor m_tab_text_colour_highlight;
 
 
 #ifdef CATKIN_MAKE
@@ -99,6 +104,8 @@ private:
     // SUBSRIBER
     // > For the pose data from a motion capture system
     ros::Subscriber m_poseDataSubscriber;
+    // > For the controller that is currently operating
+    ros::Subscriber controllerUsedSubscriber;
 #endif
 
 
@@ -110,6 +117,14 @@ private:
     //   "controllerUsedSubscriber"
     void poseDataReceivedCallback(const d_fall_pps::ViconData& viconData);
 
+    void controllerUsedChangedCallback(const std_msgs::Int32& msg);
+
+    void setControllerEnabled(int new_controller);
+
+    void setAllTabLabelsToNormalColouring();
+
+    void setTextColourOfTabLabel(QColor color , QWidget * tab_widget);
+
     // Get the paramters that specify the type and ID
     bool getTypeAndIDParameters();
 #endif
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 45dfcd9d..88a4973c 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
@@ -8,6 +8,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
     ui->setupUi(this);
 
 
+    // Specify the color for normal and highlighted tabs
+    m_tab_text_colour_normal = Qt::black;
+    m_tab_text_colour_highlight = QColor(0,200,0);
+
+
     // Initialise the object name as blank
     m_object_name_for_emitting_pose_data = "";
 
@@ -34,6 +39,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
             ui->default_controller_tab_widget , &DefaultControllerTab::poseDataUnavailableSlot
         );
 
+    QObject::connect(
+            this , &ControllerTabs::poseDataUnavailableSignal ,
+            ui->student_controller_tab_widget , &StudentControllerTab::poseDataUnavailableSlot
+        );
+
 
     // CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
     // WITH THE LIST OF AGENT IDs TO COORDINATE
@@ -45,6 +55,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
             ui->default_controller_tab_widget , &DefaultControllerTab::setAgentIDsToCoordinate
         );
 
+    QObject::connect(
+            this , &ControllerTabs::agentIDsToCoordinateChanged ,
+            ui->student_controller_tab_widget , &StudentControllerTab::setAgentIDsToCoordinate
+        );
+
     
 
 
@@ -68,6 +83,7 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
     }
 
 
+
     // CREATE A NODE HANDLE TO THIS GUI
     ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
 
@@ -77,6 +93,14 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
     // CREATE THE SUBSCRIBER TO THE MOTION CAPTURE DATA
     m_poseDataSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &ControllerTabs::poseDataReceivedCallback, this);
 
+    // CREATE THE SUBSCRIBER TO THE CONTROLLER THAT IS CURRENTLY OPERATING
+    // Only if this is an agent GUI
+    if (m_type == TYPE_AGENT)
+    {
+        controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("PPSClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
+    }
+
+
 #endif
 
 }
@@ -136,6 +160,7 @@ void ControllerTabs::showHideController_safe_changed()
 
 void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
 {
+    m_should_search_pose_data_for_object_name_mutex.lock();
     if (object_name.isEmpty())
     {
         // Set the class variable accordingly
@@ -160,6 +185,7 @@ void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
             ROS_INFO_STREAM("[CONTROLLER TABS GUI] now emitting data for object named: " << m_object_name_for_emitting_pose_data );
         #endif
     }
+    m_should_search_pose_data_for_object_name_mutex.unlock();
 }
 
 
@@ -168,6 +194,7 @@ void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
 //   "controllerUsedSubscriber"
 void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& viconData)
 {
+    m_should_search_pose_data_for_object_name_mutex.lock();
     if (m_should_search_pose_data_for_object_name)
     {
         for(std::vector<d_fall_pps::CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
@@ -188,6 +215,7 @@ void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& vicon
             }
         }
     }
+    m_should_search_pose_data_for_object_name_mutex.unlock();
 }
 #endif
 
@@ -195,6 +223,102 @@ void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& vicon
 
 
 
+
+//    ----------------------------------------------------------------------------------
+//     CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
+//    C      O   O  NN  N    T    R   R  O   O  L      L      E      R   R
+//    C      O   O  N N N    T    RRRR   O   O  L      L      EEE    RRRR
+//    C      O   O  N  NN    T    R   R  O   O  L      L      E      R   R
+//     CCCC   OOO   N   N    T    R   R   OOO   LLLLL  LLLLL  EEEEE  R   R
+//
+//    EEEEE  N   N    A    BBBB   L      EEEEE  DDDD
+//    E      NN  N   A A   B   B  L      E      D   D
+//    EEE    N N N  A   A  BBBB   L      EEE    D   D
+//    E      N  NN  AAAAA  B   B  L      E      D   D
+//    EEEEE  N   N  A   A  BBBB   LLLLL  EEEEE  DDDD
+//    ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// > For the controller currently operating, received on "controllerUsedSubscriber"
+void ControllerTabs::controllerUsedChangedCallback(const std_msgs::Int32& msg)
+{
+    //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Controller Used Changed Callback called for agentID = " << m_agentID);
+    setControllerEnabled(msg.data);
+}
+#endif
+
+
+void ControllerTabs::setControllerEnabled(int new_controller)
+{
+    // First set everything back to normal colouring
+    setAllTabLabelsToNormalColouring();
+
+    // Now switch to highlight the tab corresponding to
+    // the enable controller
+    switch(new_controller)
+    {
+        case SAFE_CONTROLLER:
+        {
+            //ui->controller_enabled_label->setText("Safe");
+            break;
+        }
+        case DEMO_CONTROLLER:
+        {
+            //ui->controller_enabled_label->setText("Demo");
+            break;
+        }
+        case STUDENT_CONTROLLER:
+        {
+            setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->student_tab );
+            break;
+        }
+        case MPC_CONTROLLER:
+        {
+            //ui->controller_enabled_label->setText("MPC");
+            break;
+        }
+        case REMOTE_CONTROLLER:
+        {
+            //ui->controller_enabled_label->setText("Remote");
+            break;
+        }
+        case TUNING_CONTROLLER:
+        {
+            //ui->controller_enabled_label->setText("Tuning");
+            break;
+        }
+        default:
+        {
+            //ui->controller_enabled_label->setText("Unknown");
+            break;
+        }
+    }
+}
+
+
+void ControllerTabs::setAllTabLabelsToNormalColouring()
+{
+    setTextColourOfTabLabel( m_tab_text_colour_normal , ui->default_tab );
+    setTextColourOfTabLabel( m_tab_text_colour_normal , ui->student_tab );
+    setTextColourOfTabLabel( m_tab_text_colour_normal , ui->picker_tab );
+    setTextColourOfTabLabel( m_tab_text_colour_normal , ui->safe_tab );
+}
+
+void ControllerTabs::setTextColourOfTabLabel(QColor color , QWidget * tab_widget)
+{
+    // Get the current index of the tab
+    int current_index_of_tab = ui->controller_tabs_widget->indexOf(tab_widget);
+    // Onlz apply the colour is the tab is found
+    if (current_index_of_tab >= 0)
+    {
+        ui->controller_tabs_widget->tabBar()->setTabTextColor(current_index_of_tab, color);
+    }
+}
+
+
+
 //    ----------------------------------------------------------------------------------
 //      A     GGGG  EEEEE  N   N  TTTTT     III  DDDD    SSSS
 //     A A   G      E      NN  N    T        I   D   D  S
@@ -206,8 +330,53 @@ void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& vicon
 
 void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
 {
-    // Simply pass on the signal to the tabs
+    // Pass on the signal to the tabs
     emit agentIDsToCoordinateChanged( agentIDs , 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());
+
+        // SUBSCRIBERS
+        // > For receiving message that the setpoint was changed
+        controllerUsedSubscriber = agent_base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
+    }
+    else
+    {
+        // Unsubscribe
+        controllerUsedSubscriber.shutdown();
+
+        // Set all tabs to be normal colours
+        setAllTabLabelsToNormalColouring();
+
+    }
+#endif
+
 }
 
 
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 2f198ff9..697a2fc5 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
@@ -46,6 +46,26 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
         setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("DefaultControllerService/SetpointChanged", 1, &DefaultControllerTab::setpointChangedCallback, this);
     }
 
+    // GET THE CURRENT SETPOINT
+    // Only if this is an agent GUI
+    if (m_type == TYPE_AGENT)
+    {
+        // > Request the current setpoint
+        ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<d_fall_pps::GetSetpointService>("DefaultControllerService/GetCurrentSetpoint", false);
+        d_fall_pps::GetSetpointService getSetpointCall;
+        getSetpointCall.request.data = 0;
+        getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+        if(getCurrentSetpointServiceClient.call(getSetpointCall))
+        {
+            setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+        }
+        else
+        {
+            // Inform the user
+            ROS_INFO("[DEFAULT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+        }
+    }
+
 #endif
 
 }
@@ -555,7 +575,7 @@ void DefaultControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool
         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 setpoint
+        // > Request the current setpoint
         ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<d_fall_pps::GetSetpointService>("DefaultControllerService/GetCurrentSetpoint", false);
         d_fall_pps::GetSetpointService getSetpointCall;
         getSetpointCall.request.data = 0;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
index 993482f2..758a4014 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
@@ -46,9 +46,29 @@ StudentControllerTab::StudentControllerTab(QWidget *parent) :
         setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("StudentControllerService/SetpointChanged", 1, &StudentControllerTab::setpointChangedCallback, this);
     }
 
-    // CREATE THE CUSTOM BUTTON PRESSED PUBLISHED
+    // CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
     customButtonPublisher = nodeHandle_for_this_gui.advertise<d_fall_pps::CustomButtonWithHeader>("StudentControllerService/CustomButtonPressed", 1);
 
+    // GET THE CURRENT SETPOINT
+    // Only if this is an agent GUI
+    if (m_type == TYPE_AGENT)
+    {
+        // > Request the current setpoint
+        ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<d_fall_pps::GetSetpointService>("StudentControllerService/GetCurrentSetpoint", false);
+        d_fall_pps::GetSetpointService getSetpointCall;
+        getSetpointCall.request.data = 0;
+        getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+        if(getCurrentSetpointServiceClient.call(getSetpointCall))
+        {
+            setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+        }
+        else
+        {
+            // Inform the user
+            ROS_INFO("[STUDENT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+        }
+    }
+
 #endif
 
 }
@@ -106,52 +126,34 @@ void StudentControllerTab::on_custom_button_1_clicked()
 {
 #ifdef CATKIN_MAKE
     publish_custom_button_command(1,ui->lineEdit_custom_1);
-    // // Initialise the message as a local variable
-    // d_fall_pps::CustomButtonWithHeader msg;
-    // // Fill the header of the message
-    // fillCustomButtonMessageHeader( msg );
-    // // Fill in the button index
-    // msg.button_index = 1;
-    // // Get the line edit data, as a float if possible
-    // bool isFloat = false;
-    // float lineEdit_as_float = (ui->lineEdit_custom_1->text()).toFloat(isFloat);
-    // // Fill in the data
-    // if (isFloat)
-    //     msg.float_data = lineEdit_as_float;
-    // else
-    //     msg.string_data = (ui->lineEdit_custom_1->text()).toStdString();
-    // // Publish the setpoint
-    // this->customButtonPublisher.publish(msg);
-    // // Inform the user about the change
-    // ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 1 clicked.");
 #endif
 }
 
 void StudentControllerTab::on_custom_button_2_clicked()
 {
 #ifdef CATKIN_MAKE
-    ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 2 clicked.");
+    publish_custom_button_command(2,ui->lineEdit_custom_2);
 #endif
 }
 
 void StudentControllerTab::on_custom_button_3_clicked()
 {
 #ifdef CATKIN_MAKE
-    ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 3 clicked.");
+    publish_custom_button_command(3,ui->lineEdit_custom_3);
 #endif
 }
 
 void StudentControllerTab::on_custom_button_4_clicked()
 {
 #ifdef CATKIN_MAKE
-    ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 4 clicked.");
+    publish_custom_button_command(4,ui->lineEdit_custom_4);
 #endif
 }
 
 void StudentControllerTab::on_custom_button_5_clicked()
 {
 #ifdef CATKIN_MAKE
-    ROS_INFO("[STUDENT CONTROLLER TAB GUI] button 5 clicked.");
+    publish_custom_button_command(5,ui->lineEdit_custom_5);
 #endif
 }
 
diff --git a/pps_ws/src/d_fall_pps/src/nodes/DefaultControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/DefaultControllerService.cpp
index 81807850..db199ea1 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/DefaultControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/DefaultControllerService.cpp
@@ -933,10 +933,10 @@ int main(int argc, char* argv[]) {
 
 	// 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:
+	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
@@ -979,7 +979,7 @@ int main(int argc, char* argv[]) {
 	// 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());
+	//ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
 
 	// Print out some information to the user.
 	ROS_INFO("[DEFAULT CONTROLLER] Service ready :-)");
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 ddca6073..c7e22404 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -656,7 +656,7 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
 			case 1:
 			{
 				// Let the user know that this part of the code was triggered
-				ROS_INFO("[STUDENT CONTROLLER] Button 1 received in controller.");
+				ROS_INFO_STREAM("[STUDENT CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
 				// Code here to respond to custom button 1
 				
 				break;
@@ -666,7 +666,7 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
 			case 2:
 			{
 				// Let the user know that this part of the code was triggered
-				ROS_INFO("[STUDENT CONTROLLER] Button 2 received in controller.");
+				ROS_INFO_STREAM("[STUDENT CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
 				// Code here to respond to custom button 2
 
 				break;
@@ -987,6 +987,14 @@ int main(int argc, char* argv[]) {
 	// 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("StudentControllerService/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
@@ -1023,11 +1031,19 @@ int main(int argc, char* argv[]) {
     // and the message received is passed as an input argument to the callback function.
     ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("CustomButtonPressed", 1, customCommandReceivedCallback);
 
+    // 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 customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("StudentControllerService/CustomButtonPressed", 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());
+    //ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
 
     // Print out some information to the user.
     ROS_INFO("[STUDENT CONTROLLER] Service ready :-)");
-- 
GitLab