diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 8fb21f1ccab2c6c03e04bfd54b6bf222cee3b25c..c77fe171ca8b606eb71302bfae2b039ae4908419 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -209,6 +209,7 @@ add_message_files(
IntWithHeader.msg
FloatWithHeader.msg
StringWithHeader.msg
+ SetpointWithHeader.msg
#------------------------
DebugMsg.msg
CustomButton.msg
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 35cb217861406c4d6507db25e81b81aa7f56cc3d..7ca9fbfcec6486dd81a40e5322775991e8d17807 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
@@ -2,6 +2,7 @@
#define CONTROLLERTABS_H
#include <QWidget>
+#include <QMutex>
#include <QVector>
#ifdef CATKIN_MAKE
@@ -18,6 +19,7 @@
//#include "d_fall_pps/IntWithHeader.h"
//#include "d_fall_pps/SetpointWithHeader.h"
#include "d_fall_pps/CrazyflieData.h"
+#include "d_fall_pps/ViconData.h"
// Include the shared definitions
//#include "nodes/Constants.h"
@@ -45,12 +47,27 @@ public:
signals:
- void measuredPoseValueChanged(QVector<float> measuredPose);
+ void measuredPoseValueChanged(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
private:
Ui::ControllerTabs *ui;
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
#ifdef CATKIN_MAKE
// --------------------------------------------------- //
@@ -58,17 +75,20 @@ private:
// SUBSRIBER
// > For the pose data from a motion capture system
- ros::Subscriber poseDataSubscriber;
+ ros::Subscriber m_poseDataSubscriber;
+#endif
+#ifdef CATKIN_MAKE
// --------------------------------------------------- //
// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
// > For the controller currently operating, received on
// "controllerUsedSubscriber"
- void poseDataReceivedCallback(const d_fall_pps::CrazyflieData& msg);
-
+ void poseDataReceivedCallback(const d_fall_pps::ViconData& viconData);
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
#endif
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 6e95ea70a886a636ab9b6c683b65e30672cc4b4f..6ee01b2e9fa1014ddd365afb69d8e19c3f5eaacd 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
@@ -2,6 +2,7 @@
#define DEFAULTCONTROLLERTAB_H
#include <QWidget>
+#include <QMutex>
#include <QVector>
#include <QTextStream>
@@ -45,7 +46,7 @@ public:
public slots:
- void setMeasuredPose(QVector<float> measuredPose);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
private slots:
@@ -70,8 +71,43 @@ private slots:
private:
Ui::DefaultControllerTab *ui;
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+
+#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;
+#endif
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+
#ifdef CATKIN_MAKE
- SetpointWithHeader m_defaultSetpoint;
+ // Fill the header for a message
+ void fillSetpointMessageHeader( d_fall_pps::SetpointWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
#endif
void publishSetpoint(float x, float y, float z, float yaw);
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
index 01af34d82219ec6d3484e2b39b3dea4b855ad478..c3c57fc1d05fc73036cd0ceff0bbef90a93383dd 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
@@ -46,7 +46,7 @@ public:
public slots:
- void setMeasuredPose(QVector<float> measuredPose);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
private:
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 4340091b0c92fcbd5a58137e78ed4fb75a6b60a0..5d5318c9724ee0e1e49ee3a55d2fc5502bf93171 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
@@ -23,6 +23,38 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->student_controller_tab_widget , &StudentControllerTab::setMeasuredPose
);
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CONTROLLER TABS GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[CONTROLLER TABS GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE A NODE HANDLE TO THE D-FaLL ROOT
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+ // CREATE THE SUBSCRIBER TO THE MOTION CAPTURE DATA
+ m_poseDataSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &ControllerTabs::poseDataReceivedCallback, this);
+
+#endif
+
}
ControllerTabs::~ControllerTabs()
@@ -35,20 +67,142 @@ ControllerTabs::~ControllerTabs()
#ifdef CATKIN_MAKE
// > For the controller currently operating, received on
// "controllerUsedSubscriber"
-void poseDataReceivedCallback(const d_fall_pps::CrazyflieData& msg)
+void ControllerTabs::poseDataReceivedCallback(const d_fall_pps::ViconData& viconData)
{
- // Initialise a Qvector to sending around
- QVector<float> poseDataForSignal;
- // Fill in the data
- poseDataForSignal.push_back(msg.x);
- poseDataForSignal.push_back(msg.y);
- poseDataForSignal.push_back(msg.z);
- poseDataForSignal.push_back(msg.roll);
- poseDataForSignal.push_back(msg.pitch);
- poseDataForSignal.push_back(msg.yaw);
- // Emit the signal
- emit measuredPoseValueChanged(poseDataForSignal);
+ for(std::vector<d_fall_pps::CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
+ {
+ d_fall_pps::CrazyflieData pose_in_global_frame = *it;
+ if(pose_in_global_frame.crazyflieName == "CF05")
+ {
+ emit measuredPoseValueChanged(
+ pose_in_global_frame.x,
+ pose_in_global_frame.y,
+ pose_in_global_frame.z,
+ pose_in_global_frame.roll,
+ pose_in_global_frame.pitch,
+ pose_in_global_frame.yaw,
+ pose_in_global_frame.occluded
+ );
+ }
+ }
+
+
+ // OLD STYLE
+ // // Initialise a Qvector to sending around
+ // QVector<float> poseDataForSignal;
+ // // Fill in the data
+ // poseDataForSignal.push_back(msg.x);
+ // poseDataForSignal.push_back(msg.y);
+ // poseDataForSignal.push_back(msg.z);
+ // poseDataForSignal.push_back(msg.roll);
+ // poseDataForSignal.push_back(msg.pitch);
+ // poseDataForSignal.push_back(msg.yaw);
+ // // Emit the signal
+ // emit measuredPoseValueChanged(poseDataForSignal);
}
#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool ControllerTabs::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 value of the "type" parameter into a local string variable
+ std::string type_string;
+ if(!nodeHandle.getParam("type", type_string))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
\ No newline at end of file
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 f2b759d73203684709d845c57a65b7f00922016d..2e9040f3b3a402cfa008ab191e5b92a3085e33d9 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
@@ -10,6 +10,32 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE COMMAND PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<d_fall_pps::SetpointWithHeader>("DefaultControllerService/RequestSetpointChange", 1);
+
+
+ // Set the default setpoint
m_defaultSetpoint.x = 0.0f;
m_defaultSetpoint.y = 0.0f;
m_defaultSetpoint.z = 0.5f;
@@ -24,39 +50,42 @@ DefaultControllerTab::~DefaultControllerTab()
}
-void DefaultControllerTab::setMeasuredPose(QVector<float> measuredPose)
+void DefaultControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
{
- // UPDATE THE MEASUREMENT COLUMN
- ui->lineEdit_measured_x ->setText(QString::number( measuredPose[0] ));
- ui->lineEdit_measured_y ->setText(QString::number( measuredPose[1] ));
- ui->lineEdit_measured_z ->setText(QString::number( measuredPose[2] ));
- ui->lineEdit_measured_roll ->setText(QString::number( measuredPose[3] ));
- ui->lineEdit_measured_pitch->setText(QString::number( measuredPose[4] ));
- ui->lineEdit_measured_yaw ->setText(QString::number( measuredPose[5] ));
-
- // GET THE CURRENT SETPOINT
- float curr_x_setpoint = (ui->lineEdit_setpoint_current_x->text() ).toFloat();;
- float curr_y_setpoint = (ui->lineEdit_setpoint_current_y->text() ).toFloat();;
- float curr_z_setpoint = (ui->lineEdit_setpoint_current_z->text() ).toFloat();;
- float curr_yaw_setpoint = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();;
-
- // UPDATE THE ERROR COLUMN
- ui->lineEdit_error_x ->setText(QString::number( measuredPose[0] - curr_x_setpoint ));
- ui->lineEdit_error_y ->setText(QString::number( measuredPose[1] - curr_y_setpoint ));
- ui->lineEdit_error_z ->setText(QString::number( measuredPose[2] - curr_z_setpoint ));
- ui->lineEdit_error_yaw->setText(QString::number( measuredPose[5] - curr_yaw_setpoint ));
+ if (!occluded)
+ {
+ // UPDATE THE MEASUREMENT COLUMN
+ ui->lineEdit_measured_x ->setText(QString::number( x ));
+ ui->lineEdit_measured_y ->setText(QString::number( y ));
+ ui->lineEdit_measured_z ->setText(QString::number( z ));
+ ui->lineEdit_measured_roll ->setText(QString::number( roll ));
+ ui->lineEdit_measured_pitch->setText(QString::number( pitch ));
+ ui->lineEdit_measured_yaw ->setText(QString::number( yaw ));
+
+ // GET THE CURRENT SETPOINT
+ float curr_x_setpoint = (ui->lineEdit_setpoint_current_x->text() ).toFloat();;
+ float curr_y_setpoint = (ui->lineEdit_setpoint_current_y->text() ).toFloat();;
+ float curr_z_setpoint = (ui->lineEdit_setpoint_current_z->text() ).toFloat();;
+ float curr_yaw_setpoint = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();;
+
+ // UPDATE THE ERROR COLUMN
+ ui->lineEdit_error_x ->setText(QString::number( x - curr_x_setpoint ));
+ ui->lineEdit_error_y ->setText(QString::number( y - curr_y_setpoint ));
+ ui->lineEdit_error_z ->setText(QString::number( z - curr_z_setpoint ));
+ ui->lineEdit_error_yaw->setText(QString::number( yaw - curr_yaw_setpoint ));
+ }
}
-void publishSetpoint(float x, float y, float z, float yaw)
+void DefaultControllerTab::publishSetpoint(float x, float y, float z, float yaw)
{
#ifdef CATKIN_MAKE
// Initialise the message as a local variable
- SetpointWithHeader msg;
+ d_fall_pps::SetpointWithHeader msg;
// Fill the header of the message
- fillSetpointHeader( msg );
+ fillSetpointMessageHeader( msg );
// Fill in the (x,y,z,yaw) values
msg.x = x;
@@ -65,10 +94,10 @@ void publishSetpoint(float x, float y, float z, float yaw)
msg.yaw = yaw;
// Publish the setpoint
- this->controllerSetpointPublisher.publish(msg);
+ this->requestSetpointChangePublisher.publish(msg);
// Inform the user about the change
- ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Published request for setpoint request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw << "]");
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Published request for setpoint change to: [" << x << ", "<< y << ", "<< z << ", "<< yaw << "]");
#else
// TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
QTextStream(stdout) << "[DEFAULT CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw << "]";
@@ -156,9 +185,9 @@ void DefaultControllerTab::on_x_increment_plus_button_clicked()
{
// Call the function to publish the setpoint
publishSetpoint(
- (ui->lineEdit_setpoint_current_x->text() ).toFloat() + (ui->lineEdit_setpoint_increment_x->text()).toFloat()
- (ui->lineEdit_setpoint_current_y->text() ).toFloat()
- (ui->lineEdit_setpoint_current_z->text() ).toFloat()
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat() + (ui->lineEdit_setpoint_increment_x->text()).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat(),
(ui->lineEdit_setpoint_current_yaw->text()).toFloat()
);
}
@@ -330,3 +359,160 @@ void DefaultControllerTab::on_yaw_increment_minus_button_clicked()
}
#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
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the head for a message
+void DefaultControllerTab::fillSetpointMessageHeader( d_fall_pps::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool DefaultControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 value of the "type" parameter into a local string variable
+ std::string type_string;
+ if(!nodeHandle.getParam("type", type_string))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
\ No newline at end of file
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 f3561d0d94ad22de9c1557a665fb2ebeedd51b08..9cb9fe7b0f9898cc097df7479d35161ffc8d48bf 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
@@ -13,25 +13,28 @@ StudentControllerTab::~StudentControllerTab()
delete ui;
}
-void StudentControllerTab::setMeasuredPose(QVector<float> measuredPose)
+void StudentControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
{
- // UPDATE THE MEASUREMENT COLUMN
- ui->lineEdit_measured_x ->setText(QString::number( measuredPose[0] ));
- ui->lineEdit_measured_y ->setText(QString::number( measuredPose[1] ));
- ui->lineEdit_measured_z ->setText(QString::number( measuredPose[2] ));
- ui->lineEdit_measured_roll ->setText(QString::number( measuredPose[3] ));
- ui->lineEdit_measured_pitch->setText(QString::number( measuredPose[4] ));
- ui->lineEdit_measured_yaw ->setText(QString::number( measuredPose[5] ));
+ if (!occluded)
+ {
+ // UPDATE THE MEASUREMENT COLUMN
+ ui->lineEdit_measured_x ->setText(QString::number( x ));
+ ui->lineEdit_measured_y ->setText(QString::number( y ));
+ ui->lineEdit_measured_z ->setText(QString::number( z ));
+ ui->lineEdit_measured_roll ->setText(QString::number( roll ));
+ ui->lineEdit_measured_pitch->setText(QString::number( pitch ));
+ ui->lineEdit_measured_yaw ->setText(QString::number( yaw ));
- // GET THE CURRENT SETPOINT
- float curr_x_setpoint = (ui->lineEdit_setpoint_current_x->text() ).toFloat();;
- float curr_y_setpoint = (ui->lineEdit_setpoint_current_y->text() ).toFloat();;
- float curr_z_setpoint = (ui->lineEdit_setpoint_current_z->text() ).toFloat();;
- float curr_yaw_setpoint = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();;
+ // GET THE CURRENT SETPOINT
+ float curr_x_setpoint = (ui->lineEdit_setpoint_current_x->text() ).toFloat();;
+ float curr_y_setpoint = (ui->lineEdit_setpoint_current_y->text() ).toFloat();;
+ float curr_z_setpoint = (ui->lineEdit_setpoint_current_z->text() ).toFloat();;
+ float curr_yaw_setpoint = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();;
- // UPDATE THE ERROR COLUMN
- ui->lineEdit_error_x ->setText(QString::number( measuredPose[0] - curr_x_setpoint ));
- ui->lineEdit_error_y ->setText(QString::number( measuredPose[1] - curr_y_setpoint ));
- ui->lineEdit_error_z ->setText(QString::number( measuredPose[2] - curr_z_setpoint ));
- ui->lineEdit_error_yaw->setText(QString::number( measuredPose[5] - curr_yaw_setpoint ));
+ // UPDATE THE ERROR COLUMN
+ ui->lineEdit_error_x ->setText(QString::number( x - curr_x_setpoint ));
+ ui->lineEdit_error_y ->setText(QString::number( y - curr_y_setpoint ));
+ ui->lineEdit_error_z ->setText(QString::number( z - curr_z_setpoint ));
+ ui->lineEdit_error_yaw->setText(QString::number( yaw - curr_yaw_setpoint ));
+ }
}