diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h
index 285a788cf615b237c87579fb06e546b06e7a0e5d..fbfd8c7136fc4a76228581b1e125fc0d25afc475 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h
@@ -1,10 +1,42 @@
#ifndef CONNECTSTARTSTOPBAR_H
#define CONNECTSTARTSTOPBAR_H
+#include <string>
#include <QWidget>
+#include <QMutex>
-//#include <QGraphicsSvgItem>
-//#include <QSvgRenderer>
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.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/AreaBounds.h"
+#include "d_fall_pps/CrazyflieContext.h"
+#include "d_fall_pps/CMQuery.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace d_fall_pps;
+#endif
+
+// BATTERY LABEL IMAGE INDEX
+#define BATTERY_LABEL_IMAGE_INDEX_UNVAILABLE -1
+#define BATTERY_LABEL_IMAGE_INDEX_EMPTY 0
+#define BATTERY_LABEL_IMAGE_INDEX_20 1
+#define BATTERY_LABEL_IMAGE_INDEX_40 2
+#define BATTERY_LABEL_IMAGE_INDEX_60 3
+#define BATTERY_LABEL_IMAGE_INDEX_80 4
+#define BATTERY_LABEL_IMAGE_INDEX_FULL 5
+#define BATTERY_LABEL_IMAGE_INDEX_UNKNOWN 6
namespace Ui {
class ConnectStartStopBar;
@@ -18,11 +50,122 @@ public:
explicit ConnectStartStopBar(QWidget *parent = 0);
~ConnectStartStopBar();
+private:
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+ Ui::ConnectStartStopBar *ui;
+
+ // > For the type of this node,
+ // i.e., an agent or a coordinator
+ int m_type = 0;
+
+ // > For the ID of this node
+ int m_ID = 0;
+
+ // The namespace into which node operates
+ std::string m_base_namespace;
+
+
+
+ // > For keeping track of the current battery state
+ int m_battery_state;
+ // > For keeping track of which image is currently displayed
+ int m_battery_status_label_image_current_index;
+
+
+
+ // MUTEX FOR HANDLING ACCESS
+ // > For the "rf_status_label" UI element
+ //QMutex m_rf_status_label_mutex;
+ // > For the "battery_voltage_lineEdit" UI element
+ //QMutex m_battery_voltage_lineEdit_mutex;
+ // > For the "battery_status_label" UI element
+ QMutex m_battery_status_label_mutex;
+
+
+
+#ifdef CATKIN_MAKE
+ // PUBLISHERS AND SUBSRIBERS
+ // > For Crazyradio commands based on button clicks
+ ros::Publisher crazyRadioCommandPublisher;
+ // > For updating the "rf_status_label" picture
+ ros::Subscriber crazyRadioStatusSubscriber;
+ // > For updating the current battery voltage
+ ros::Subscriber batteryVoltageSubscriber;
+ // > For updating the current battery state
+ //ros::Subscriber batteryStateSubscriber;
+ // > For updating the current battery level
+ ros::Subscriber batteryLevelSubscriber;
+ // > For Flying state commands based on button clicks
+ ros::Publisher flyingStateCommandPublisher;
+ // > For updating the "flying_state_label" picture
+ ros::Subscriber flyingStateSubscriber;
+#endif
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+ // > For making the "enable flight" and "disable flight"
+ // buttons (un-)available
+ void disableFlyingStateButtons();
+ void enableFlyingStateButtons();
+
+ // > For updating the RF Radio status shown in the
+ // UI element of "rf_status_label"
+ void setCrazyRadioStatus(int radio_status);
+ // > For updating the battery state
+ void setBatteryState(int new_battery_state);
+ // > For updating the battery voltage shown in the UI elements
+ // of "battery_voltage_lineEdit" and "battery_status_label"
+ void setBatteryVoltageText(float battery_voltage);
+ void setBatteryImageBasedOnLevel(int battery_level);
+ // > For updating the "my_flying_state" variable, and
+ // the UI element of "flying_state_label"
+ void setFlyingState(int new_flying_state);
+
+
+
+#ifdef CATKIN_MAKE
+ // Get the type and ID of this node
+ bool getTypeAndIDParameters();
+ // Fill the head for a message
+ void fillIntMessageHeader( d_fall_pps::IntWithHeader & msg );
+
+ // > For the CrazyRadio status, received on the
+ // "crazyRadioStatusSubscriber"
+ void crazyRadioStatusCallback(const std_msgs::Int32& msg);
+ // > For the Battery Voltage, received on the
+ // "batteryVoltageSubscriber"
+ void batteryVoltageCallback(const std_msgs::Float32& msg);
+ // > For the Battery State, receieved on the
+ // "batteryStateSubscriber"
+ void batteryStateChangedCallback(const std_msgs::Int32& msg);
+ // > For the Battery Level, receieved on the
+ // "batteryLevelSubscriber"
+ void batteryLevelCallback(const std_msgs::Int32& msg);
+ // > For the Flying State, received on the
+ // "flyingStateSubscriber"
+ void flyingStateChangedCallback(const std_msgs::Int32& msg);
+
+#endif
+
+
+
+
+
private slots:
+
+ // PRIVATE METHODS FOR BUTTON CALLBACKS
+ // > For the RF Crazyradio connect/disconnect buttons
+ void on_rf_connect_button_clicked();
void on_rf_disconnect_button_clicked();
+ // > For the enable, disable, motors-off buttons
+ void on_enable_flying_button_clicked();
+ void on_disable_flying_button_clicked();
+ void on_motors_off_button_clicked();
+
-private:
- Ui::ConnectStartStopBar *ui;
};
#endif // CONNECTSTARTSTOPBAR_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h
index 6fa61806d1b32c0c6e78a750d2e17d1ea4e09725..f80eb8ba8feaca692c1298a86665c768015e5f19 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h
@@ -53,62 +53,22 @@
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/CMQuery.h"
+// Include the shared definitions
+#include "nodes/Constants.h"
-
+// SPECIFY THE PACKAGE NAMESPACE
using namespace d_fall_pps;
#endif
-// TYPES OF CONTROLLER BEING USED
-#define SAFE_CONTROLLER 1
-#define DEMO_CONTROLLER 2
-#define STUDENT_CONTROLLER 3
-#define MPC_CONTROLLER 4
-#define REMOTE_CONTROLLER 5
-#define TUNING_CONTROLLER 6
-
-// COMMANDS FOR CRAZYRADIO
-#define CMD_RECONNECT 0
-#define CMD_DISCONNECT 1
-
-// CRAZYRADIO STATES
-#define CONNECTED 0
-#define CONNECTING 1
-#define DISCONNECTED 2
-
-// COMMANDS FOR THE FLYING STATE/CONTROLLER USED
-// The constants that "command" changes in the
-// operation state of this agent. These "commands"
-// are sent from this GUI node to the "PPSClient"
-// node where the command is enacted
-#define CMD_USE_SAFE_CONTROLLER 1
-#define CMD_USE_DEMO_CONTROLLER 2
-#define CMD_USE_STUDENT_CONTROLLER 3
-#define CMD_USE_MPC_CONTROLLER 4
-#define CMD_USE_REMOTE_CONTROLLER 5
-#define CMD_USE_TUNING_CONTROLLER 6
-
-#define CMD_CRAZYFLY_TAKE_OFF 11
-#define CMD_CRAZYFLY_LAND 12
-#define CMD_CRAZYFLY_MOTORS_OFF 13
-
-// FLYING STATES
-#define STATE_MOTORS_OFF 1
-#define STATE_TAKE_OFF 2
-#define STATE_FLYING 3
-#define STATE_LAND 4
-
-// BATTERY STATES
-#define BATTERY_STATE_NORMAL 0
-#define BATTERY_STATE_LOW 1
-
// BATTERY LABEL IMAGE INDEX
-#define BATTERY_LABEL_IMAGE_INDEX_EMPTY 0
-#define BATTERY_LABEL_IMAGE_INDEX_20 1
-#define BATTERY_LABEL_IMAGE_INDEX_40 2
-#define BATTERY_LABEL_IMAGE_INDEX_60 3
-#define BATTERY_LABEL_IMAGE_INDEX_80 4
-#define BATTERY_LABEL_IMAGE_INDEX_FULL 5
-#define BATTERY_LABEL_IMAGE_INDEX_UNKNOWN 6
+#define BATTERY_LABEL_IMAGE_INDEX_UNVAILABLE -1
+#define BATTERY_LABEL_IMAGE_INDEX_EMPTY 0
+#define BATTERY_LABEL_IMAGE_INDEX_20 1
+#define BATTERY_LABEL_IMAGE_INDEX_40 2
+#define BATTERY_LABEL_IMAGE_INDEX_60 3
+#define BATTERY_LABEL_IMAGE_INDEX_80 4
+#define BATTERY_LABEL_IMAGE_INDEX_FULL 5
+#define BATTERY_LABEL_IMAGE_INDEX_UNKNOWN 6
namespace Ui {
@@ -141,34 +101,21 @@ private:
// > For the name of the allocated Crazyflie
QString m_crazyflie_name_as_string;
- // > For keeping track of the current RF Crazyradio state
- int my_radio_status;
+
// > For keeping track of the current battery state
- int my_battery_state;
+ int m_battery_state;
// > For keeping track of which image is currently displayed
- int my_battery_status_label_image_current_index;
- // > For keeping track of the current operating state
- int my_flying_state;
+ int m_battery_status_label_image_current_index;
+
// MUTEX FOR HANDLING ACCESS
// > For the "rf_status_label" UI element
- QMutex my_rf_status_label_mutex;
- // > For the "my_battery_state" variable
- QMutex my_battery_state_mutex;
+ //QMutex m_rf_status_label_mutex;
// > For the "battery_voltage_lineEdit" UI element
- QMutex my_battery_voltage_lineEdit_mutex;
+ //QMutex m_battery_voltage_lineEdit_mutex;
// > For the "battery_status_label" UI element
- QMutex my_battery_status_label_mutex;
- // > For the "my_flying_state" variable
- QMutex my_flying_state_mutex;
+ QMutex m_battery_status_label_mutex;
- // BATTERY VOLTAGE LIMITS (THESE SHOULD BE READ IN AS PARAMTERS)
- // > When in a "standby" type of state
- float battery_voltage_standby_empty;
- float battery_voltage_standby_full;
- // > When in a "flying" type of state
- float battery_voltage_flying_empty;
- float battery_voltage_flying_full;
// --------------------------------------------------- //
@@ -179,11 +126,8 @@ private:
// > For updating the battery state
void setBatteryState(int new_battery_state);
// > For updating the battery voltage shown in the UI elements of "battery_voltage_lineEdit" and "battery_status_label"
- void setBatteryVoltageTextAndImage(float battery_voltage);
void setBatteryVoltageText(float battery_voltage);
- void setBatteryVoltageImage(float battery_voltage);
- // > For converting a voltage to a percentage, depending on the current "my_flying_state" value
- float fromVoltageToPercent(float voltage);
+ void setBatteryImageBasedOnLevel(int battery_level);
// > For making the "enable flight" and "disable flight" buttons (un-)available
void disableFlyingStateButtons();
void enableFlyingStateButtons();
@@ -217,7 +161,9 @@ private:
// > For updating the current battery voltage
ros::Subscriber batteryVoltageSubscriber;
// > For updating the current battery state
- ros::Subscriber batteryStateSubscriber;
+ //ros::Subscriber batteryStateSubscriber;
+ // > For updating the current battery level
+ ros::Subscriber batteryLevelSubscriber;
// > For Flying state commands based on button clicks
ros::Publisher flyingStateCommandPublisher;
// > For updating the "flying_state_label" picture
@@ -238,6 +184,8 @@ private:
void batteryVoltageCallback(const std_msgs::Float32& msg);
// > For the Battery State, receieved on the "batteryStateSubscriber"
void batteryStateChangedCallback(const std_msgs::Int32& msg);
+ // > For the Battery Level, receieved on the "batteryLevelSubscriber"
+ void batteryLevelCallback(const std_msgs::Int32& msg);
// > For the Flying State, received on the "flyingStateSubscriber"
void flyingStateChangedCallback(const std_msgs::Int32& msg);
// > For the notification that the database was changes, received on the "DatabaseChangedSubscriber"
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
index 1d757b1cd3018bb30f8f72f4965978cbbf48cee7..b42d1186f727df998d345dd45b2b2c83f58fcc04 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
@@ -70,9 +70,9 @@ private slots:
private:
Ui::EnableControllerLoadYamlBar *ui;
-#ifdef CATKIN_MAKE
+
// --------------------------------------------------- //
- // PRIVATE VARIABLES FOR ROS
+ // PRIVATE VARIABLES
// The type of this node, i.e., agent or a coordinator,
// specified as a parameter in the "*.launch" file
@@ -81,22 +81,24 @@ private:
// The ID of this node
int m_ID;
+#ifdef CATKIN_MAKE
// PUBLISHERS AND SUBSRIBERS
// > For {take-off,land,motors-off} and controller selection
ros::Publisher commandPublisher;
+#endif
// --------------------------------------------------- //
- // PRIVATE FUNCTIONS FOR ROS
+ // PRIVATE FUNCTIONS
+#ifdef CATKIN_MAKE
// Fill the head for a message
void fillIntMessageHeader( d_fall_pps::IntWithHeader & msg );
// Get the paramters that specify the type and ID
bool getTypeAndIDParameters();
+#endif
-
-#endif
};
#endif // ENABLECONTROLLERLOADYAMLBAR_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h
index 8281e4868c736196ea6b096cb5b3f814f75624cb..c9f071fa0ae89a3eaa73b97dc4fbbf01389c4c1f 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h
@@ -71,26 +71,22 @@ public:
explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
~MainWindow();
-private slots:
- void on_actionShowHide_Coordinator_triggered();
- void on_action_LoadYAML_BatteryMonitor_triggered();
- void on_action_LoadYAML_ClientConfig_triggered();
+
private:
Ui::MainWindow *ui;
QShortcut* m_close_GUI_shortcut;
-#ifdef CATKIN_MAKE
- rosNodeThread* m_rosNodeThread;
+ // > For the type of this node,
+ // i.e., an agent or a coordinator
+ int m_type = 0;
- // Variables for the type and ID
- // The type of this node, i.e., agent or a coordinator,
- // specififed as a parameter in the "*.launch" file
- int m_type = 0;
+ // > For the ID of this node
+ int m_ID = 0;
- // The ID of this node
- int m_ID = 0;
+#ifdef CATKIN_MAKE
+ rosNodeThread* m_rosNodeThread;
// The namespace into which this parameter service loads yaml parameters
std::string m_parameter_service_namespace;
@@ -98,6 +94,22 @@ private:
ros::Publisher m_requestLoadYamlFilenamePublisher;
#endif
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+#ifdef CATKIN_MAKE
+ bool getTypeAndIDParameters();
+#endif
+
+
+
+
+private slots:
+ // PRIVATE METHODS FOR MENU ITEM CALLBACKS
+ void on_actionShowHide_Coordinator_triggered();
+ void on_action_LoadYAML_BatteryMonitor_triggered();
+ void on_action_LoadYAML_ClientConfig_triggered();
+
};
#endif // MAINWINDOW_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
index a7ec4e9d8fe9430e8e7ffd5d8274a3d68d918db1..ab73056511d3374832b201d9b7b87854c13f714a 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
@@ -1,46 +1,141 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// The bar of the GUI for (dis-)connecting (from)to the Crazyradio
+// and for sending the {take-off,land,motors-off} commands
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
#include "connectstartstopbar.h"
#include "ui_connectstartstopbar.h"
+
+
+
+
ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
QWidget(parent),
ui(new Ui::ConnectStartStopBar)
{
ui->setupUi(this);
- // SET A FEW PROPERTIES OF THE UI ELEMENTS
- // > Default the battery voltage field to be "blank"
- QString qstr = "-.-- V";
- ui->battery_voltage_lineEdit->setText(qstr);
- // > Red font colour for the battery message label
- //ui->battery_message_label->setStyleSheet("QLabel { color : red; }");
- // > Default the battery message label to be "blank"
- //ui->battery_message_label->setText("");
+#ifdef CATKIN_MAKE
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CONNECT START STOP GUI BAR] ros::this_node::getNamespace() = " << this_namespace);
+
+
+ // Get the type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the TYPE and ID are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[CONNECT START STOP GUI BAR] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+#endif
+
+ // SET THE INITIAL VALUE OF THE PRIVATE VARIABLES FOR THIS CLASS
+ // > For keeping track of the current battery state
+ m_battery_state = BATTERY_STATE_NORMAL;
+ // > For keeping track of which image is currently displayed
+ m_battery_status_label_image_current_index = -999;
+
+
+ // SET THE STARTING RADIO STATUS TO BE: DISCONNECTED
+ setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+
+ // SET THE STARTING BATTERY VOLTAGE TO BE: UNKNOWN
+ setBatteryVoltageText(-1.0f);
+
+ // SET THE STARTING BATTERY LEVEL TO BE: UNAVAILABLE
+ setBatteryImageBasedOnLevel(BATTERY_LEVEL_UNAVAILABLE);
+
+ // SET THE STARTING FLYING STATE STATUS TO BE: MOTORS OFF
+ setFlyingState(CMD_CRAZYFLY_MOTORS_OFF);
+
+ // ENSURE THE F:YING STATE BUTTONS ARE AVAILABLE FOR A COORDINATOR
+ if (m_type == TYPE_COORDINATOR)
+ {
+ enableFlyingStateButtons();
+ }
- // SET THE DEFAULT IMAGE FOR THE RF CONNECTION STATUS
- QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
- ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
- ui->rf_status_label->setScaledContents(true);
- // SET THE DEFAULT IMAGE FOR THE BATTERY STATUS
- QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
- ui->battery_status_label->setPixmap(battery_unknown_pixmap);
- //ui->battery_status_label->setPixmap(battery_status_unknown_pixmap.scaled(ui->battery_status_label->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
- ui->battery_status_label->setScaledContents(true);
- // SET THE DEFAULT IMAGE FOR THE FLYING STATE
- QPixmap flying_state_unknown_pixmap(":/images/flying_state_unknown.png");
- ui->flying_state_label->setPixmap(flying_state_unknown_pixmap);
- ui->flying_state_label->setScaledContents(true);
+#ifdef CATKIN_MAKE
+ // CREATE A NODE HANDLE TO THE BASE NAMESPACE
+ ros::NodeHandle base_nodeHandle(this_namespace);
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle dfall_root_nodeHandle("/dfall");
+ // SUBSCRIBERS AND PUBLISHERS:
+ // > For Crazyradio commands based on button clicks
+ crazyRadioCommandPublisher = base_nodeHandle.advertise<d_fall_pps::IntWithHeader>("PPSClient/crazyRadioCommand", 1);
+
+ // > For Flying state commands based on button clicks
+ flyingStateCommandPublisher = base_nodeHandle.advertise<d_fall_pps::IntWithHeader>("PPSClient/Command", 1);
+
+ if (m_type == TYPE_AGENT)
+ {
+ // > For updating the "rf_status_label" picture
+ crazyRadioStatusSubscriber = base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &ConnectStartStopBar::crazyRadioStatusCallback, this);
+
+ // > For updating the current battery voltage
+ batteryVoltageSubscriber = base_nodeHandle.subscribe("BatteryMonitor/FilteredVoltage", 1, &ConnectStartStopBar::batteryVoltageCallback, this);
+
+ // > For updating the current battery state
+ //batteryStateSubscriber = base_nodeHandle.subscribe("BatteryMonitor/ChangedStateTo", 1, &ConnectStartStopBar::batteryStateChangedCallback, this);
+
+ // > For updating the current battery level
+ batteryLevelSubscriber = base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &ConnectStartStopBar::batteryLevelCallback, this);
+
+ // > For updating the "flying_state_label" picture
+ flyingStateSubscriber = base_nodeHandle.subscribe("PPSClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
+ }
+#endif
+
+ // FURTHER INITILIASATIONS NEED TO OCCUR AFTER THE ROS RELATED
+ // INITIALISATIONS ARE COMPLETE
+ if (m_type == TYPE_AGENT)
+ {
+ //loadCrazyflieContext();
+ }
// ADD KEYBOARD SHORTCUTS
// > For "all motors off", press the space bar
ui->motors_off_button->setShortcut(tr("Space"));
-
-
}
ConnectStartStopBar::~ConnectStartStopBar()
@@ -48,7 +143,554 @@ ConnectStartStopBar::~ConnectStartStopBar()
delete ui;
}
+
+
+// > For making the "enable flight" and "disable flight" buttons unavailable
+void ConnectStartStopBar::disableFlyingStateButtons()
+{
+ //ui->motors_off_button->setEnabled(true);
+ //ui->enable_flying_button->setEnabled(false);
+ //ui->disable_flying_button->setEnabled(false);
+}
+
+// > For making the "enable flight" and "disable flight" buttons available
+void ConnectStartStopBar::enableFlyingStateButtons()
+{
+ //ui->motors_off_button->setEnabled(true);
+ //ui->enable_flying_button->setEnabled(true);
+ //ui->disable_flying_button->setEnabled(true);
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC RRRR A ZZZZZ Y Y RRRR A DDDD III OOO
+// C R R A A Z Y Y R R A A D D I O O
+// C RRRR A A Z Y RRRR A A D D I O O
+// C R R AAAAA Z Y R R AAAAA D D I O O
+// CCCC R R A A ZZZZZ Y R R A A DDDD III OOO
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+// > For the Battery Voltage
+void ConnectStartStopBar::crazyRadioStatusCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Crazy Radio Status Callback called for agentID = " << m_agentID);
+ setCrazyRadioStatus( msg.data );
+}
+#endif
+
+
+// PRIVATE METHODS FOR SETTING PROPERTIES
+
+void ConnectStartStopBar::setCrazyRadioStatus(int new_radio_status)
+{
+ // add more things whenever the status is changed
+ switch(new_radio_status)
+ {
+ case CRAZY_RADIO_STATE_CONNECTED:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_connected_pixmap(":/images/rf_connected.png");
+ ui->rf_status_label->setPixmap(rf_connected_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+
+ // ENABLE THE REMAINDER OF THE GUI
+ if (m_type == TYPE_AGENT)
+ {
+ enableFlyingStateButtons();
+ }
+ break;
+ }
+
+ case CRAZY_RADIO_STATE_CONNECTING:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
+ ui->rf_status_label->setPixmap(rf_connecting_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+ break;
+ }
+
+ case CRAZY_RADIO_STATE_DISCONNECTED:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
+ ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+
+ // DISABLE THE REMAINDER OF THE GUI
+ if (m_type == TYPE_AGENT)
+ {
+ disableFlyingStateButtons();
+ }
+ break;
+ }
+
+ default:
+ {
+ break;
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+// > For the Battery Voltage
+void ConnectStartStopBar::batteryVoltageCallback(const std_msgs::Float32& msg)
+{
+ //setBatteryVoltageTextAndImage( msg.data );
+ setBatteryVoltageText( msg.data );
+}
+
+
+void ConnectStartStopBar::batteryStateChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Battery State Changed Callback called for agentID = " << m_agentID);
+ setBatteryState( msg.data );
+}
+
+
+void ConnectStartStopBar::batteryLevelCallback(const std_msgs::Int32& msg)
+{
+ setBatteryImageBasedOnLevel( msg.data );
+}
+#endif
+
+
+
+// PRIVATE METHODS FOR SETTING PROPERTIES
+
+// > For updating the battery state
+void ConnectStartStopBar::setBatteryState(int new_battery_state)
+{
+ // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
+ m_battery_state = new_battery_state;
+}
+
+
+
+// > For updating the battery voltage shown in the UI elements of "battery_voltage_lineEdit"
+void ConnectStartStopBar::setBatteryVoltageText(float battery_voltage)
+{
+ // Lock the mutex
+ //m_battery_voltage_lineEdit_mutex.lock();
+ // Construct the text string
+ QString qstr = "";
+ if (battery_voltage >= 0.0f)
+ {
+ qstr.append(QString::number(battery_voltage, 'f', 2));
+ }
+ else
+ {
+ qstr.append("-.--");
+ }
+ qstr.append(" V");
+ // Set the text to the battery voltage line edit
+ ui->battery_voltage_lineEdit->setText(qstr);
+ // Unlock the mutex
+ //m_battery_voltage_lineEdit_mutex.unlock();
+}
+
+
+
+// > For updating the battery image shown in the UI element of "battery_status_label"
+void ConnectStartStopBar::setBatteryImageBasedOnLevel(int battery_level)
+{
+ // COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
+ //float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);
+
+ // CONVERT THE VOLTAGE LEVEL TO AN INDEX OF WHICH BATTERY LEVEL IMAGE TO DISPLAY
+ // > Initialise a local variable that will be set in the switch case below
+ int new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
+ // > Initialise a local variable for the string of which image to use
+ QString qstr_new_image = "";
+ qstr_new_image.append(":/images/");
+
+ // Fill in these two local variables accordingly
+ switch(battery_level)
+ {
+ case BATTERY_LEVEL_000:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
+ qstr_new_image.append("battery_empty.png");
+ break;
+ }
+ case BATTERY_LEVEL_010:
+ case BATTERY_LEVEL_020:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_20;
+ qstr_new_image.append("battery_20.png");
+ break;
+ }
+ case BATTERY_LEVEL_030:
+ case BATTERY_LEVEL_040:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_40;
+ qstr_new_image.append("battery_40.png");
+ break;
+ }
+ case BATTERY_LEVEL_050:
+ case BATTERY_LEVEL_060:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_60;
+ qstr_new_image.append("battery_60.png");
+ break;
+ }
+ case BATTERY_LEVEL_070:
+ case BATTERY_LEVEL_080:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_80;
+ qstr_new_image.append("battery_80.png");
+ break;
+ }
+ case BATTERY_LEVEL_090:
+ case BATTERY_LEVEL_100:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_FULL;
+ qstr_new_image.append("battery_full.png");
+ break;
+ }
+ case BATTERY_LEVEL_UNAVAILABLE:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNVAILABLE;
+ qstr_new_image.append("battery_unknown.png");
+ break;
+ }
+ default:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
+ qstr_new_image.append("battery_unknown.png");
+ break;
+ }
+ }
+ // UPDATE THE IMAGE DISPLAYED BASED ON THE "new index"
+ // > Only if it is different from the current index
+ m_battery_status_label_mutex.lock();
+ if (m_battery_status_label_image_current_index != new_image_index)
+ {
+ // SET THE IMAGE FOR THE BATTERY STATUS LABEL
+ ui->battery_status_label->clear();
+ QPixmap battery_image_pixmap(qstr_new_image);
+ ui->battery_status_label->setPixmap(battery_image_pixmap);
+ ui->battery_status_label->setScaledContents(true);
+ m_battery_status_label_image_current_index = new_image_index;
+ ui->battery_status_label->update();
+ }
+ m_battery_status_label_mutex.unlock();
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF L Y Y III GGGG SSSS TTTTT A TTTTT EEEE
+// F L Y Y I G S T A A T E
+// FFF L Y I G SSS T A A T EEE
+// F L Y I G G S T AAAAA T E
+// F LLLLL Y III GGGG SSSS T A A T EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+// RESPONDING TO CHANGES IN THE FLYING STATE
+#ifdef CATKIN_MAKE
+void ConnectStartStopBar::flyingStateChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Flying State Changed Callback called for agentID = " << m_agentID);
+ setFlyingState(msg.data);
+}
+#endif
+
+void ConnectStartStopBar::setFlyingState(int new_flying_state)
+{
+ // UPDATE THE LABEL TO DISPLAY THE FLYING STATE
+ switch(new_flying_state)
+ {
+ case STATE_MOTORS_OFF:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_off_pixmap(":/images/flying_state_off.png");
+ ui->flying_state_label->setPixmap(flying_state_off_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_TAKE_OFF:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_enabling_pixmap(":/images/flying_state_enabling.png");
+ ui->flying_state_label->setPixmap(flying_state_enabling_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_FLYING:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_flying_pixmap(":/images/flying_state_flying.png");
+ ui->flying_state_label->setPixmap(flying_state_flying_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_LAND:
+ {
+ //qstr.append("Land");
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_disabling_pixmap(":/images/flying_state_disabling.png");
+ ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ default:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_unknown_pixmap(":/images/flying_state_unknown.png");
+ ui->flying_state_label->setPixmap(flying_state_unknown_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+
+void ConnectStartStopBar::on_rf_connect_button_clicked()
+{
+ d_fall_pps::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_RECONNECT;
+ this->crazyRadioCommandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Safe Controller");
+}
+
void ConnectStartStopBar::on_rf_disconnect_button_clicked()
{
+ d_fall_pps::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_DISCONNECT;
+ this->crazyRadioCommandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Safe Controller");
+}
+
+void ConnectStartStopBar::on_enable_flying_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ d_fall_pps::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_CRAZYFLY_TAKE_OFF;
+ this->flyingStateCommandPublisher.publish(msg);
+#endif
+}
+
+void ConnectStartStopBar::on_disable_flying_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ d_fall_pps::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_CRAZYFLY_LAND;
+ this->flyingStateCommandPublisher.publish(msg);
+#endif
+}
+
+void ConnectStartStopBar::on_motors_off_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ d_fall_pps::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ this->flyingStateCommandPublisher.publish(msg);
+#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 ConnectStartStopBar::fillIntMessageHeader( d_fall_pps::IntWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ msg.shouldCheckForID = true;
+ msg.agentIDs.push_back(7);
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForID = true;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] 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 ConnectStartStopBar::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("[CONNECT START STOP GUI BAR] 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("[CONNECT START STOP GUI BAR] 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("[CONNECT START STOP GUI BAR] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[CONNECT START STOP GUI BAR] 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("[CONNECT START STOP GUI BAR] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[CONNECT START STOP GUI BAR] 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("[CONNECT START STOP GUI BAR] 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/coordinator.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinator.cpp
index be2efe6665b1486ed99c132e98315801cbd19d8b..6c8419a13fd668eeb76e6379b434b047f4fe2297 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinator.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinator.cpp
@@ -94,12 +94,13 @@ void Coordinator::on_refresh_button_clicked()
vector_of_coordinatorRows.append(temp_coordinatorRow);
ui->coordinated_agents_scrollAreaWidgetContents->layout()->addWidget(temp_coordinatorRow);
-
- // Call the function that applies this level
- // of detail to all the entries
- apply_level_of_detail_to_all_entries(level_of_detail_to_display);
}
#endif
+
+
+ // Call the function that applies this level
+ // of detail to all the entries
+ apply_level_of_detail_to_all_entries(level_of_detail_to_display);
}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
index bffcaafee7e5527fa5ff193b22c0f506a2c31d10..31a0e1a4fb75eee3b897fcb6e1f748a2428d1ad1 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
@@ -30,9 +30,16 @@
// ----------------------------------------------------------------------------------
+
+
+
#include "coordinatorrow.h"
#include "ui_coordinatorrow.h"
+
+
+
+
CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
QWidget(parent),
ui(new Ui::CoordinatorRow),
@@ -60,32 +67,28 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
std::string ros_base_namespace = qstr_ros_base_namespace.toStdString();
// SET THE INITIAL VALUE OF THE PRIVATE VARIABLES FOR THIS CLASS
- // > For keeping track of the current RF Crazyradio state
- my_radio_status = CONNECTED;
// > For keeping track of the current battery state
- my_battery_state = BATTERY_STATE_NORMAL;
+ m_battery_state = BATTERY_STATE_NORMAL;
// > For keeping track of which image is currently displayed
- my_battery_status_label_image_current_index = -999;
- // > For keeping track of the current operating state
- my_flying_state = STATE_MOTORS_OFF;
-
- // FOR BATTERY VOLTAGE LIMITS (THESE SHOULD BE READ IN AS PARAMTERS)
- // > When in a "standby" type of state
- battery_voltage_standby_empty = 3.30f;
- battery_voltage_standby_full = 4.20f;
- // > When in a "flying" type of state
- battery_voltage_flying_empty = 2.60f;
- battery_voltage_flying_full = 3.70f;
+ m_battery_status_label_image_current_index = -999;
+
// SET THE STARTING RADIO STATUS TO BE: DISCONNECTED
- // > this also updates the image for the "rf_status_label", "battery_voltage_lineEdit", and "battery_status_label"
- setCrazyRadioStatus(DISCONNECTED);
+ setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+
+ // SET THE STARTING BATTERY VOLTAGE TO BE: UNKNOWN
+ setBatteryVoltageText(-1.0f);
+
+ // SET THE STARTING BATTERY LEVEL TO BE: UNAVAILABLE
+ setBatteryImageBasedOnLevel(BATTERY_LEVEL_UNAVAILABLE);
+
// SET THE STARTING FLYING STATE STATUS TO BE: MOTORS OFF
- // > this also updates the image for the "flying_state_label"
setFlyingState(CMD_CRAZYFLY_MOTORS_OFF);
+
// SET THE DEFAULT NAME FOR THE SELECTED CONTROLLER
setControllerEnabled(SAFE_CONTROLLER);
+
#ifdef CATKIN_MAKE
//m_rosNodeThread = new rosNodeThread(argc, argv, "coordinatorRowGUI");
//m_rosNodeThread->init();
@@ -103,11 +106,11 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
// LET THE USER KNOW WHAT THE BASE NAMESPACE IS
- ROS_INFO_STREAM("[Coordinator Row GUI] using base namespace: " << ros_base_namespace.c_str() << ", for agentID = " << m_agentID);
+ ROS_INFO_STREAM("[COORDINATOR ROW GUI] using base namespace: " << ros_base_namespace.c_str() << ", for agentID = " << m_agentID);
// DEBUGGING FOR NAMESPACES
//std::string temp_ros_namespace = ros::this_node::getNamespace();
- //ROS_INFO_STREAM("[Coordinator Row GUI] compared to: ros::this_node::getNamespace() = " << temp_ros_namespace.c_str());
+ //ROS_INFO_STREAM("[COORDINATOR ROW GUI] compared to: ros::this_node::getNamespace() = " << temp_ros_namespace.c_str());
// CREATE A NODE HANDLE TO THE BASE NAMESPACE
ros::NodeHandle base_nodeHandle(ros_base_namespace);
@@ -117,19 +120,21 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
// SUBSCRIBERS AND PUBLISHERS:
// > For Crazyradio commands based on button clicks
- crazyRadioCommandPublisher = base_nodeHandle.advertise<std_msgs::Int32>("PPSClient/crazyRadioCommand", 1);
+ crazyRadioCommandPublisher = base_nodeHandle.advertise<d_fall_pps::IntWithHeader>("PPSClient/crazyRadioCommand", 1);
// > For updating the "rf_status_label" picture
crazyRadioStatusSubscriber = base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &CoordinatorRow::crazyRadioStatusCallback, this);
// > For updating the current battery voltage
- batteryVoltageSubscriber = base_nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &CoordinatorRow::batteryVoltageCallback, this);
+ batteryVoltageSubscriber = base_nodeHandle.subscribe("BatteryMonitor/FilteredVoltage", 1, &CoordinatorRow::batteryVoltageCallback, this);
// > For updating the current battery state
- batteryStateSubscriber = base_nodeHandle.subscribe("PPSClient/batteryState", 1, &CoordinatorRow::batteryStateChangedCallback, this);
+ //batteryStateSubscriber = base_nodeHandle.subscribe("BatteryMonitor/ChangedStateTo", 1, &CoordinatorRow::batteryStateChangedCallback, this);
+ // > For updating the current battery level
+ batteryLevelSubscriber = base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &CoordinatorRow::batteryLevelCallback, this);
// > For Flying state commands based on button clicks
flyingStateCommandPublisher = base_nodeHandle.advertise<d_fall_pps::IntWithHeader>("PPSClient/Command", 1);
// > For updating the "flying_state_label" picture
flyingStateSubscriber = base_nodeHandle.subscribe("PPSClient/flyingState", 1, &CoordinatorRow::flyingStateChangedCallback, this);
// > For changes in the database that defines {agentID,cfID,flying zone} links
- databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("/my_GUI/DBChanged", 1, &CoordinatorRow::databaseChangedCallback, this);;
+ databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &CoordinatorRow::databaseChangedCallback, this);;
centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<CMQuery>("CentralManagerService/Query", false);
// > For updating the controller that is currently operating
controllerUsedSubscriber = base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this);
@@ -242,13 +247,23 @@ void CoordinatorRow::enableFlyingStateButtons()
+// ----------------------------------------------------------------------------------
+// CCCC RRRR A ZZZZZ Y Y RRRR A DDDD III OOO
+// C R R A A Z Y Y R R A A D D I O O
+// C RRRR A A Z Y RRRR A A D D I O O
+// C R R AAAAA Z Y R R AAAAA D D I O O
+// CCCC R R A A ZZZZZ Y R R A A DDDD III OOO
+// ----------------------------------------------------------------------------------
+
+
+
#ifdef CATKIN_MAKE
// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
// > For the Battery Voltage
void CoordinatorRow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Crazy Radio Status Callback called for agentID = " << m_agentID);
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Crazy Radio Status Callback called for agentID = " << m_agentID);
setCrazyRadioStatus( msg.data );
}
#endif
@@ -261,69 +276,46 @@ void CoordinatorRow::setCrazyRadioStatus(int new_radio_status)
// add more things whenever the status is changed
switch(new_radio_status)
{
- case CONNECTED:
+ case CRAZY_RADIO_STATE_CONNECTED:
{
// SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
- my_rf_status_label_mutex.lock();
+ //m_rf_status_label_mutex.lock();
//ui->rf_status_label->clear();
QPixmap rf_connected_pixmap(":/images/rf_connected.png");
ui->rf_status_label->setPixmap(rf_connected_pixmap);
ui->rf_status_label->setScaledContents(true);
//ui->rf_status_label->update();
- my_rf_status_label_mutex.unlock();
- // ENABLE THE REMAINDER OF THE GUI
- my_battery_state_mutex.lock();
- if (my_battery_state == BATTERY_STATE_NORMAL)
- {
- enableFlyingStateButtons();
- }
- my_battery_state_mutex.unlock();
+ //m_rf_status_label_mutex.unlock();
+ // ENABLE THE REMAINDER OF THE GUI
+ enableFlyingStateButtons();
break;
}
- case CONNECTING:
+ case CRAZY_RADIO_STATE_CONNECTING:
{
// SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
- my_rf_status_label_mutex.lock();
+ //m_rf_status_label_mutex.lock();
//ui->rf_status_label->clear();
QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
ui->rf_status_label->setPixmap(rf_connecting_pixmap);
ui->rf_status_label->setScaledContents(true);
//ui->rf_status_label->update();
- my_rf_status_label_mutex.unlock();
+ //m_rf_status_label_mutex.unlock();
break;
}
- case DISCONNECTED:
+ case CRAZY_RADIO_STATE_DISCONNECTED:
{
// SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
- my_rf_status_label_mutex.lock();
+ //m_rf_status_label_mutex.lock();
//ui->rf_status_label->clear();
QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
ui->rf_status_label->setScaledContents(true);
//ui->rf_status_label->update();
- my_rf_status_label_mutex.unlock();
- // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
- QString qstr = "-.-- V";
- my_battery_voltage_lineEdit_mutex.lock();
- ui->battery_voltage_lineEdit->setText(qstr);
- my_battery_voltage_lineEdit_mutex.unlock();
- // SET THE APPROPRIATE IMAGE FOR THE BATTERY STATUS LABEL
- // > Lock the mutex for accessing both "my_battery_status_label_image_current_index"
- // and "ui->battery_status_label"
- my_battery_status_label_mutex.lock();
- if (my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
- {
- ui->battery_status_label->clear();
- QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
- ui->battery_status_label->setPixmap(battery_unknown_pixmap);
- ui->battery_status_label->setScaledContents(true);
- my_battery_status_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
- ui->battery_status_label->update();
- }
- my_battery_status_label_mutex.unlock();
+ //m_rf_status_label_mutex.unlock();
+
// DISABLE THE REMAINDER OF THE GUI
disableFlyingStateButtons();
break;
@@ -334,26 +326,44 @@ void CoordinatorRow::setCrazyRadioStatus(int new_radio_status)
break;
}
}
- my_radio_status = new_radio_status;
}
+
+
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+
+
#ifdef CATKIN_MAKE
// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
// > For the Battery Voltage
void CoordinatorRow::batteryVoltageCallback(const std_msgs::Float32& msg)
{
- setBatteryVoltageTextAndImage( msg.data );
+ //setBatteryVoltageTextAndImage( msg.data );
+ setBatteryVoltageText( msg.data );
}
void CoordinatorRow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Battery State Changed Callback called for agentID = " << m_agentID);
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Battery State Changed Callback called for agentID = " << m_agentID);
setBatteryState( msg.data );
}
+
+
+void CoordinatorRow::batteryLevelCallback(const std_msgs::Int32& msg)
+{
+ setBatteryImageBasedOnLevel( msg.data );
+}
#endif
@@ -363,214 +373,133 @@ void CoordinatorRow::batteryStateChangedCallback(const std_msgs::Int32& msg)
// > For updating the battery state
void CoordinatorRow::setBatteryState(int new_battery_state)
{
- // LOCK THE MUTEX FOR THE WHOLE SWITCH CASE STATEMENT
- my_battery_state_mutex.lock();
- // Switch depending the the new battery state provided
- switch(new_battery_state)
- {
- case BATTERY_STATE_LOW:
- {
- // MAKE UNAVAILABLE THE BUTTONS FOR ENABLING AND DISABLING FLIGHT
- disableFlyingStateButtons();
-
- // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
- my_battery_state = BATTERY_STATE_LOW;
- break;
- }
-
- case BATTERY_STATE_NORMAL:
- {
- // MAKE UNAVAILABLE THE BUTTONS FOR ENABLING AND DISABLING FLIGHT
- enableFlyingStateButtons();
-
- // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
- my_battery_state = BATTERY_STATE_NORMAL;
- break;
- }
-
- default:
- break;
- }
- // UNLOCK THE MUTEX
- my_battery_state_mutex.unlock();
+ // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
+ m_battery_state = new_battery_state;
}
-// > For the battery voltage label and image
-void CoordinatorRow::setBatteryVoltageTextAndImage(float battery_voltage)
-{
- setBatteryVoltageText( battery_voltage );
- setBatteryVoltageImage( battery_voltage );
-}
+
// > For updating the battery voltage shown in the UI elements of "battery_voltage_lineEdit"
void CoordinatorRow::setBatteryVoltageText(float battery_voltage)
{
// Lock the mutex
- my_battery_voltage_lineEdit_mutex.lock();
+ //m_battery_voltage_lineEdit_mutex.lock();
// Construct the text string
QString qstr = "";
- qstr.append(QString::number(battery_voltage, 'f', 2));
+ if (battery_voltage >= 0.0f)
+ {
+ qstr.append(QString::number(battery_voltage, 'f', 2));
+ }
+ else
+ {
+ qstr.append("-.--");
+ }
qstr.append(" V");
// Set the text to the battery voltage line edit
ui->battery_voltage_lineEdit->setText(qstr);
// Unlock the mutex
- my_battery_voltage_lineEdit_mutex.unlock();
+ //m_battery_voltage_lineEdit_mutex.unlock();
}
-// > For updating the battery voltage shown in the UI elements of "battery_status_label"
-void CoordinatorRow::setBatteryVoltageImage(float battery_voltage)
+
+
+// > For updating the battery image shown in the UI element of "battery_status_label"
+void CoordinatorRow::setBatteryImageBasedOnLevel(int battery_level)
{
// COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
- float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);
+ //float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);
- // CONVERT THE VOLTAGE PERCENTAGE TO AN INDEX OF WHICH BATTERY LEVEL IMAGE TO DISPLAY
+ // CONVERT THE VOLTAGE LEVEL TO AN INDEX OF WHICH BATTERY LEVEL IMAGE TO DISPLAY
// > Initialise a local variable that will be set in the switch case below
- int new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
+ int new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
// > Initialise a local variable for the string of which image to use
QString qstr_new_image = "";
qstr_new_image.append(":/images/");
- // > Get the value of the "my_battery_state" variable into a local variable
- my_battery_state_mutex.lock();
- int local_copy_of_my_battery_state = my_battery_state;
- my_battery_state_mutex.unlock();
- // > Switch based on the current battery state, first locking the mutex for accessing
- // both "my_battery_status_label_image_current_index" and "ui->battery_status_label"
- my_battery_status_label_mutex.lock();
- switch(local_copy_of_my_battery_state)
+
+ // Fill in these two local variables accordingly
+ switch(battery_level)
{
- // WHEN THE BATTERY IS IN A LOW STATE
- case BATTERY_STATE_LOW:
+ case BATTERY_LEVEL_000:
{
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
qstr_new_image.append("battery_empty.png");
break;
}
-
- // WHEN THE BATTERY IS IN A NORMAL STATE
- case BATTERY_STATE_NORMAL:
+ case BATTERY_LEVEL_010:
+ case BATTERY_LEVEL_020:
{
-
- if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY) && (battery_voltage_percentage <= 0.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_EMPTY) && (battery_voltage_percentage <= 2.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
- qstr_new_image.append("battery_empty.png");
- }
- else if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20) && (battery_voltage_percentage <= 20.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_20) && (battery_voltage_percentage <= 22.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_20;
- qstr_new_image.append("battery_20.png");
- }
- else if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40) && (battery_voltage_percentage <= 40.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_40) && (battery_voltage_percentage <= 42.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_40;
- qstr_new_image.append("battery_40.png");
- }
- else if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60) && (battery_voltage_percentage <= 60.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_60) && (battery_voltage_percentage <= 62.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_60;
- qstr_new_image.append("battery_60.png");
- }
- else if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80) && (battery_voltage_percentage <= 80.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_80) && (battery_voltage_percentage <= 82.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_80;
- qstr_new_image.append("battery_80.png");
- }
- else
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_FULL;
- qstr_new_image.append("battery_full.png");
- }
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_20;
+ qstr_new_image.append("battery_20.png");
+ break;
+ }
+ case BATTERY_LEVEL_030:
+ case BATTERY_LEVEL_040:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_40;
+ qstr_new_image.append("battery_40.png");
+ break;
+ }
+ case BATTERY_LEVEL_050:
+ case BATTERY_LEVEL_060:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_60;
+ qstr_new_image.append("battery_60.png");
+ break;
+ }
+ case BATTERY_LEVEL_070:
+ case BATTERY_LEVEL_080:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_80;
+ qstr_new_image.append("battery_80.png");
+ break;
+ }
+ case BATTERY_LEVEL_090:
+ case BATTERY_LEVEL_100:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_FULL;
+ qstr_new_image.append("battery_full.png");
+ break;
+ }
+ case BATTERY_LEVEL_UNAVAILABLE:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNVAILABLE;
+ qstr_new_image.append("battery_unknown.png");
break;
}
-
default:
{
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
qstr_new_image.append("battery_unknown.png");
break;
}
}
// UPDATE THE IMAGE DISPLAYED BASED ON THE "new index"
// > Only if it is different from the current index
- if (my_battery_status_label_image_current_index != new_battery_label_image_index)
+ m_battery_status_label_mutex.lock();
+ if (m_battery_status_label_image_current_index != new_image_index)
{
// SET THE IMAGE FOR THE BATTERY STATUS LABEL
ui->battery_status_label->clear();
QPixmap battery_image_pixmap(qstr_new_image);
ui->battery_status_label->setPixmap(battery_image_pixmap);
ui->battery_status_label->setScaledContents(true);
- my_battery_status_label_image_current_index = new_battery_label_image_index;
+ m_battery_status_label_image_current_index = new_image_index;
ui->battery_status_label->update();
}
- // Finally unlock the mutex
- my_battery_status_label_mutex.unlock();
+ m_battery_status_label_mutex.unlock();
}
-// > For converting a voltage to a percentage, depending on the current "my_flying_state" value
-float CoordinatorRow::fromVoltageToPercent(float voltage)
-{
- // INITIALISE THE LOCAL VARIABLE FOR THE VOLTAGE WHEN FULL/EMPTY
- float voltage_when_full;
- float voltage_when_empty;
-
- // COMPUTE THE PERCENTAGE DIFFERENTLY DEPENDING ON
- // THE CURRENT FLYING STATE
- // > First lock the mutex before accessing the "my_flying_state" variable
- my_flying_state_mutex.lock();
- if (my_flying_state == STATE_MOTORS_OFF)
- {
- // Voltage limits for a "standby" type of state
- voltage_when_empty = battery_voltage_standby_empty;
- voltage_when_full = battery_voltage_standby_full;
- }
- else
- {
- // Voltage limits for a "flying" type of state
- voltage_when_empty = battery_voltage_flying_empty;
- voltage_when_full = battery_voltage_flying_full;
- }
- // > Unlock the mutex
- my_flying_state_mutex.unlock();
- // COMPUTE THE PERCENTAGE
- float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
- // CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
- // > This should not happen to often
- if(percentage > 100.0f)
- {
- percentage = 100.0f;
- }
- if(percentage < 0.0f)
- {
- percentage = 0.0f;
- }
-
- // RETURN THE PERCENTAGE
- return percentage;
-}
+// ----------------------------------------------------------------------------------
+// FFFFF L Y Y III GGGG SSSS TTTTT A TTTTT EEEE
+// F L Y Y I G S T A A T E
+// FFF L Y I G SSS T A A T EEE
+// F L Y I G G S T AAAAA T E
+// F LLLLL Y III GGGG SSSS T A A T EEEEE
+// ----------------------------------------------------------------------------------
@@ -578,18 +507,13 @@ float CoordinatorRow::fromVoltageToPercent(float voltage)
#ifdef CATKIN_MAKE
void CoordinatorRow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Flying State Changed Callback called for agentID = " << m_agentID);
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Flying State Changed Callback called for agentID = " << m_agentID);
setFlyingState(msg.data);
}
#endif
void CoordinatorRow::setFlyingState(int new_flying_state)
{
- // PUT THE CURRENT STATE INTO THE CLASS VARIABLE
- my_flying_state_mutex.lock();
- my_flying_state = new_flying_state;
- my_flying_state_mutex.unlock();
-
// UPDATE THE LABEL TO DISPLAY THE FLYING STATE
switch(new_flying_state)
{
@@ -642,12 +566,18 @@ void CoordinatorRow::setFlyingState(int new_flying_state)
}
+
+
+
+
+
+
// RESPONDING TO CHANGES IN THE DATABASE
#ifdef CATKIN_MAKE
// > For the notification that the database was changes, received on the "DatabaseChangedSubscriber"
void CoordinatorRow::databaseChangedCallback(const std_msgs::Int32& msg)
{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Database Changed Callback called for agentID = " << m_agentID);
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Database Changed Callback called for agentID = " << m_agentID);
loadCrazyflieContext();
}
#endif
@@ -666,13 +596,13 @@ void CoordinatorRow::loadCrazyflieContext()
if(centralManagerDatabaseService.call(contextCall))
{
my_context = contextCall.response.crazyflieContext;
- ROS_INFO_STREAM("[Coordinator Row GUI] CrazyflieContext:\n" << my_context);
+ ROS_INFO_STREAM("[COORDINATOR ROW GUI] CrazyflieContext:\n" << my_context);
qstr_crazyflie_name.append(QString::fromStdString(my_context.crazyflieName));
}
else
{
- ROS_ERROR_STREAM("[Coordinator Row GUI] Failed to load context for agentID = " << m_agentID);
+ ROS_ERROR_STREAM("[COORDINATOR ROW GUI] Failed to load context for agentID = " << m_agentID);
}
// This updating of the radio only needs to be done by the actual agent's node
//ros::NodeHandle nh("CrazyRadio");
@@ -698,7 +628,7 @@ void CoordinatorRow::loadCrazyflieContext()
// > For the controller currently operating, received on "controllerUsedSubscriber"
void CoordinatorRow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Controller Used Changed Callback called for agentID = " << m_agentID);
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Controller Used Changed Callback called for agentID = " << m_agentID);
setControllerEnabled(msg.data);
}
#endif
@@ -749,30 +679,39 @@ void CoordinatorRow::setControllerEnabled(int new_controller)
-// ------------------------------------------------------------------- //
-// # RF Crazyradio Connect Disconnect
+
+// ----------------------------------------------------------------------------------
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+
void CoordinatorRow::on_rf_connect_button_clicked()
{
#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
+ msg.shouldCheckForID = false;
msg.data = CMD_RECONNECT;
this->crazyRadioCommandPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Command to RF reconnect published");
+ ROS_INFO("[COORDINATOR ROW GUI] Command to RF reconnect published");
#endif
}
void CoordinatorRow::on_rf_disconnect_button_clicked()
{
#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
+ msg.shouldCheckForID = false;
msg.data = CMD_DISCONNECT;
this->crazyRadioCommandPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Command to RF disconnect published");
+ ROS_INFO("[COORDINATOR ROW GUI] Command to RF disconnect published");
#endif
}
-// ------------------------------------------------------------------- //
-// # Take off, land, motors off
void CoordinatorRow::on_enable_flying_button_clicked()
{
#ifdef CATKIN_MAKE
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 14df7d63bb4897b9ade22bc2479a68768b2aaef2..94f26a33f05a848fc823617263d0f2521e99e927 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
@@ -13,7 +13,7 @@ EnableControllerLoadYamlBar::EnableControllerLoadYamlBar(QWidget *parent) :
// Get the namespace of this node
std::string this_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[FLYING AGENT GUI] ros::this_node::getNamespace() = " << this_namespace);
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] ros::this_node::getNamespace() = " << this_namespace);
// Get the type and ID of this flying agent GUI
bool isValid_type_and_ID = getTypeAndIDParameters();
@@ -21,7 +21,7 @@ EnableControllerLoadYamlBar::EnableControllerLoadYamlBar(QWidget *parent) :
// Stall if the node IDs are not valid
if ( !isValid_type_and_ID )
{
- ROS_ERROR("[FLYING AGENT GUI] Node NOT FUNCTIONING :-)");
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Node NOT FUNCTIONING :-)");
ros::spin();
}
@@ -52,7 +52,7 @@ void EnableControllerLoadYamlBar::on_enable_safe_button_clicked()
fillIntMessageHeader(msg);
msg.data = CMD_USE_SAFE_CONTROLLER;
this->commandPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Enable Safe Controller");
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Safe Controller");
#endif
}
@@ -63,7 +63,7 @@ void EnableControllerLoadYamlBar::on_enable_demo_button_clicked()
fillIntMessageHeader(msg);
msg.data = CMD_USE_DEMO_CONTROLLER;
this->commandPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Enable Demo Controller");
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Demo Controller");
#endif
}
@@ -74,7 +74,7 @@ void EnableControllerLoadYamlBar::on_enable_student_button_clicked()
fillIntMessageHeader(msg);
msg.data = CMD_USE_STUDENT_CONTROLLER;
this->commandPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Enable Student Controller");
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Student Controller");
#endif
}
@@ -107,6 +107,19 @@ void EnableControllerLoadYamlBar::on_load_yaml_default_button_clicked()
}
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 EnableControllerLoadYamlBar::fillIntMessageHeader( d_fall_pps::IntWithHeader & msg )
@@ -128,7 +141,7 @@ void EnableControllerLoadYamlBar::fillIntMessageHeader( d_fall_pps::IntWithHeade
default:
{
msg.shouldCheckForID = true;
- ROS_ERROR("[FLYING AGENT GUI] The 'm_type' variable was not recognised.");
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
break;
}
}
@@ -136,6 +149,20 @@ void EnableControllerLoadYamlBar::fillIntMessageHeader( d_fall_pps::IntWithHeade
#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 EnableControllerLoadYamlBar::getTypeAndIDParameters()
{
@@ -151,7 +178,7 @@ bool EnableControllerLoadYamlBar::getTypeAndIDParameters()
if(!nodeHandle.getParam("type", type_string))
{
// Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("[FLYING AGENT GUI] Failed to get type");
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get type");
}
// Set the "m_type" class variable based on this string loaded
@@ -168,7 +195,7 @@ bool EnableControllerLoadYamlBar::getTypeAndIDParameters()
// Set "m_type" to the value indicating that it is invlid
m_type = TYPE_INVALID;
return_was_successful = false;
- ROS_ERROR("[FLYING AGENT GUI] The 'type' parameter retrieved was not recognised.");
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'type' parameter retrieved was not recognised.");
}
@@ -182,12 +209,12 @@ bool EnableControllerLoadYamlBar::getTypeAndIDParameters()
{
// Throw an error if the agent ID parameter could not be obtained
return_was_successful = false;
- ROS_ERROR("[FLYING AGENT GUI] Failed to get agentID");
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get agentID");
}
else
{
// Inform the user about the type and ID
- ROS_INFO_STREAM("[FLYING AGENT GUI] Is of type AGENT with ID = " << m_ID);
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] Is of type AGENT with ID = " << m_ID);
}
break;
}
@@ -201,12 +228,12 @@ bool EnableControllerLoadYamlBar::getTypeAndIDParameters()
{
// Throw an error if the coord ID parameter could not be obtained
return_was_successful = false;
- ROS_ERROR("[FLYING AGENT GUI] Failed to get coordID");
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get coordID");
}
else
{
// Inform the user about the type and ID
- ROS_INFO_STREAM("[FLYING AGENT GUI] Is of type COORDINATOR with ID = " << m_ID);
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] Is of type COORDINATOR with ID = " << m_ID);
}
break;
}
@@ -215,7 +242,7 @@ bool EnableControllerLoadYamlBar::getTypeAndIDParameters()
{
// Throw an error if the type is not recognised
return_was_successful = false;
- ROS_ERROR("[FLYING AGENT GUI] The 'm_type' variable was not recognised.");
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
break;
}
}
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 a0e08224aeb438f3af25dc529e22aaa3ff168e85..3d839967ad11558718ade9b284ba6d8cc5daeb81 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
@@ -51,89 +51,32 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
m_close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
#ifdef CATKIN_MAKE
- // 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("[FLYING AGENT GUI] Failed to get type");
- }
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[FLYING AGENT GUI MAIN WINDOW] ros::this_node::getNamespace() = " << this_namespace);
- // 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;
- ROS_ERROR("[FLYING AGENT GUI] The 'type' parameter retrieved was not recognised.");
- }
-
+ // Get the type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
- // Construct the string to the namespace of this Paramater Service
- switch (m_type)
+ // Stall if the TYPE and ID are not valid
+ if ( !isValid_type_and_ID )
{
- 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
- ROS_ERROR("[FLYING AGENT GUI] Failed to get agentID");
- }
- else
- {
- // Inform the user about the type and ID
- ROS_INFO_STREAM("[FLYING AGENT 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
- ROS_ERROR("[FLYING AGENT GUI] Failed to get coordID");
- }
- else
- {
- // Inform the user about the type and ID
- ROS_INFO_STREAM("[FLYING AGENT GUI] Is of type COORDINATOR with ID = " << m_ID);
- }
- break;
- }
-
- default:
- {
- // Throw an error if the type is not recognised
- ROS_ERROR("[FLYING AGENT GUI] The 'm_type' variable was not recognised.");
- break;
- }
+ ROS_ERROR("[FLYING AGENT GUI MAIN WINDOW] Node NOT FUNCTIONING :-)");
+ ros::spin();
}
-
-
+
+ // 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 "ParameterService" node
std::string this_node_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[FLYING AGENT GUI] ros::this_node::getNamespace() = " << this_node_namespace);
+ ROS_INFO_STREAM("[FLYING AGENT GUI MAIN WINDOW] ros::this_node::getNamespace() = " << this_node_namespace);
// Construct the string to the namespace of this Paramater Service
m_parameter_service_namespace = this_node_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("[FLYING AGENT GUI] parameter service is: " << m_parameter_service_namespace);
+ ROS_INFO_STREAM("[FLYING AGENT GUI MAIN WINDOW] parameter service is: " << m_parameter_service_namespace);
// Get the node handle to this parameter service
ros::NodeHandle nodeHandle_to_parameter_service(m_parameter_service_namespace);
@@ -180,7 +123,7 @@ void MainWindow::on_action_LoadYAML_BatteryMonitor_triggered()
{
#ifdef CATKIN_MAKE
// Inform the user that the menu item was selected
- ROS_INFO("[FLYING AGENT GUI] Load Battery Monitor YAML was clicked.");
+ ROS_INFO("[FLYING AGENT GUI MAIN WINDOW] Load Battery Monitor YAML was clicked.");
// Create a local variable for the message
StringWithHeader yaml_filename_msg;
@@ -198,7 +141,7 @@ void MainWindow::on_action_LoadYAML_ClientConfig_triggered()
{
#ifdef CATKIN_MAKE
// Inform the user that the menu item was selected
- ROS_INFO("[FLYING AGENT GUI] Load Client Config YAML was clicked.");
+ ROS_INFO("[FLYING AGENT GUI MAIN WINDOW] Load Client Config YAML was clicked.");
// Create a local variable for the message
StringWithHeader yaml_filename_msg;
@@ -210,3 +153,106 @@ void MainWindow::on_action_LoadYAML_ClientConfig_triggered()
m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
#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 MainWindow::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("[ENABLE CONTROLLER LOAD YAML GUI BAR] 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("[ENABLE CONTROLLER LOAD YAML GUI BAR] 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("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] 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("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] 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("[ENABLE CONTROLLER LOAD YAML GUI BAR] 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/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index d12f08d228b2ed21c062f748195791ab9cb6950d..13de624803cb0e2fdf109e6ac4e4e05109dd775f 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -146,7 +146,7 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED)
def change_status_to(self, new_status):
- print "status changed to: %s" % new_status
+ print "[CRAZY RADIO] status changed to: %s" % new_status
self._status = new_status
self.status_pub.publish(new_status)
@@ -160,13 +160,13 @@ class PPSRadioClient:
# update link from ros params
self.update_link_uri()
- print "Connecting to %s" % self.link_uri
+ print "[CRAZY RADIO] Connecting to %s" % self.link_uri
self.change_status_to(CONNECTING)
- rospy.loginfo("connecting...")
+ rospy.loginfo("[CRAZY RADIO] connecting...")
self._cf.open_link(self.link_uri)
def disconnect(self):
- print "Motors OFF"
+ print "[CRAZY RADIO] sending Motors OFF command before disconnecting"
self._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
msg = IntWithHeader()
@@ -174,7 +174,7 @@ class PPSRadioClient:
msg.data = CMD_CRAZYFLY_MOTORS_OFF
self.PPSClient_command_pub.publish(msg)
time.sleep(0.1)
- print "Disconnecting from %s" % self.link_uri
+ print "[CRAZY RADIO] Disconnecting from %s" % self.link_uri
self._cf.close_link()
self.change_status_to(DISCONNECTED)
@@ -200,7 +200,7 @@ class PPSRadioClient:
def _logging_error(self, logconf, msg):
- print "Error when logging %s" % logconf.name
+ print "[CRAZY RADIO] Error when logging %s" % logconf.name
# def _init_logging(self):
@@ -215,12 +215,12 @@ class PPSRadioClient:
if self.logconf.valid:
self.logconf.data_received_cb.add_callback(self._data_received_callback)
self.logconf.error_cb.add_callback(self._logging_error)
- print "logconf valid"
+ print "[CRAZY RADIO] logconf valid"
else:
- print "logconf invalid"
+ print "[CRAZY RADIO] logconf invalid"
self.logconf.start()
- print "logconf start"
+ print "[CRAZY RADIO] logconf start"
def _connected(self, link_uri):
"""
@@ -234,7 +234,7 @@ class PPSRadioClient:
msg.data = CMD_CRAZYFLY_MOTORS_OFF
cf_client.PPSClient_command_pub.publish(msg)
- rospy.loginfo("Connection to %s successful: " % link_uri)
+ rospy.loginfo("[CRAZY RADIO] Connection to %s successful: " % link_uri)
# Config for Logging
self._start_logging()
@@ -245,18 +245,18 @@ class PPSRadioClient:
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
self.change_status_to(DISCONNECTED)
- rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
+ rospy.logerr("[CRAZY RADIO] Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
self.change_status_to(DISCONNECTED)
- rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
+ rospy.logerr("[CRAZY RADIO] Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
self.change_status_to(DISCONNECTED)
- rospy.logwarn("Disconnected from %s" % link_uri)
+ rospy.logwarn("[CRAZY RADIO] Disconnected from %s" % link_uri)
# change state to motors OFF
msg = IntWithHeader()
@@ -295,23 +295,45 @@ class PPSRadioClient:
def crazyRadioCommandCallback(self, msg):
"""Callback to tell CrazyRadio to reconnect"""
- print "crazyRadio command received %s" % msg.data
-
- if msg.data == CMD_RECONNECT: # reconnect, check _status first and then do whatever needs to be done
- print "entered reconnect"
- print "_status: %s" % self._status
- if self.get_status() == DISCONNECTED:
- print "entered disconnected"
- self.connect()
- if self.get_status() == CONNECTED:
- self.status_pub.publish(CONNECTED)
-
- elif msg.data == CMD_DISCONNECT:
- print "disconnect received"
- if self.get_status() != DISCONNECTED: # what happens if we disconnect while we are in connecting state?
- self.disconnect()
- else:
- self.status_pub.publish(DISCONNECTED)
+
+ print "[CRAZY RADIO] DEBUGGING received command"
+
+ # Initialise a boolean flag that the command is NOT relevant
+ command_is_relevant = False
+
+ # Check the header details of the message for it relevance
+ if (!msg.shouldCheckForID):
+ command_is_relevant = True
+ else
+ for this_ID in msg.agentIDs:
+ if (this_ID == m_agentID):
+ command_is_relevant = True
+ break
+
+ # Only consider the command if it is relevant
+ if (command_is_relevant):
+ #print "[CRAZY RADIO] received command to: %s" % msg.data
+ if msg.data == CMD_RECONNECT:
+ if self.get_status() == DISCONNECTED:
+ print "[CRAZY RADIO] received command to CONNECT (current status is DISCONNECTED)"
+ self.connect()
+ elif self.get_status() == CONNECTING:
+ print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTING)"
+ #self.status_pub.publish(CONNECTING)
+ elif self.get_status() == CONNECTED:
+ print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTED)"
+ #self.status_pub.publish(CONNECTED)
+
+ elif msg.data == CMD_DISCONNECT:
+ if self.get_status() == CONNECTED:
+ print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTED)"
+ self.disconnect()
+ elif self.get_status() == CONNECTING:
+ print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTING)"
+ #self.status_pub.publish(CONNECTING)
+ elif self.get_status() == DISCONNECTED:
+ print "[CRAZY RADIO] received command to DISCONNECT (current status is DISCONNECTED)"
+ #self.status_pub.publish(DISCONNECTED)
def controlCommandCallback(data):
"""Callback for controller actions"""
@@ -333,12 +355,17 @@ def controlCommandCallback(data):
if __name__ == '__main__':
+
+ # Starting the ROS-node
global node_name
node_name = "CrazyRadio"
rospy.init_node(node_name, anonymous=True)
+ # Get the namespace of this node
global ros_namespace
ros_namespace = rospy.get_namespace()
+
+
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
@@ -349,23 +376,42 @@ if __name__ == '__main__':
#use this following two lines to connect without data from CentralManager
# radio_address = "radio://0/72/2M"
# rospy.loginfo("manual address loaded")
+
+ # Fetch the YAML paramter "battery polling period"
global battery_polling_period
battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
+ # Fetch the YAML paramter "agentID" and "coordID"
+ global m_agentID
+ m_agentID = rospy.get_param(ros_namespace + "/PPSClient/agentID")
+ coordID = rospy.get_param(ros_namespace + "/PPSClient/coordID")
+ # Convert the coordinator ID to a zero-padded string
+ coordID_as_string = format(coordID, '03')
+
+
+ # Initialise a publisher for the battery voltage
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
+ # Initialise a "PPSRadioClient" type variable that handles
+ # all communication over the CrazyRadio
global cf_client
-
cf_client = PPSRadioClient()
- rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
+
+ # Subscribe to the commands for when to (dis-)connect the
+ # CrazyRadio connection with the Crazyflie
+ # > For the radio commands from the PPSClient of this agent
+ rospy.Subscriber("PPSClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback)
+ # > For the radio command from the coordinator
+ rospy.Subscriber("/dfall/coord" + coordID_as_string + "/PPSClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback)
+
time.sleep(1.0)
rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
rospy.spin()
- rospy.loginfo("Turning off crazyflie")
+ rospy.loginfo("[CRAZY RADIO] Turning off crazyflie")
cf_client._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
@@ -378,7 +424,7 @@ if __name__ == '__main__':
bag.close()
- rospy.loginfo("bag closed")
+ rospy.loginfo("[CRAZY RADIO] bag closed")
cf_client._cf.close_link()
- rospy.loginfo("Link closed")
+ rospy.loginfo("[CRAZY RADIO] Link closed")
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
index 2ab71e041acdb8188504dfd78482777b99a1ca84..9793c7ef6e34ba44c77dc1424561f19521648801 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -260,12 +260,6 @@ void viconCallback(const ViconData& viconData);
-// > For the {dis/re}-connect command received from the coordinator
-//void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
-void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
-
-
-
void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index 868bb5319675b7e5aa21b83898488df1563fb275..ef9305a78e0f8c6f3ed12ce7ace69c45a0d4e626 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -24,7 +24,7 @@
output = "screen"
type = "CrazyRadio.py"
>
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
+ <rosparam command="load" file="$(find d_fall_pps)/param/BatteryMonitor.yaml" />
</node>
<!-- PPS CLIENT -->
@@ -34,7 +34,6 @@
output = "screen"
type = "PPSClient"
>
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
<param name="agentID" value="$(arg agentID)" />
<param name="coordID" value="$(arg coordID)" />
</node>
@@ -120,6 +119,11 @@
>
<param name="type" type="str" value="agent" />
<param name="agentID" value="$(arg agentID)" />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/ClientConfig.yaml"
+ ns = "PPSClient"
+ />
<rosparam
command = "load"
file = "$(find d_fall_pps)/param/BatteryMonitor.yaml"
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
index 0754860eead138793fef4e6c97fd30deb9097190..4642273215f40ef7a157c228800fc7a2a57d4624 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -416,7 +416,8 @@ void loadCrazyflieContext() {
ROS_INFO("[PPS CLIENT] CF is now different for this student. Disconnect and turn it off");
- std_msgs::Int32 msg;
+ IntWithHeader msg;
+ msg.shouldCheckForID = false;
msg.data = CMD_DISCONNECT;
crazyRadioCommandPublisher.publish(msg);
}
@@ -558,22 +559,6 @@ void safeSetPointCallback(const Setpoint& newSetpoint)
-void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg)
-{
- // The "msg" received can be directly published on the "crazyRadioCommandPublisher"
- // class variable because it is same format message
- // > NOTE: this may be inefficient to "just" pass on the message,
- // the intention is that it is more transparent that the "coordinator"
- // node requests all agents to (re/dis)-connect from, and the
- // individual agents pass this along to their respective radio node.
- crazyRadioCommandPublisher.publish(msg);
-}
-
-
-
-
-
-
@@ -1268,7 +1253,7 @@ int main(int argc, char* argv[])
//this topic lets us use the terminal to communicate with crazyRadio node.
- crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
+ crazyRadioCommandPublisher = nodeHandle.advertise<IntWithHeader>("crazyRadioCommand", 1);
// this topic will publish flying state whenever it changes.
flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
@@ -1302,9 +1287,6 @@ int main(int argc, char* argv[])
// crazyradio status. Connected, connecting or disconnected
ros::Subscriber crazyRadioStatusSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);
- // Subscriber for "crazyRadioCommandAllAgents" commands that are sent from the coordinator node
- ros::Subscriber crazyRadioCommandAllAgentsSubscriber = namespaceNodeHandle.subscribe("/my_GUI/crazyRadioCommandAllAgents", 1, crazyRadioCommandAllAgentsCallback);
-