diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 294a21b8aa42b139a7164a8b101cbd3160ee7249..8fb21f1ccab2c6c03e04bfd54b6bf222cee3b25c 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -325,8 +325,10 @@ add_executable(PPSClient src/nodes/PPSClient.cpp
add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
-add_executable(DemoControllerService src/nodes/DemoControllerService.cpp)
-add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
+add_executable(DemoControllerService src/nodes/DemoControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(StudentControllerService src/nodes/StudentControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
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 878b6a17e698da3418d798909a375a17fedc4a96..4b8d228471b22da9c9d09c8903a439b69c0e9d0e 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,3 +1,35 @@
+// Copyright (C) 2018, 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//
+// ----------------------------------------------------------------------------------
+
+
#ifndef CONNECTSTARTSTOPBAR_H
#define CONNECTSTARTSTOPBAR_H
@@ -74,9 +106,6 @@ private:
// > For the ID of this node
int m_ID = 0;
- // The namespace into which node operates
- std::string m_base_namespace;
-
// For coordinating multiple agents
std::vector<int> m_vector_of_agentIDs_toCoordinate;
bool m_shouldCoordinateAll = true;
@@ -99,27 +128,8 @@ private:
//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
@@ -129,22 +139,50 @@ private:
void disableFlyingStateButtons();
void enableFlyingStateButtons();
- // > For updating the RF Radio status shown in the
- // UI element of "rf_status_label"
+ // > 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"
+
+ // > For updating the "my_flying_state" variable, and the
+ // UI element of "flying_state_label"
void setFlyingState(int new_flying_state);
#ifdef CATKIN_MAKE
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES FOR ROS
+
+
+ // 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;
+
+
+ // --------------------------------------------------- //
+ // PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
// Get the type and ID of this node
bool getTypeAndIDParameters();
// Fill the head for a message
@@ -153,6 +191,7 @@ private:
// > 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);
@@ -162,6 +201,7 @@ private:
// > 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);
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 b7bb4eb6a85fcc191ab8ea60427e55cca5e51e26..6d0a39430f7938319aee48415ece472d6eedbf9d 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
@@ -57,7 +57,7 @@
#include "nodes/Constants.h"
// SPECIFY THE PACKAGE NAMESPACE
-using namespace d_fall_pps;
+//using namespace d_fall_pps;
#else
// Include the shared definitions
@@ -131,21 +131,32 @@ private:
// --------------------------------------------------- //
// PRIVATE FUNCTIONS
- // > For updating the RF Radio status shown in the UI element of "rf_status_label"
+ // > 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"
+ // > 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 making the "enable flight" and "disable flight" buttons (un-)available
- void disableFlyingStateButtons();
- void enableFlyingStateButtons();
- // > For updating the "my_flying_state" variable, and the UI element of "flying_state_label"
+
+ // > For updating the "my_flying_state" variable, and the
+ // UI element of "flying_state_label"
void setFlyingState(int new_flying_state);
- // > For loading the "context" for this agent, i.e., the {agentID,cfID,flying zone} tuple
+
+ // > For loading the "context" for this agent,
+ // i.e., the {agentID,cfID,flying zone} tuple
void loadCrazyflieContext();
- // > For updating the text in the UI element of "controller_enabled_label"
+
+ // > For updating the text in the UI element of
+ // "controller_enabled_label"
void setControllerEnabled(int new_controller);
@@ -154,30 +165,27 @@ private:
// --------------------------------------------------- //
// PRIVATE VARIABLES FOR ROS
- // > For running this is a ROS node thread
- //rosNodeThread* myrosNodeThread;
-
- // > For the namespace of this node
- std::string my_ros_namespace;
-
// > For the "context" of this agent
- CrazyflieContext my_context;
+ d_fall_pps::CrazyflieContext my_context;
// 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;
+
// > For changes in the database that defines {agentID,cfID,flying zone} links
ros::Subscriber databaseChangedSubscriber;
ros::ServiceClient centralManagerDatabaseService;
@@ -188,19 +196,30 @@ private:
// --------------------------------------------------- //
// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
- // > For the CrazyRadio status, received on the "crazyRadioStatusSubscriber"
+ // > For the CrazyRadio status, received on the
+ // "crazyRadioStatusSubscriber"
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
- // > For the Battery Voltage, received on the "batteryVoltageSubscriber"
+
+ // > For the Battery Voltage, received on the
+ // "batteryVoltageSubscriber"
void batteryVoltageCallback(const std_msgs::Float32& msg);
- // > For the Battery State, receieved on the "batteryStateSubscriber"
+ // > For the Battery State, receieved on the
+ // "batteryStateSubscriber"
void batteryStateChangedCallback(const std_msgs::Int32& msg);
- // > For the Battery Level, receieved on the "batteryLevelSubscriber"
+ // > For the Battery Level, receieved on the
+ // "batteryLevelSubscriber"
void batteryLevelCallback(const std_msgs::Int32& msg);
- // > For the Flying State, received on the "flyingStateSubscriber"
+
+ // > 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"
+
+ // > For the notification that the database was changes,
+ // received on the "DatabaseChangedSubscriber"
void databaseChangedCallback(const std_msgs::Int32& msg);
- // > For the controller currently operating, received on "controllerUsedSubscriber"
+
+ // > For the controller currently operating, received on
+ // "controllerUsedSubscriber"
void controllerUsedChangedCallback(const std_msgs::Int32& msg);
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 3df7edddfd6a8241c8000b0be7349aee8fbca0dc..d04e939bcf0a8c129eed6f35e95d57dfaa3b9717 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
@@ -12,10 +12,11 @@
// Include the standard message types
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
-//#include <std_msgs/String.h>
+#include <std_msgs/String.h>
// Include the DFALL message types
#include "d_fall_pps/IntWithHeader.h"
+#include "d_fall_pps/StringWithHeader.h"
// Include the DFALL service types
// #include "d_fall_pps/AreaBounds.h"
@@ -102,14 +103,18 @@ private:
// PUBLISHERS AND SUBSRIBERS
// > For {take-off,land,motors-off} and controller selection
ros::Publisher commandPublisher;
+ // > For requesting the loading of yaml files
+ ros::Publisher m_requestLoadYamlFilenamePublisher;
+
#endif
// --------------------------------------------------- //
// PRIVATE FUNCTIONS
#ifdef CATKIN_MAKE
- // Fill the head for a message
+ // Fill the header for a message
void fillIntMessageHeader( d_fall_pps::IntWithHeader & msg );
+ void fillStringMessageHeader( d_fall_pps::StringWithHeader & msg );
// Get the paramters that specify the type and ID
bool getTypeAndIDParameters();
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/topbanner.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/topbanner.h
index 684801fb3bf0efc91590f1421715ddda53862294..7a7b3e3a6bee98128746095365f92bb140cefb98 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/topbanner.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/topbanner.h
@@ -1,8 +1,67 @@
+// 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 title displayed at the top of the GUI
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
#ifndef TOPBANNER_H
#define TOPBANNER_H
#include <QWidget>
+#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
+
namespace Ui {
class TopBanner;
}
@@ -16,7 +75,57 @@ public:
~TopBanner();
private:
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
Ui::TopBanner *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;
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+ // > For loading the "context" for this agent,
+ // i.e., the {agentID,cfID,flying zone} tuple
+ void loadCrazyflieContext();
+
+
+
+ #ifdef CATKIN_MAKE
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES FOR ROS
+
+ // > For the "context" of this agent
+ d_fall_pps::CrazyflieContext my_context;
+
+ // PUBLISHERS AND SUBSRIBERS
+
+ // > For changes in the database that defines {agentID,cfID,flying zone} links
+ ros::Subscriber databaseChangedSubscriber;
+ ros::ServiceClient centralManagerDatabaseService;
+
+
+ // --------------------------------------------------- //
+ // PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+ // Get the type and ID of this node
+ bool getTypeAndIDParameters();
+
+ // > For the notification that the database was changes,
+ // received on the "DatabaseChangedSubscriber"
+ void databaseChangedCallback(const std_msgs::Int32& msg);
+
+
+#endif
};
#endif // TOPBANNER_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 1e842c83128cc839eae6c2acb0016535249b6fbc..f06c2875ca74a81dcc49b2c814ace81774dfccbd 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
@@ -50,8 +50,8 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
#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);
+ std::string base_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CONNECT START STOP GUI BAR] ros::this_node::getNamespace() = " << base_namespace);
// Get the type and ID of this parameter service
@@ -98,7 +98,7 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
#ifdef CATKIN_MAKE
// CREATE A NODE HANDLE TO THE BASE NAMESPACE
- ros::NodeHandle base_nodeHandle(this_namespace);
+ ros::NodeHandle base_nodeHandle(base_namespace);
// CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
ros::NodeHandle dfall_root_nodeHandle("/dfall");
@@ -132,10 +132,11 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
// FURTHER INITILIASATIONS NEED TO OCCUR AFTER THE ROS RELATED
// INITIALISATIONS ARE COMPLETE
- if (m_type == TYPE_AGENT)
- {
+ //if (m_type == TYPE_AGENT)
+ //{
+ // The loading of the "Context" is handled by the "topbanner"
//loadCrazyflieContext();
- }
+ //}
// ADD KEYBOARD SHORTCUTS
// > For "all motors off", press the space bar
@@ -152,17 +153,17 @@ ConnectStartStopBar::~ConnectStartStopBar()
// > 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);
+ 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);
+ ui->motors_off_button->setEnabled(true);
+ ui->enable_flying_button->setEnabled(true);
+ ui->disable_flying_button->setEnabled(true);
}
@@ -473,7 +474,6 @@ void ConnectStartStopBar::setFlyingState(int new_flying_state)
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);
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 fe437e64eb2940e5ed3f09222a43b4f8c4ef522a..da14cfc034e209038cee6723ebc1ceda509f00cb 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
@@ -62,9 +62,9 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
m_agentID_as_string = QString::number(m_agentID).rightJustified(3, '0');
// CONVERT THE AGENT ID TO A STRING FOR THE BASE NAMESPACE
- QString qstr_ros_base_namespace = "/dfall/agent";
- qstr_ros_base_namespace.append(m_agentID_as_string);
- std::string ros_base_namespace = qstr_ros_base_namespace.toStdString();
+ QString qstr_base_namespace = "/dfall/agent";
+ qstr_base_namespace.append(m_agentID_as_string);
+ std::string base_namespace = qstr_base_namespace.toStdString();
// SET THE INITIAL VALUE OF THE PRIVATE VARIABLES FOR THIS CLASS
// > For keeping track of the current battery state
@@ -90,52 +90,43 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
#ifdef CATKIN_MAKE
- //m_rosNodeThread = new rosNodeThread(argc, argv, "coordinatorRowGUI");
- //m_rosNodeThread->init();
-
- //m_ros_namespace = ros::this_node::getNamespace();
-
- //qRegisterMetaType<ptrToMessage>("ptrToMessage");
- //QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
-
- //ros::NodeHandle nodeHandle(m_ros_namespace);
-
- // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
- //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
- //ros::NodeHandle nh_PPSClient("PPSClient");
-
// 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 agentID:" << m_agentID << " GUI] using base namespace: " << 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());
// CREATE A NODE HANDLE TO THE BASE NAMESPACE
- ros::NodeHandle base_nodeHandle(ros_base_namespace);
+ ros::NodeHandle base_nodeHandle(base_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 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("BatteryMonitor/FilteredVoltage", 1, &CoordinatorRow::batteryVoltageCallback, this);
// > For updating the current battery state
//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("CentralManagerService/DBChanged", 1, &CoordinatorRow::databaseChangedCallback, this);;
- centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<CMQuery>("CentralManagerService/Query", false);
+ centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<d_fall_pps::CMQuery>("CentralManagerService/Query", false);
+
// > For updating the controller that is currently operating
controllerUsedSubscriber = base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this);
@@ -145,9 +136,6 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
// INITIALISATIONS ARE COMPLETE
loadCrazyflieContext();
- // FOR DEBUGGING:
- //ui->shouldCoordinate_checkBox->setText(m_agentID_as_string);
- //ui->shouldCoordinate_checkBox->setText(QString::fromStdString(base_namespace));
}
CoordinatorRow::~CoordinatorRow()
@@ -546,7 +534,6 @@ void CoordinatorRow::setFlyingState(int new_flying_state)
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);
@@ -569,6 +556,13 @@ void CoordinatorRow::setFlyingState(int new_flying_state)
+// ----------------------------------------------------------------------------------
+// CCCC OOO N N TTTTT EEEEE X X TTTTT
+// C O O NN N T E X X T
+// C O O N N N T EEE X T
+// C O O N NN T E X X T
+// CCCC OOO N N T EEEEE X X T
+// ----------------------------------------------------------------------------------
@@ -587,7 +581,7 @@ void CoordinatorRow::loadCrazyflieContext()
{
QString qstr_crazyflie_name = "";
#ifdef CATKIN_MAKE
- CMQuery contextCall;
+ d_fall_pps::CMQuery contextCall;
contextCall.request.studentID = m_agentID;
//ROS_INFO_STREAM("StudentID:" << m_agentID);
@@ -596,13 +590,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 agentID:" << m_agentID << " 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 agentID:" << m_agentID << " 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");
@@ -624,6 +618,18 @@ void CoordinatorRow::loadCrazyflieContext()
}
+
+
+// ----------------------------------------------------------------------------------
+// CCCC OOO N N TTTTT RRRR OOO L L EEEEE RRRR
+// C O O NN N T R R O O L L E R R
+// C O O N N N T RRRR O O L L EEE RRRR
+// C O O N NN T R R O O L L E R R
+// CCCC OOO N N T R R OOO LLLLL LLLLL EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
#ifdef CATKIN_MAKE
// > For the controller currently operating, received on "controllerUsedSubscriber"
void CoordinatorRow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
@@ -697,7 +703,7 @@ void CoordinatorRow::on_rf_connect_button_clicked()
msg.shouldCheckForID = false;
msg.data = CMD_RECONNECT;
this->crazyRadioCommandPublisher.publish(msg);
- ROS_INFO("[COORDINATOR ROW GUI] Command to RF reconnect published");
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Connect button clicked");
#endif
}
@@ -708,7 +714,7 @@ void CoordinatorRow::on_rf_disconnect_button_clicked()
msg.shouldCheckForID = false;
msg.data = CMD_DISCONNECT;
this->crazyRadioCommandPublisher.publish(msg);
- ROS_INFO("[COORDINATOR ROW GUI] Command to RF disconnect published");
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Disconnect button clicked");
#endif
}
@@ -719,6 +725,7 @@ void CoordinatorRow::on_enable_flying_button_clicked()
msg.shouldCheckForID = false;
msg.data = CMD_CRAZYFLY_TAKE_OFF;
this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Enable flying button clicked");
#endif
}
@@ -729,6 +736,7 @@ void CoordinatorRow::on_disable_flying_button_clicked()
msg.shouldCheckForID = false;
msg.data = CMD_CRAZYFLY_LAND;
this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Disable flying button clicked");
#endif
}
@@ -739,6 +747,7 @@ void CoordinatorRow::on_motors_off_button_clicked()
msg.shouldCheckForID = false;
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Motors-off button clicked");
#endif
}
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 6eaaa3cf0b7346f4180d3983a565942b3097ac5c..e0c4905436735e81cbee708b92115bad5c24daa4 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
@@ -31,6 +31,10 @@ EnableControllerLoadYamlBar::EnableControllerLoadYamlBar(QWidget *parent) :
// CREATE THE COMMAND PUBLISHER
commandPublisher = nodeHandle_for_this_gui.advertise<d_fall_pps::IntWithHeader>("PPSClient/Command", 1);
+
+ // CREATE THE REQUEST LOAD YAML FILE PUBLISHER
+ // Get the node handle to this parameter service
+ m_requestLoadYamlFilenamePublisher = nodeHandle_for_this_gui.advertise<d_fall_pps::StringWithHeader>("ParameterService/requestLoadYamlFilename", 1);
#endif
}
@@ -94,17 +98,41 @@ void EnableControllerLoadYamlBar::on_enable_default_button_clicked()
void EnableControllerLoadYamlBar::on_load_yaml_safe_button_clicked()
{
+#ifdef CATKIN_MAKE
+#endif
}
void EnableControllerLoadYamlBar::on_load_yaml_demo_button_clicked()
{
-
+#ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ d_fall_pps::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "DemoController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Demo Controller YAML was clicked.");
+#endif
}
void EnableControllerLoadYamlBar::on_load_yaml_student_button_clicked()
{
-
+#ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ d_fall_pps::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "StudentController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Student Controller YAML was clicked.");
+#endif
}
void EnableControllerLoadYamlBar::on_load_yaml_default_button_clicked()
@@ -215,6 +243,50 @@ void EnableControllerLoadYamlBar::fillIntMessageHeader( d_fall_pps::IntWithHeade
+#ifdef CATKIN_MAKE
+// Fill the head for a message
+void EnableControllerLoadYamlBar::fillStringMessageHeader( d_fall_pps::StringWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForID = !(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.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
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/topbanner.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
index 4c71863d655ad45dbeb0b04fe641ac24fc8bd7a7..771eed6b4a97df441f470654b8c62fecd0b73bd8 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
@@ -1,14 +1,295 @@
+// 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 title displayed at the top of the GUI
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
#include "topbanner.h"
#include "ui_topbanner.h"
+
+
+
+
TopBanner::TopBanner(QWidget *parent) :
QWidget(parent),
ui(new Ui::TopBanner)
{
ui->setupUi(this);
+
+
+#ifdef CATKIN_MAKE
+ // Get the namespace of this node
+ std::string base_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CONNECT START STOP GUI BAR] ros::this_node::getNamespace() = " << base_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();
+ }
+#else
+ // Default as a coordinator when compiling with QtCreator
+ m_type = TYPE_COORDINATOR;
+ m_ID = 1;
+#endif
+
+
+
+#ifdef CATKIN_MAKE
+ // CREATE A NODE HANDLE TO THE BASE NAMESPACE
+ //ros::NodeHandle base_nodeHandle(base_namespace);
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle dfall_root_nodeHandle("/dfall");
+
+ // SUBSCRIBER AND SERVICE:
+ if (m_type == TYPE_AGENT)
+ {
+ // > For changes in the database that defines {agentID,cfID,flying zone} links
+ databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);;
+ centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<d_fall_pps::CMQuery>("CentralManagerService/Query", false);
+ }
+#endif
+
+
+ // FURTHER INITILIASATIONS NEED TO OCCUR AFTER THE ROS RELATED
+ // INITIALISATIONS ARE COMPLETE
+ if (m_type == TYPE_AGENT)
+ {
+ loadCrazyflieContext();
+ }
+ else if (m_type == TYPE_COORDINATOR)
+ {
+ // Set the label appropriate for a cooridnator
+ QString qstr_title = "Flying Device GUI: for COORDINATOR ID ";
+ qstr_title.append( QString::number(m_ID) );
+ ui->top_banner_label->setText(qstr_title);
+ }
+ else
+ {
+ // Set the label to inform the user of the error
+ QString qstr_title = "Flying Device GUI: for UNKNOWN NODE TYPE";
+ ui->top_banner_label->setText(qstr_title);
+ }
+
}
TopBanner::~TopBanner()
{
delete ui;
}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC OOO N N TTTTT EEEEE X X TTTTT
+// C O O NN N T E X X T
+// C O O N N N T EEE X T
+// C O O N NN T E X X T
+// CCCC OOO N N T EEEEE X X T
+// ----------------------------------------------------------------------------------
+
+
+
+// RESPONDING TO CHANGES IN THE DATABASE
+#ifdef CATKIN_MAKE
+// > For the notification that the database was changes, received on the "DatabaseChangedSubscriber"
+void TopBanner::databaseChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Database Changed Callback called for agentID = " << m_agentID);
+ loadCrazyflieContext();
+}
+#endif
+
+// > For loading the "context" for this agent, i.e., the {agentID,cfID,flying zone} tuple
+void TopBanner::loadCrazyflieContext()
+{
+
+ QString qstr_crazyflie_name = "";
+
+ if (m_type == TYPE_AGENT)
+ {
+
+#ifdef CATKIN_MAKE
+ d_fall_pps::CMQuery contextCall;
+ contextCall.request.studentID = m_ID;
+ //ROS_INFO_STREAM("StudentID:" << m_agentID);
+
+ centralManagerDatabaseService.waitForExistence(ros::Duration(-1));
+
+ if(centralManagerDatabaseService.call(contextCall))
+ {
+ my_context = contextCall.response.crazyflieContext;
+ ROS_INFO_STREAM("[TOP BANNER GUI] CrazyflieContext:\n" << my_context);
+
+ qstr_crazyflie_name.append(QString::fromStdString(my_context.crazyflieName));
+ }
+ else
+ {
+ ROS_ERROR_STREAM("[TOP BANNER GUI] Failed to load context for agentID = " << m_ID);
+ }
+ // This updating of the radio only needs to be done by the actual agent's node
+ //ros::NodeHandle nh("CrazyRadio");
+ //nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
+#else
+ // Set the Crazyflie Name String to be a question mark
+ qstr_crazyflie_name.append("?");
+#endif
+
+ // Construct and set the string for the checkbox label
+ QString qstr_title = "Flying Device GUI: for AGENT ID ";
+ qstr_title.append( QString::number(m_ID) );
+ qstr_title.append(", connected to ");
+ qstr_title.append(qstr_crazyflie_name);
+ ui->top_banner_label->setText(qstr_title);
+
+ }
+ else
+ {
+#ifdef CATKIN_MAKE
+ ROS_ERROR("[TOP BANNER GUI] loadCrazyflieContext called by a node that is NOT of type AGENT.");
+#endif
+ qstr_crazyflie_name.append("??");
+ }
+
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 TopBanner::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/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 13de624803cb0e2fdf109e6ac4e4e05109dd775f..7b2850f4d4de476bbba5d21651dfdcb0dbc3dfca 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -296,15 +296,13 @@ class PPSRadioClient:
def crazyRadioCommandCallback(self, msg):
"""Callback to tell CrazyRadio to reconnect"""
- 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):
+ if (msg.shouldCheckForID == False):
command_is_relevant = True
- else
+ else:
for this_ID in msg.agentIDs:
if (this_ID == m_agentID):
command_is_relevant = True
@@ -398,6 +396,8 @@ if __name__ == '__main__':
global cf_client
cf_client = PPSRadioClient()
+ print "[CRAZY RADIO] DEBUG 2"
+
# 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
@@ -410,6 +410,7 @@ if __name__ == '__main__':
rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
+ print "[CRAZY RADIO] Node READY :-)"
rospy.spin()
rospy.loginfo("[CRAZY RADIO] Turning off crazyflie")
diff --git a/pps_ws/src/d_fall_pps/include/nodes/Constants.h b/pps_ws/src/d_fall_pps/include/nodes/Constants.h
index ac3ecb916be33a14eac4881c7415b5f3cec6cf16..2962a8b3a45e2f0b7fca0d0dfc1380b9ab884a5f 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/Constants.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/Constants.h
@@ -30,6 +30,9 @@
// ----------------------------------------------------------------------------------
+
+
+
// ----------------------------------------------------------------------------------
// U U
// U U
@@ -49,11 +52,54 @@
-// Types PPS firmware
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board the Crazyflie 2.0.
+// The following is a short description about each mode:
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+//
+// CF_COMMAND_TYPE_ANGLE
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
#define CF_COMMAND_TYPE_MOTORS 6
#define CF_COMMAND_TYPE_RATE 7
#define CF_COMMAND_TYPE_ANGLE 8
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
// Types of controllers being used:
#define SAFE_CONTROLLER 1
#define DEMO_CONTROLLER 2
diff --git a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
index 7d9f825411a1ebde9a4e217496246e43b9116445..e82252a98b83dde717b083de4355e85e09f6c4f2 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
@@ -50,7 +50,14 @@
#include "ros/ros.h"
#include <ros/package.h>
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
//the generated structs from the msg-files have to be included
+#include "d_fall_pps/IntWithHeader.h"
+#include "d_fall_pps/StringWithHeader.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
@@ -61,7 +68,18 @@
// Include the Parameter Service shared definitions
#include "nodes/Constants.h"
-#include <std_msgs/Int32.h>
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace d_fall_pps;
@@ -77,23 +95,33 @@
// These constants are defined to make the code more readable and adaptable.
-// These constants define the modes that can be used for controller the Crazyflie 2.0,
-// the constants defined here need to be in agreement with those defined in the
-// firmware running on the Crazyflie 2.0.
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board the Crazyflie 2.0.
// The following is a short description about each mode:
-// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors
-// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angular rates from the PID rate
-// controllers implemented in the Crazyflie 2.0 firmware.
-// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angles from the PID attitude
-// controllers implemented in the Crazyflie 2.0 firmware.
-#define CF_COMMAND_TYPE_MOTOR 6
-#define CF_COMMAND_TYPE_RATE 7
-#define CF_COMMAND_TYPE_ANGLE 8
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+//
+// CF_COMMAND_TYPE_ANGLE
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+//#define CF_COMMAND_TYPE_MOTORS 6
+//#define CF_COMMAND_TYPE_RATE 7
+//#define CF_COMMAND_TYPE_ANGLE 8
+
+
+
// These constants define the controller used for computing the response in the
@@ -122,6 +150,9 @@
#define CONTROLLER_MODE_ANGLE_RESPONSE_TEST 6
+
+
+
// These constants define the method used for estimating the Inertial
// frame state.
// All methods are run at all times, this flag indicates which estimate
@@ -145,10 +176,6 @@
#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
-// Namespacing the package
-using namespace d_fall_pps;
-
-
@@ -289,9 +316,9 @@ std::vector<float> PMKF_Kinf_for_angles (2,0.0);
// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
-std::string namespace_to_own_agent_parameter_service;
+std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
+std::string m_namespace_to_coordinator_parameter_service;
// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
@@ -317,7 +344,10 @@ bool shouldDisplayDebugInfo = false;
// POSITION
// The ID of this agent, i.e., the ID of this compute
-int my_agentID = 0;
+int m_agentID = 0;
+
+// The ID of this agent, i.e., the ID of this compute
+int m_coordID = 0;
// Boolean indicating whether the (x,y,z,yaw) of this agent should be published or not
// > The default behaviour is: do not publish,
@@ -429,14 +459,7 @@ void customCommandReceivedCallback(const CustomButton& commandReceived);
void publish_current_xyz_yaw(float x, float y, float z, float yaw);
// LOAD PARAMETERS
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void isReadyDemoControllerYamlCallback(const IntWithHeader & msg);
+void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
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 9793c7ef6e34ba44c77dc1424561f19521648801..c59b6c4ba7a1776550060eebd6b93bb808e66ff2 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -63,7 +63,7 @@
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/CMQuery.h"
-// Include the Parameter Service shared definitions
+// Include the shared definitions
#include "nodes/Constants.h"
// Include other classes
@@ -72,7 +72,12 @@
// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>
-#include "d_fall_pps/ControlCommand.h"
+
+
+
+
+// Namespacing the package
+using namespace d_fall_pps;
@@ -88,8 +93,7 @@
-// Namespacing the package
-using namespace d_fall_pps;
+
@@ -244,10 +248,16 @@ ros::Timer timer_land;
// > For the LOAD PARAMETERS
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
-void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
+//void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+//void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
+//void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
+// > For the LOADING of YAML PARAMETERS
+void isReadySafeControllerYamlCallback(const IntWithHeader & msg);
+void fetchSafeControllerYamlParameters(ros::NodeHandle& nodeHandle);
+
+void isReadyClientConfigYamlCallback(const IntWithHeader & msg);
+void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
@@ -291,17 +301,4 @@ int getControllerUsed();
//void setBatteryStateTo(int new_battery_state);
//float movingAverageBatteryFilter(float new_input);
//void CFBatteryCallback(const std_msgs::Float32& msg);
-void batteryMonitorStateChangedCallback(std_msgs::Int32 msg);
-
-
-// > For the LOADING of YAML PARAMETERS
-void isReadySafeControllerYamlCallback(const IntWithHeader & msg);
-void fetchSafeControllerYamlParameters(ros::NodeHandle& nodeHandle);
-
-void isReadyClientConfigYamlCallback(const IntWithHeader & msg);
-void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
-
-
-
-
-
+void batteryMonitorStateChangedCallback(std_msgs::Int32 msg);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
index 6ead6b7fdb980a1a65aeb0e25420b00646d007ff..343141f7b8e21c2537d7fe8f641bb5cb3bdb61f2 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
@@ -48,19 +48,35 @@
#include "ros/ros.h"
#include <std_msgs/String.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/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
-#include "d_fall_pps/LoadYamlFiles.h"
-#include <std_msgs/Int32.h>
// Include the shared definitions
#include "nodes/Constants.h"
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+
+
+
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+
// ----------------------------------------------------------------------------------
@@ -73,26 +89,36 @@
// These constants are defined to make the code more readable and adaptable.
-// The constants define the modes that can be used for controller the Crazyflie 2.0,
-// the constants defined here need to be in agreement with those defined in the
-// firmware running on the Crazyflie 2.0.
+
+// NOTE: these constants are already defined in the "Constant.h" header file
+// and are repeated here for convenience
+
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board the Crazyflie 2.0.
// The following is a short description about each mode:
-// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors
-// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angular rates from the PID rate
-// controllers implemented in the Crazyflie 2.0 firmware.
-// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angles from the PID attitude
-// controllers implemented in the Crazyflie 2.0 firmware.
-#define MOTOR_MODE 6
-#define RATE_MODE 7
-#define ANGLE_MODE 8
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+//
+// CF_COMMAND_TYPE_ANGLE
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+//#define CF_COMMAND_TYPE_MOTORS 6
+//#define CF_COMMAND_TYPE_RATE 7
+//#define CF_COMMAND_TYPE_ANGLE 8
+
-// Namespacing the package
-using namespace d_fall_pps;
diff --git a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
index 203680119d3b0bc19231e7caebfac5978a0defa6..8eea640a9f5e8a92d778757f8e86be50b117124e 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
@@ -50,7 +50,14 @@
#include "ros/ros.h"
#include <ros/package.h>
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
//the generated structs from the msg-files have to be included
+#include "d_fall_pps/IntWithHeader.h"
+#include "d_fall_pps/StringWithHeader.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
@@ -58,10 +65,21 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
-// Include the Parameter Service shared definitions
+// Include the shared definitions
#include "nodes/Constants.h"
-#include <std_msgs/Int32.h>
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace d_fall_pps;
@@ -77,38 +95,38 @@
// These constants are defined to make the code more readable and adaptable.
-// These constants define the modes that can be used for controller the Crazyflie 2.0,
-// the constants defined here need to be in agreement with those defined in the
-// firmware running on the Crazyflie 2.0.
-// The following is a short description about each mode:
-// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors
-// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angular rates from the PID rate
-// controllers implemented in the Crazyflie 2.0 firmware.
-// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angles from the PID attitude
-// controllers implemented in the Crazyflie 2.0 firmware.
-#define MOTOR_MODE 6
-#define RATE_MODE 7
-#define ANGLE_MODE 8
-
-// These constants define the controller used for computing the response in the
-// "calculateControlOutput" function
+
+
+// NOTE: these constants are already defined in the "Constant.h" header file
+// and are repeated here for convenience
+
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board the Crazyflie 2.0.
// The following is a short description about each mode:
-// LQR_RATE_MODE LQR controller based on the state vector:
-// [position,velocity,angles]
//
-// LQR_ANGLE_MODE LQR controller based on the state vector:
-// [position,velocity]
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
//
-#define LQR_RATE_MODE 1 // (DEFAULT)
-#define LQR_ANGLE_MODE 2
+// CF_COMMAND_TYPE_RATE
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+//
+// CF_COMMAND_TYPE_ANGLE
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+//#define CF_COMMAND_TYPE_MOTORS 6
+//#define CF_COMMAND_TYPE_RATE 7
+//#define CF_COMMAND_TYPE_ANGLE 8
+
+
-// Namespacing the package
-using namespace d_fall_pps;
@@ -122,36 +140,43 @@ using namespace d_fall_pps;
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
+
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+
// Variables for controller
-float cf_mass_in_grams = 25.0; // Crazyflie mass in grams
-std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
-float control_frequency = 200.0; // Frequency at which the controller is running
-float cf_weight_in_newtons = 0.0; // The weight of the Crazyflie in Newtons, i.e., mg
+float yaml_cf_mass_in_grams = 25.0; // Crazyflie mass in grams
+std::vector<float> yaml_motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
+float yaml_control_frequency = 200.0; // Frequency at which the controller is running
+float m_cf_weight_in_newtons = 0.0; // The weight of the Crazyflie in Newtons, i.e., mg
-float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
+float m_previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
-std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
+std::vector<float> m_setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
// The LQR Controller parameters for "LQR_RATE_MODE"
-std::vector<float> gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
-std::vector<float> gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
-std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
-std::vector<float> gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
+std::vector<float> m_gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
+std::vector<float> m_gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
+std::vector<float> m_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
+std::vector<float> m_gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
// ROS Publisher for debugging variables
-ros::Publisher debugPublisher;
-
-
-// Variable for the namespaces for the paramter services
-// > For the paramter service of this agent
-std::string namespace_to_own_agent_parameter_service;
-// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
+ros::Publisher m_debugPublisher;
-// The ID of this agent, i.e., the ID of this compute
-int my_agentID = 0;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
@@ -186,11 +211,12 @@ int my_agentID = 0;
// P R R OOO T OOO T Y P EEEEE SSSS
// ----------------------------------------------------------------------------------
-// These function prototypes are not strictly required for this code to complile, but
-// adding the function prototypes here means the the functions can be written below in
-// any order. If the function prototypes are not included then the function need to
-// written below in an order that ensure each function is implemented before it is
-// called from another function, hence why the "main" function is at the bottom.
+// These function prototypes are not strictly required for this code to
+// complile, but adding the function prototypes here means the the functions
+// can be written below in any order. If the function prototypes are not
+// included then the function need to written below in an order that ensure
+// each function is implemented before it is called from another function,
+// hence why the "main" function is at the bottom.
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
@@ -207,15 +233,21 @@ void setpointCallback(const Setpoint& newSetpoint);
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButton& commandReceived);
+
+// > For the LOADING of YAML PARAMETERS
+void isReadyStudentControllerYamlCallback(const IntWithHeader & msg);
+void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle);
+
+
// LOAD PARAMETERS
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParameters(ros::NodeHandle& nodeHandle);
-void processFetchedParameters();
+//float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+//void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+//int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+//void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+//int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+//bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+
+//void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+//void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+//void processFetchedParameters();
//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index ef9305a78e0f8c6f3ed12ce7ace69c45a0d4e626..942ee8ad6acc5346c868992d1256f505e60a5a1b 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -74,15 +74,6 @@
>
</node>
- <!-- MPC CONTROLLER -->
- <node
- pkg = "d_fall_pps"
- name = "MpcControllerService"
- output = "screen"
- type = "MpcControllerService"
- >
- </node>
-
<!-- REMOTE CONTROLLER -->
<node
pkg = "d_fall_pps"
@@ -134,21 +125,6 @@
file = "$(find d_fall_pps)/param/SafeController.yaml"
ns = "SafeController"
/>
- <rosparam
- command = "load"
- file = "$(find d_fall_pps)/param/DemoController.yaml"
- ns = "DemoController"
- />
- <rosparam
- command = "load"
- file = "$(find d_fall_pps)/param/StudentController.yaml"
- ns = "StudentController"
- />
- <rosparam
- command = "load"
- file = "$(find d_fall_pps)/param/MpcController.yaml"
- ns = "MpcController"
- />
<rosparam
command = "load"
file = "$(find d_fall_pps)/param/RemoteController.yaml"
diff --git a/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
index a4ffa5643672ff24d4c9256b4a8e584ef1b6db21..d8b8aebf42f812bc8e05ca6b8580d9868ab8f63c 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
@@ -113,22 +113,30 @@
// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
//
// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines" section at the
-// top of this file, they are:
-// CF_COMMAND_TYPE_MOTOR
-// CF_COMMAND_TYPE_RATE
-// CF_COMMAND_TYPE_ANGLE.
-// > With CF_COMMAND_TYPE_RATE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
-// specify the angular rate in [radians/second] that will be requested from the
-// PID controllers running in the Crazyflie 2.0 firmware.
-// > With CF_COMMAND_TYPE_RATE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
-// are the baseline motor commands requested from the Crazyflie, with the adjustment
-// for body rates being added on top of this in the firmware (i.e., as per the code
-// of the "distribute_power" function provided in exercise sheet 2).
-// > With CF_COMMAND_TYPE_RATE the axis convention for the roll, pitch, and yaw body rates returned
-// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
-// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
-// thrust). To assist, teh following is an ASCII art of this convention:
+// > The allowed values for "onboardControllerType" are in the "Defines"
+// section in the header file, they are:
+// - CF_COMMAND_TYPE_MOTORS
+// - CF_COMMAND_TYPE_RATE
+// - CF_COMMAND_TYPE_ANGLE
+//
+// > For completeing the class exercises it is only required to use
+// the CF_COMMAND_TYPE_RATE option.
+//
+// > For the CF_COMMAND_TYPE_RATE optoin:
+// 1) the ".roll", ".ptich", and ".yaw" properties of
+// "response.ControlCommand" specify the angular rate in
+// [radians/second] that will be requested from the PID controllers
+// running in the Crazyflie 2.0 firmware.
+// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// "response.ControlCommand" are the baseline motor commands
+// requested from the Crazyflie, with the adjustment for body rates
+// being added on top of this in the firmware (i.e., as per the
+// code of the "distribute_power" found in the firmware).
+// 3) the axis convention for the roll, pitch, and yaw body rates
+// returned in "response.ControlCommand" should use right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the following is an ASCII art of this convention.
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -459,7 +467,7 @@ void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
response.controlOutput.motorCmd4 = 0;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
break;
}
}
@@ -542,7 +550,7 @@ void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller
// Specify that this controller is a rate controller
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -633,7 +641,7 @@ void calculateControlOutput_viaLQRforActuators(float stateErrorBody[12], Control
// Specify that this controller is a rate controller
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -730,7 +738,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -816,7 +824,7 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -921,7 +929,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -1055,7 +1063,7 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Cont
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -1443,94 +1451,97 @@ void publish_current_xyz_yaw(float x, float y, float z, float yaw)
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+// This function DOES NOT NEED TO BE edited for successful completion
+// ofthe exercise
+void isReadyDemoControllerYamlCallback(const IntWithHeader & msg)
{
- // Extract from the "msg" for which controller the and from where to fetch the YAML
- // parameters
- int controller_to_fetch_yaml = msg.data;
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
- // Switch between fetching for the different controllers and from different locations
- switch(controller_to_fetch_yaml)
+ // Continue if the message is relevant
+ if (isRevelant)
{
-
- // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT:
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
{
- // Let the user know that this message was received
- ROS_INFO("[DEMO CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
- // Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
- // Call the function that fetches the parameters
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
- break;
- }
-
- // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR:
- {
- // Let the user know that this message was received
- ROS_INFO("[DEMO CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
- // Create a node handle to the parameter service running on the coordinator machine
- ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
- // Call the function that fetches the parameters
- fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
- break;
- }
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[DEMO CONTROLLER] Now fetching the DemoController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[DEMO CONTROLLER] Now fetching the DemoController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
- default:
- {
- // Let the user know that the command was not relevant
- //ROS_INFO("The DemoControllerService received the message that YAML parameters were (re-)loaded");
- //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
- break;
+ default:
+ {
+ ROS_ERROR("[STUDENT CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
}
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchDemoControllerYamlParameters(nodeHandle_to_use);
}
}
+
// This function CAN BE edited for successful completion of the PPS exercise, and the
// use of parameters fetched from the YAML file is highly recommended to make tuning of
// your controller easier and quicker.
-void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle)
{
- // Here we load the parameters that are specified in the DemoController.yaml file
+ // Here we load the parameters that are specified in the file:
+ // DemoController.yaml file
// Add the "DemoController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_demoController(nodeHandle, "DemoController");
+ ros::NodeHandle nodeHandle_for_paraMaters(nodeHandle, "DemoController");
// > The mass of the crazyflie
- cf_mass = getParameterFloat(nodeHandle_for_demoController , "mass");
+ cf_mass = getParameterFloat(nodeHandle_for_paraMaters , "mass");
// Display one of the YAML parameters to debug if it is working correctly
//ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- vicon_frequency = getParameterFloat(nodeHandle_for_demoController, "vicon_frequency");
+ vicon_frequency = getParameterFloat(nodeHandle_for_paraMaters, "vicon_frequency");
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- control_frequency = getParameterFloat(nodeHandle_for_demoController, "control_frequency");
+ control_frequency = getParameterFloat(nodeHandle_for_paraMaters, "control_frequency");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_demoController, "motorPoly", motorPoly, 3);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
- shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_demoController, "shouldPerformConvertIntoBodyFrame");
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paraMaters, "shouldPerformConvertIntoBodyFrame");
// > The boolean indicating whether the (x,y,z,yaw) of this agent should be published
// or not
- shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_demoController, "shouldPublishCurrent_xyz_yaw");
+ shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_paraMaters, "shouldPublishCurrent_xyz_yaw");
// > The boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
// an attempt to follow the "my_current_xyz_yaw_topic" from another agent
- shouldFollowAnotherAgent = getParameterBool(nodeHandle_for_demoController, "shouldFollowAnotherAgent");
+ shouldFollowAnotherAgent = getParameterBool(nodeHandle_for_paraMaters, "shouldFollowAnotherAgent");
// > The ordered vector for ID's that specifies how the agents should follow eachother
follow_in_a_line_agentIDs.clear();
- int temp_number_of_agents_in_a_line = getParameterIntVectorWithUnknownLength(nodeHandle_for_demoController, "follow_in_a_line_agentIDs", follow_in_a_line_agentIDs);
+ int temp_number_of_agents_in_a_line = getParameterIntVectorWithUnknownLength(nodeHandle_for_paraMaters, "follow_in_a_line_agentIDs", follow_in_a_line_agentIDs);
// > Double check that the sizes agree
if ( temp_number_of_agents_in_a_line != follow_in_a_line_agentIDs.size() )
{
@@ -1539,70 +1550,70 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
}
// Boolean indiciating whether the "Debug Message" of this agent should be published or not
- shouldPublishDebugMessage = getParameterBool(nodeHandle_for_demoController, "shouldPublishDebugMessage");
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paraMaters, "shouldPublishDebugMessage");
// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
- shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_demoController, "shouldDisplayDebugInfo");
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paraMaters, "shouldDisplayDebugInfo");
// THE CONTROLLER GAINS:
// A flag for which controller to use:
- controller_mode = getParameterInt( nodeHandle_for_demoController , "controller_mode" );
+ controller_mode = getParameterInt( nodeHandle_for_paraMaters , "controller_mode" );
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_demoController , "estimator_method" );
+ estimator_method = getParameterInt( nodeHandle_for_paraMaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_MOTOR"
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor1", gainMatrixMotor1, 12);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor2", gainMatrixMotor2, 12);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor3", gainMatrixMotor3, 12);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor4", gainMatrixMotor4, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor1", gainMatrixMotor1, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor2", gainMatrixMotor2, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor3", gainMatrixMotor3, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor4", gainMatrixMotor4, 12);
// The LQR Controller parameters for "LQR_MODE_MOTOR"
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// The parameters for the "Angle Reponse Test" controller mode
- angleResponseTest_pitchAngle_degrees = getParameterFloat(nodeHandle_for_demoController, "angleResponseTest_pitchAngle_degrees");
- angleResponseTest_distance_meters = getParameterFloat(nodeHandle_for_demoController, "angleResponseTest_distance_meters");
+ angleResponseTest_pitchAngle_degrees = getParameterFloat(nodeHandle_for_paraMaters, "angleResponseTest_pitchAngle_degrees");
+ angleResponseTest_distance_meters = getParameterFloat(nodeHandle_for_paraMaters, "angleResponseTest_distance_meters");
// 16-BIT COMMAND LIMITS
- cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_demoController, "command_sixteenbit_min");
- cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_demoController, "command_sixteenbit_max");
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_paraMaters, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_paraMaters, "command_sixteenbit_max");
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
@@ -1646,7 +1657,7 @@ void processFetchedParameters()
for ( int i=0 ; i<follow_in_a_line_agentIDs.size() ; i++ )
{
// Check if the current entry matches the ID of this agent
- if (follow_in_a_line_agentIDs[i] == my_agentID)
+ if (follow_in_a_line_agentIDs[i] == m_agentID)
{
// Set the boolean flag that we have found out own agent ID
foundMyAgentID = true;
@@ -1681,109 +1692,38 @@ void processFetchedParameters()
// Check whether we found our own agent ID
if (!foundMyAgentID)
{
- //ROS_INFO("DEBUGGING: not following because my ID was not found");
- // Revert to the default of not following any agent
- shouldFollowAnotherAgent = false;
- agentID_to_follow = 0;
- }
- }
- else
- {
- // As the "follow_in_a_line_agentIDs" vector has length zero, revert to the
- // default of not following any agent
- shouldFollowAnotherAgent = false;
- agentID_to_follow = 0;
- //ROS_INFO("DEBUGGING: not following because line vector has length zero");
- }
- }
- else
- {
- // Set to its default value the integer specifying the ID of the agent that will
- // be followed by this agent
+ //ROS_INFO("DEBUGGING: not following because my ID was not found");
+ // Revert to the default of not following any agent
+ shouldFollowAnotherAgent = false;
+ agentID_to_follow = 0;
+ }
+ }
+ else
+ {
+ // As the "follow_in_a_line_agentIDs" vector has length zero, revert to the
+ // default of not following any agent
+ shouldFollowAnotherAgent = false;
+ agentID_to_follow = 0;
+ //ROS_INFO("DEBUGGING: not following because line vector has length zero");
+ }
+ }
+ else
+ {
+ // Set to its default value the integer specifying the ID of the agent that will
+ // be followed by this agent
agentID_to_follow = 0;
//ROS_INFO("DEBUGGING: not following because I was asked not to follow");
- }
+ }
- if (shouldFollowAnotherAgent)
- {
- ROS_INFO_STREAM("[DEMO CONTROLLER] This agent (with ID " << my_agentID << ") will now follow agent ID " << agentID_to_follow );
- }
+ if (shouldFollowAnotherAgent)
+ {
+ ROS_INFO_STREAM("[DEMO CONTROLLER] This agent (with ID " << m_agentID << ") will now follow agent ID " << agentID_to_follow );
+ }
}
-// ----------------------------------------------------------------------------------
-// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
-// G E T P P A A R R A A MM MM ( )
-// G EEE T PPPP A A RRRR A A M M M ( )
-// G G E T P AAAAA R R AAAAA M M ( )
-// GGGG EEEEE T P A A R R A A M M ( )
-// ----------------------------------------------------------------------------------
-
-
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
-{
- float val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
-{
- int val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val.size();
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
-{
- bool val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-
-
-
-
// ----------------------------------------------------------------------------------
@@ -1796,143 +1736,181 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
int main(int argc, char* argv[]) {
-
- // Starting the ROS-node
- ros::init(argc, argv, "DemoControllerService");
-
- // 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 "DemoControllerService" node
- std::string m_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[DEMO CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
-
- // Get the agent ID as the "ROS_NAMESPACE" this computer.
- // NOTES:
- // > If you look at the "Student.launch" file in the "launch" folder, you will see
- // the following line of code:
- // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
- // This line of code adds a parameter named "studentID" to the "PPSClient"
- // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
- // to the "PPSClient" node within which this controller service is nested.
-
- // Get the handle to the "PPSClient" node
- ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
- // Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
- {
- // Throw an error if the student ID parameter could not be obtained
- ROS_ERROR("[DEMO CONTROLLER] Failed to get agentID from PPSClient");
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "DemoControllerService");
+
+ // 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 "DemoControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[DEMO CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "PPSClient" node.
+ // > Thus, to get access to this "studentID" paremeter, we first
+ // need to get a handle to the "PPSClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/PPSClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[DEMO CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[DEMO CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
}
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
- // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
- // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
- // namespace string for the parameter service that is running on the machine of this
- // agent
- namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
- // Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[DEMO CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[DEMO CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
- // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
- // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
- // and calls the class function "yamlReadyForFetchCallback" each time a message is
- // received on this topic and the message is passed as an input argument to the
- // "yamlReadyForFetchCallback" class function.
- ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
- // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
- // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
- // for the parameter service that is running on the coordinate machine
- // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
- // is very important because it specifies that the name is global
- namespace_to_coordinator_parameter_service = "/ParameterService";
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
- // Create a node handle to the parameter service running on the coordinator machine
- ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
- //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
-
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber demoContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "DemoController", 1, isReadyDemoControllerYamlCallback);
+ ros::Subscriber demoContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("DemoController", 1, isReadyDemoControllerYamlCallback);
- // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
- // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
- // and calls the class function "yamlReadyForFetchCallback" each time a message is
- // received on this topic and the message is passed as an input argument to the
- // "yamlReadyForFetchCallback" class function.
- ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
- //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
- // PRINT OUT SOME INFORMATION
+ // GIVE YAML VARIABLES AN INITIAL VALUE
- // Let the user know what namespaces are being used for linking to the parameter service
- ROS_INFO_STREAM("[DEMO CONTROLLER] the namespace strings for accessing the Paramter Services are:");
- ROS_INFO_STREAM("[DEMO CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("[DEMO CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+ // This can be done either here or as part of declaring the variable
+ // in the header file
+ //yaml_cf_mass_in_grams = 25.0;
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to "Parametr
+ // Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // message containing the filename of the *.yaml file, and
+ // then a message will be received on the above subscribers
+ // when the paramters are ready.
+
+ // Create a publisher for request the yaml load
+ // > created as a local variable
+ // > note importantly that the final argument is "true", this
+ // enables "latching" on the connection. When a connection is
+ // latched, the last message published is saved and
+ // automatically sent to any future subscribers that connect.
+ ros::Publisher requestLoadYamlFilenamePublisher = nodeHandle_to_own_agent_parameter_service.advertise<StringWithHeader>("requestLoadYamlFilename", 1, true);
+ // Create a local variable for the message
+ StringWithHeader yaml_filename_msg;
+ // Specify the data
+ yaml_filename_msg.data = "DemoController";
+ // Set for whom this applies to
+ yaml_filename_msg.shouldCheckForID = false;
+ // Sleep to make the publisher known to ROS (in seconds)
+ //ros::Duration(1.0).sleep();
+ // Send the message
+ requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
- // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
// Call the class function that loads the parameters for this class.
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
-
- // *********************************************************************************
-
-
-
- // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
- // advertises under the name "DebugTopic" and is a message with the structure
- // defined in the file "DebugMsg.msg" (located in the "msg" folder).
- debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
-
- // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
- // type variable that subscribes to the "Setpoint" topic and calls the class function
- // "setpointCallback" each time a messaged is received on this topic and the message
- // is passed as an input argument to the "setpointCallback" class function.
- ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
-
- // Instantiate the local variable "service" to be a "ros::ServiceServer" type
- // variable that advertises the service called "DemoController". This service has
- // the input-output behaviour defined in the "Controller.srv" file (located in the
- // "srv" folder). This service, when called, is provided with the most recent
- // measurement of the Crazyflie and is expected to respond with the control action
- // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
- // this is where the "outer loop" controller function starts. When a request is made
- // of this service the "calculateControlOutput" function is called.
- ros::ServiceServer service = nodeHandle.advertiseService("DemoController", calculateControlOutput);
-
- // Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
- // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
- // "using namespace d_fall_pps;"
- // in the "DEFINES" section at the top of this file.
- ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
-
- // Instantiate the instance variable "my_current_xyz_yaw_publisher" to be a "ros::Publisher"
- // that advertises under the name "<my_agentID>/my_current_xyz_yaw_topic" where <my_agentID>
- // is filled in with the student ID number of this computer. The messages published will
- // have the structure defined in the file "Setpoint.msg" (located in the "msg" folder).
- my_current_xyz_yaw_publisher = nodeHandle.advertise<Setpoint>("my_current_xyz_yaw_topic", 1);
-
- // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
- // type variable that subscribes to the "StudentCustomButton" topic and calls the class
- // function "customCommandReceivedCallback" each time a messaged is received on this topic
- // and the message received is passed as an input argument to the callback function.
- ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);
-
- // Print out some information to the user.
- ROS_INFO("[DEMO CONTROLLER] Service ready :-)");
-
- // Enter an endless while loop to keep the node alive.
- ros::spin();
-
- // Return zero if the "ross::spin" is cancelled.
- return 0;
+ //fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "Setpoint" topic and calls the class function
+ // "setpointCallback" each time a messaged is received on this topic and the message
+ // is passed as an input argument to the "setpointCallback" class function.
+ ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "DemoController". This service has
+ // the input-output behaviour defined in the "Controller.srv" file (located in the
+ // "srv" folder). This service, when called, is provided with the most recent
+ // measurement of the Crazyflie and is expected to respond with the control action
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("DemoController", calculateControlOutput);
+
+ // Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
+ // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
+ // "using namespace d_fall_pps;"
+ // in the "DEFINES" section at the top of this file.
+ ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+
+ // Instantiate the instance variable "my_current_xyz_yaw_publisher" to be a "ros::Publisher"
+ // that advertises under the name "<m_agentID>/my_current_xyz_yaw_topic" where <m_agentID>
+ // is filled in with the student ID number of this computer. The messages published will
+ // have the structure defined in the file "Setpoint.msg" (located in the "msg" folder).
+ my_current_xyz_yaw_publisher = nodeHandle.advertise<Setpoint>("my_current_xyz_yaw_topic", 1);
+
+ // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "StudentCustomButton" topic and calls the class
+ // function "customCommandReceivedCallback" each time a messaged is received on this topic
+ // and the message received is passed as an input argument to the callback function.
+ ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);
+
+ // Print out some information to the user.
+ ROS_INFO("[DEMO CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
index 4642273215f40ef7a157c228800fc7a2a57d4624..de1c8b3963fb12216e39c32915c2b213a9fe7ed7 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -483,6 +483,7 @@ void commandCallback(const IntWithHeader & msg) {
break;
case CMD_CRAZYFLY_TAKE_OFF:
+ ROS_INFO("[PPS CLIENT] TAKE_OFF Command received");
if(flying_state == STATE_MOTORS_OFF)
{
changeFlyingStateTo(STATE_TAKE_OFF);
@@ -490,12 +491,14 @@ void commandCallback(const IntWithHeader & msg) {
break;
case CMD_CRAZYFLY_LAND:
+ ROS_INFO("[PPS CLIENT] LAND Command received");
if(flying_state != STATE_MOTORS_OFF)
{
changeFlyingStateTo(STATE_LAND);
}
break;
case CMD_CRAZYFLY_MOTORS_OFF:
+ ROS_INFO("[PPS CLIENT] MOTORS_OFF Command received");
changeFlyingStateTo(STATE_MOTORS_OFF);
break;
@@ -508,11 +511,13 @@ void commandCallback(const IntWithHeader & msg) {
void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
{
+ ROS_INFO("[PPS CLIENT] Received message that the Context Database Changed");
loadCrazyflieContext();
}
void emergencyStopCallback(const IntWithHeader & msg)
{
+ ROS_INFO("[PPS CLIENT] Received message to EMERGENCY STOP");
commandCallback(msg);
}
@@ -523,6 +528,7 @@ void emergencyStopCallback(const IntWithHeader & msg)
void crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
+ ROS_INFO_STREAM("[PPS CLIENT] Received message with Crazy Radio Status = " << msg.data );
crazyradio_status = msg.data;
// RESET THE BATTERY STATE IF DISCONNECTED
//if (crazyradio_status == CRAZY_RADIO_STATE_DISCONNECTED)
@@ -585,7 +591,9 @@ void safeSetPointCallback(const Setpoint& newSetpoint)
void loadSafeController() {
- ros::NodeHandle nodeHandle("~");
+ //ros::NodeHandle nodeHandle("~");
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
std::string safeControllerName;
if(!nodeHandle.getParam("safeController", safeControllerName)) {
@@ -600,7 +608,9 @@ void loadSafeController() {
void loadDemoController()
{
- ros::NodeHandle nodeHandle("~");
+ //ros::NodeHandle nodeHandle("~");
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
std::string demoControllerName;
if(!nodeHandle.getParam("demoController", demoControllerName))
@@ -615,7 +625,9 @@ void loadDemoController()
void loadStudentController()
{
- ros::NodeHandle nodeHandle("~");
+ //ros::NodeHandle nodeHandle("~");
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
std::string studentControllerName;
if(!nodeHandle.getParam("studentController", studentControllerName))
@@ -630,7 +642,9 @@ void loadStudentController()
void loadMpcController()
{
- ros::NodeHandle nodeHandle("~");
+ //ros::NodeHandle nodeHandle("~");
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
std::string mpcControllerName;
if(!nodeHandle.getParam("mpcController", mpcControllerName))
@@ -645,7 +659,9 @@ void loadMpcController()
void loadRemoteController()
{
- ros::NodeHandle nodeHandle("~");
+ //ros::NodeHandle nodeHandle("~");
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
std::string remoteControllerName;
if(!nodeHandle.getParam("remoteController", remoteControllerName))
@@ -660,7 +676,9 @@ void loadRemoteController()
void loadTuningController()
{
- ros::NodeHandle nodeHandle("~");
+ //ros::NodeHandle nodeHandle("~");
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
std::string tuningControllerName;
if(!nodeHandle.getParam("tuningController", tuningControllerName))
@@ -675,7 +693,9 @@ void loadTuningController()
void loadPickerController()
{
- ros::NodeHandle nodeHandle("~");
+ //ros::NodeHandle nodeHandle("~");
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
std::string pickerControllerName;
if(!nodeHandle.getParam("pickerController", pickerControllerName))
@@ -768,15 +788,27 @@ void batteryMonitorStateChangedCallback(std_msgs::Int32 msg)
int new_battery_state = msg.data;
// Take action if changed to low battery state
- if ((flying_state != STATE_MOTORS_OFF) && (new_battery_state == BATTERY_STATE_LOW))
+ if (new_battery_state == BATTERY_STATE_LOW)
{
- ROS_INFO("[PPS CLIENT] low level battery triggered, now landing.");
- changeFlyingStateTo(STATE_LAND);
+ if (flying_state != STATE_MOTORS_OFF)
+ {
+ ROS_INFO("[PPS CLIENT] low level battery triggered, now landing.");
+ changeFlyingStateTo(STATE_LAND);
+ }
+ else
+ {
+ ROS_INFO("[PPS CLIENT] low level battery triggered, please turn off the Crazyflie.");
+ }
}
- else if ((flying_state == STATE_MOTORS_OFF) && (new_battery_state == BATTERY_STATE_LOW))
+ else if (new_battery_state == BATTERY_STATE_NORMAL)
{
- ROS_INFO("[PPS CLIENT] low level battery triggered, please turn off the Crazyflie.");
+ ROS_INFO("[PPS CLIENT] received message that battery state changed to normal");
}
+ else
+ {
+ ROS_INFO("[PPS CLIENT] received message that battery state changed to something unknown");
+ }
+
}
@@ -1152,7 +1184,7 @@ int main(int argc, char* argv[])
// SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
- // The parameters service publish messages with names of the form:
+ // The parameter service publishes messages with names of the form:
// /dfall/.../ParameterService/<filename with .yaml extension>
ros::Subscriber clientConfig_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "ClientConfig", 1, isReadyClientConfigYamlCallback);
ros::Subscriber clientConfig_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("ClientConfig", 1, isReadyClientConfigYamlCallback);
diff --git a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
index 0aa0aefed9e04232e1c30116b78bbf8dc4989d90..825f8108ea949cc30ea95be52f461b86650753f1 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -264,7 +264,7 @@ void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_lo
// Create publisher as a local variable, using the filename
// as the name of the message
- ros::Publisher yamlParametersReadyForFetchPublisher = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1);
+ ros::Publisher yamlParametersReadyForFetchPublisher = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1, true);
//ros::spinOnce();
// Create a local variable for the message
IntWithHeader yaml_ready_msg;
diff --git a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
index 6607ee9a43ae794b655342bea64395021b1df61d..d5bea535477812c3a02deec159409a0e1a9d4a76 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
@@ -146,7 +146,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
// ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
- response.controlOutput.onboardControllerType = RATE_MODE;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
previousLocation = request.ownCrazyflie;
diff --git a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
index 7f2098be738605bc360a23230e2efedbe510c7a8..5c357b3144bfa85ba3f024002a55ee791a0bc11f 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -114,20 +114,30 @@
// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
//
// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines" section at the
-// top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
-// > In the PPS exercise we will only use the RATE_MODE.
-// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
-// specify the angular rate in [radians/second] that will be requested from the
-// PID controllers running in the Crazyflie 2.0 firmware.
-// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
-// are the baseline motor commands requested from the Crazyflie, with the adjustment
-// for body rates being added on top of this in the firmware (i.e., as per the code
-// of the "distribute_power" function provided in exercise sheet 2).
-// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
-// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
-// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
-// thrust). To assist, teh following is an ASCII art of this convention:
+// > The allowed values for "onboardControllerType" are in the "Defines"
+// section in the header file, they are:
+// - CF_COMMAND_TYPE_MOTORS
+// - CF_COMMAND_TYPE_RATE
+// - CF_COMMAND_TYPE_ANGLE
+//
+// > For completeing the class exercises it is only required to use
+// the CF_COMMAND_TYPE_RATE option.
+//
+// > For the CF_COMMAND_TYPE_RATE optoin:
+// 1) the ".roll", ".ptich", and ".yaw" properties of
+// "response.ControlCommand" specify the angular rate in
+// [radians/second] that will be requested from the PID controllers
+// running in the Crazyflie 2.0 firmware.
+// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// "response.ControlCommand" are the baseline motor commands
+// requested from the Crazyflie, with the adjustment for body rates
+// being added on top of this in the firmware (i.e., as per the
+// code of the "distribute_power" found in the firmware).
+// 3) the axis convention for the roll, pitch, and yaw body rates
+// returned in "response.ControlCommand" should use right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the following is an ASCII art of this convention.
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -173,15 +183,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
float stateErrorInertial[9];
// Fill in the (x,y,z) position error
- stateErrorInertial[0] = request.ownCrazyflie.x - setpoint[0];
- stateErrorInertial[1] = request.ownCrazyflie.y - setpoint[1];
- stateErrorInertial[2] = request.ownCrazyflie.z - setpoint[2];
+ stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
+ stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
+ stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
// Compute an estimate of the velocity
// > As simply the derivative between the current and previous position
- stateErrorInertial[3] = (stateErrorInertial[0] - previous_stateErrorInertial[0]) * control_frequency;
- stateErrorInertial[4] = (stateErrorInertial[1] - previous_stateErrorInertial[1]) * control_frequency;
- stateErrorInertial[5] = (stateErrorInertial[2] - previous_stateErrorInertial[2]) * control_frequency;
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
// Fill in the roll and pitch angle measurements directly
stateErrorInertial[6] = request.ownCrazyflie.roll;
@@ -191,7 +201,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > This error should be "unwrapped" to be in the range
// ( -pi , pi )
// > First, get the yaw error into a local variable
- float yawError = request.ownCrazyflie.yaw - setpoint[3];
+ float yawError = request.ownCrazyflie.yaw - m_setpoint[3];
// > Second, "unwrap" the yaw error to the interval ( -pi , pi )
while(yawError > PI) {yawError -= 2 * PI;}
while(yawError < -PI) {yawError += 2 * PI;}
@@ -213,7 +223,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > as we have already used previous error we can now update it update it
for(int i = 0; i < 9; ++i)
{
- previous_stateErrorInertial[i] = stateErrorInertial[i];
+ m_previous_stateErrorInertial[i] = stateErrorInertial[i];
}
@@ -235,7 +245,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Perform the "-Kx" LQR computation for the yaw rate to respoond with
for(int i = 0; i < 9; ++i)
{
- yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ yawRate_forResponse -= m_gainMatrixYawRate[i] * stateErrorBody[i];
}
// Put the computed yaw rate into the "response" variable
@@ -260,7 +270,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Perform the "-Kx" LQR computation for the thrust adjustment to respoond with
for(int i = 0; i < 9; ++i)
{
- thrustAdjustment -= gainMatrixThrust[i] * stateErrorBody[i];
+ thrustAdjustment -= m_gainMatrixThrust[i] * stateErrorBody[i];
}
// Put the computed thrust adjustment into the "response" variable,
@@ -268,10 +278,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
- // > NOTE: the "cf_weight_in_newtons" value is the total thrust required
+ // > NOTE: the "m_cf_weight_in_newtons" value is the total thrust required
// as feed-forward. Assuming the the Crazyflie is symmetric, this
// value is divided by four.
- float feed_forward_thrust_per_motor = cf_weight_in_newtons / 4.0;
+ float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
// > NOTE: the function "computeMotorPolyBackward" converts the input argument
// from Newtons to the 16-bit command expected by the Crazyflie.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
@@ -297,7 +307,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Perform the "-Kx" LQR computation for the pitch rate to respoond with
for(int i = 0; i < 9; ++i)
{
- pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ pitchRate_forResponse -= m_gainMatrixPitchRate[i] * stateErrorBody[i];
}
// Put the computed pitch rate into the "response" variable
@@ -322,7 +332,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Perform the "-Kx" LQR computation for the roll rate to respoond with
for(int i = 0; i < 9; ++i)
{
- rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
+ rollRate_forResponse -= m_gainMatrixRollRate[i] * stateErrorBody[i];
}
// Put the computed roll rate into the "response" variable
@@ -334,9 +344,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
/*choosing the Crazyflie onBoard controller type.
it can either be Motor, Rate or Angle based */
- // response.controlOutput.onboardControllerType = MOTOR_MODE;
- response.controlOutput.onboardControllerType = RATE_MODE;
- // response.controlOutput.onboardControllerType = ANGLE_MODE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -349,11 +359,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// D D E B B U U G G M M S G G
// DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
- // DEBUGGING CODE:
- // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
- // By fill this message with data and publishing it you can display the data in
- // real time using rpt plots. Instructions for using rqt plots can be found on
- // the wiki of the D-FaLL-System repository
+ // DEBUGGING CODE:
+ // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
+ // By fill this message with data and publishing it you can display the data in
+ // real time using rpt plots. Instructions for using rqt plots can be found on
+ // the wiki of the D-FaLL-System repository
// Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
// (located in the "msg" folder) to see the full structure of this message.
@@ -378,41 +388,41 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// debugMsg.value_10 = your_variable_name;
// Publish the "debugMsg"
- debugPublisher.publish(debugMsg);
+ m_debugPublisher.publish(debugMsg);
// An alternate debugging technique is to print out data directly to the
- // command line from which this node was launched.
-
- // An example of "printing out" the data from the "request" argument to the
- // command line. This might be useful for debugging.
- // ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
- // ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
- // ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
- // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
- // ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
- // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
- // ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
-
- // An example of "printing out" the control actions computed.
- // ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- // ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- // ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- // ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
-
- // An example of "printing out" the "thrust-to-command" conversion parameters.
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
-
- // An example of "printing out" the per motor 16-bit command computed.
- // ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
- // ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
- // ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
- // ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
-
- // Return "true" to indicate that the control computation was performed successfully
- return true;
-}
+ // command line from which this node was launched.
+
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ // ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
+ // ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
+ // ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
+ // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
+ // ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
+ // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
+ // ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
+
+ // An example of "printing out" the control actions computed.
+ // ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
+ // ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
+ // ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
+ // ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ // ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
+ // ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
+ // ROS_INFO_STREAM("motorPoly 2:" << yaml_motorPoly[2]);
+
+ // An example of "printing out" the per motor 16-bit command computed.
+ // ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ // ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ // ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ // ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+ }
@@ -457,20 +467,20 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
{
- // Fill in the (x,y,z) position estimates to be returned
- stateBody[0] = stateInertial[0];
- stateBody[1] = stateInertial[1];
- stateBody[2] = stateInertial[2];
-
- // Fill in the (x,y,z) velocity estimates to be returned
- stateBody[3] = stateInertial[3];
- stateBody[4] = stateInertial[4];
- stateBody[5] = stateInertial[5];
-
- // Fill in the (roll,pitch,yaw) estimates to be returned
- stateBody[6] = stateInertial[6];
- stateBody[7] = stateInertial[7];
- stateBody[8] = stateInertial[8];
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0];
+ stateBody[1] = stateInertial[1];
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3];
+ stateBody[4] = stateInertial[4];
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
}
@@ -491,10 +501,26 @@ void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float y
// CCCC OOO N N V EEEEE R R SSSS III OOO N N
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NEED TO BE edited for successful completion of
+// the exercise
float computeMotorPolyBackward(float thrust)
{
- return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+ // Compute the 16-but command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
+
+
+
+ // > Could this formula compute a result "cmd_16bit" that is
+ // greater than ((2^16)-1)?
+ // > What might happen when such a number is case as a 16-bit
+ // integer? In other words, what is uint16(cmd_sixteen_bit)?
+
+
+
+ // Return the result
+ return cmd_16bit;
}
@@ -518,10 +544,10 @@ float computeMotorPolyBackward(float thrust)
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
void setpointCallback(const Setpoint& newSetpoint)
{
- setpoint[0] = newSetpoint.x;
- setpoint[1] = newSetpoint.y;
- setpoint[2] = newSetpoint.z;
- setpoint[3] = newSetpoint.yaw;
+ m_setpoint[0] = newSetpoint.x;
+ m_setpoint[1] = newSetpoint.y;
+ m_setpoint[2] = newSetpoint.z;
+ m_setpoint[3] = newSetpoint.yaw;
}
@@ -611,170 +637,100 @@ void customCommandReceivedCallback(const CustomButton& commandReceived)
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+// This function DOES NOT NEED TO BE edited for successful completion
+// ofthe exercise
+void isReadyStudentControllerYamlCallback(const IntWithHeader & msg)
{
- // Extract from the "msg" for which controller the and from where to fetch the YAML
- // parameters
- int controller_to_fetch_yaml = msg.data;
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
- // Switch between fetching for the different controllers and from different locations
- switch(controller_to_fetch_yaml)
+ // Continue if the message is relevant
+ if (isRevelant)
{
-
- // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT:
- {
- // Let the user know that this message was received
- ROS_INFO("[STUDENT CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
- // Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
- // Call the function that fetches the parameters
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
- break;
- }
-
- // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR:
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
{
- // Let the user know that this message was received
- ROS_INFO("[STUDENT CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
- // Create a node handle to the parameter service running on the coordinator machine
- ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
- // Call the function that fetches the parameters
- fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
- break;
- }
-
- default:
- {
- // Let the user know that the command was not relevant
- //ROS_INFO("The StudentControllerService received the message that YAML parameters were (re-)loaded");
- //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
- break;
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[STUDENT CONTROLLER] Now fetching the StudentController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[STUDENT CONTROLLER] Now fetching the StudentController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[STUDENT CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
}
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchStudentControllerYamlParameters(nodeHandle_to_use);
}
}
-// This function CAN BE edited for successful completion of the PPS exercise, and the
-// use of parameters fetched from the YAML file is highly recommended to make tuning of
-// your controller easier and quicker.
-void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+// This function CAN BE edited for successful completion of the
+// exercise, and the use of parameters fetched from the YAML file
+// is highly recommended to make tuning of your controller easier
+// and quicker.
+void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle)
{
- // Here we load the parameters that are specified in the StudentController.yaml file
+ // Here we load the parameters that are specified in the file:
+ // StudentController.yaml
// Add the "StudentController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_studentController(nodeHandle, "StudentController");
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "StudentController");
+
+
+
+ // GET THE PARAMETERS:
// > The mass of the crazyflie
- cf_mass_in_grams = getParameterFloat(nodeHandle_for_studentController , "mass");
+ yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
// Display one of the YAML parameters to debug if it is working correctly
- //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass_in_grams );
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << yaml_cf_mass_in_grams );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- control_frequency = getParameterFloat(nodeHandle_for_studentController, "control_frequency");
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_studentController, "motorPoly", motorPoly, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
- // DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass_in_grams);
- // Call the function that computes details an values that are needed from these
- // parameters loaded above
- processFetchedParameters();
-
-}
-
-
-// This function CAN BE edited for successful completion of the PPS exercise, and the
-// use of parameters loaded from the YAML file is highly recommended to make tuning of
-// your controller easier and quicker.
-void processFetchedParameters()
-{
- // Compute the feed-forward force that we need to counteract gravity (i.e., mg)
- // > in units of [Newtons]
- cf_weight_in_newtons = cf_mass_in_grams * 9.81/1000.0;
-
- // DEBUGGING: Print out one of the computed quantities
- ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << cf_weight_in_newtons);
-}
+ // > DEBUGGING: Print out one of the parameters that was loaded to
+ // debug if the fetching of parameters worked correctly
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << yaml_cf_mass_in_grams);
-// ----------------------------------------------------------------------------------
-// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
-// G E T P P A A R R A A MM MM ( )
-// G EEE T PPPP A A RRRR A A M M M ( )
-// G G E T P AAAAA R R AAAAA M M ( )
-// GGGG EEEEE T P A A R R A A M M ( )
-// ----------------------------------------------------------------------------------
+ // PROCESS THE PARAMTERS
+ // > Compute the feed-forward force that we need to counteract
+ // gravity (i.e., mg) in units of [Newtons]
+ m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
-{
- float val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
-{
- int val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val.size();
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
-{
- bool val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
}
-
@@ -790,101 +746,140 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
int main(int argc, char* argv[]) {
-
- // Starting the ROS-node
- ros::init(argc, argv, "StudentControllerService");
-
- // 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 "StudentControllerService" node
- std::string m_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[STUDENT CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
-
- // Get the agent ID as the "ROS_NAMESPACE" this computer.
- // NOTES:
- // > If you look at the "Agent.launch" file in the "launch" folder, you will see
- // the following line of code:
- // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
- // This line of code adds a parameter named "studentID" to the "PPSClient"
- // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
- // to the "PPSClient" node within which this controller service is nested.
- // Get the handle to the "PPSClient" node
- ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
- // Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
- {
- // Throw an error if the student ID parameter could not be obtained
- ROS_ERROR("[STUDENT CONTROLLER] Failed to get agentID from PPSClient");
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "StudentControllerService");
+
+ // 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 "StudentControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "PPSClient" node.
+ // > Thus, to get access to this "studentID" paremeter, we first
+ // need to get a handle to the "PPSClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/PPSClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[STUDENT CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
}
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
- // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
- // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
- // namespace string for the parameter service that is running on the machine of this
- // agent
- namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
- // Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
- // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
- // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
- // and calls the class function "yamlReadyForFetchCallback" each time a message is
- // received on this topic and the message is passed as an input argument to the
- // "yamlReadyForFetchCallback" class function.
- ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "StudentController", 1, isReadyStudentControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("StudentController", 1, isReadyStudentControllerYamlCallback);
- // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
- // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
- // for the parameter service that is running on the coordinate machine
- // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
- // is very important because it specifies that the name is global
- namespace_to_coordinator_parameter_service = "/ParameterService";
- // Create a node handle to the parameter service running on the coordinator machine
- ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
- //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
-
+ // GIVE YAML VARIABLES AN INITIAL VALUE
- // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
- // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
- // and calls the class function "yamlReadyForFetchCallback" each time a message is
- // received on this topic and the message is passed as an input argument to the
- // "yamlReadyForFetchCallback" class function.
- ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
- //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ // This can be done either here or as part of declaring the variable
+ // in the header file
+ yaml_cf_mass_in_grams = 25.0;
+ yaml_motorPoly[0] = 5.484560e-4;
+ yaml_motorPoly[1] = 1.032633e-6;
+ yaml_motorPoly[2] = 2.130295e-11;
+ yaml_control_frequency = 200.0;
- // PRINT OUT SOME INFORMATION
- // Let the user know what namespaces are being used for linking to the parameter service
- ROS_INFO_STREAM("[STUDENT CONTROLLER] The namespace string for accessing the Paramter Services are:");
- ROS_INFO_STREAM("[STUDENT CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("[STUDENT CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+ // The yaml files for the controllers are not added to "Parameter
+ // Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // message containing the filename of the *.yaml file, and
+ // then a message will be received on the above subscribers
+ // when the paramters are ready.
- // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+ // Create a publisher for request the yaml load
+ // > created as a local variable
+ // > note importantly that the final argument is "true", this
+ // enables "latching" on the connection. When a connection is
+ // latched, the last message published is saved and
+ // automatically sent to any future subscribers that connect.
+ ros::Publisher requestLoadYamlFilenamePublisher = nodeHandle_to_own_agent_parameter_service.advertise<StringWithHeader>("requestLoadYamlFilename", 1, true);
+ // Create a local variable for the message
+ StringWithHeader yaml_filename_msg;
+ // Specify the data
+ yaml_filename_msg.data = "StudentController";
+ // Set for whom this applies to
+ yaml_filename_msg.shouldCheckForID = false;
+ // Sleep to make the publisher known to ROS (in seconds)
+ //ros::Duration(1.0).sleep();
+ // Send the message
+ requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
- // Call the class function that loads the parameters for this class.
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
- // *********************************************************************************
+ // PUBLISHERS AND SUBSCRIBERS
- // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
- // advertises under the name "DebugTopic" and is a message with the structure
- // defined in the file "DebugMsg.msg" (located in the "msg" folder).
- debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
// Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
// type variable that subscribes to the "Setpoint" topic and calls the class function