diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 84a1ade4b018bf254b11e41b16ce57709acddae0..294a21b8aa42b139a7164a8b101cbd3160ee7249 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -320,17 +320,19 @@ if(VICON_LIBRARY)
add_executable(ViconDataPublisher src/nodes/ViconDataPublisher.cpp)
endif()
-add_executable(PPSClient src/nodes/PPSClient.cpp)
-add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp)
-add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
-add_executable(DemoControllerService src/nodes/DemoControllerService.cpp)
-add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
-add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
-add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
-add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
-add_executable(PickerControllerService src/nodes/PickerControllerService.cpp)
-add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
-add_executable(ParameterService src/nodes/ParameterService.cpp)
+add_executable(PPSClient src/nodes/PPSClient.cpp
+ src/classes/GetParamtersAndNamespaces.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(MpcControllerService src/nodes/MpcControllerService.cpp)
+add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
+add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
+add_executable(PickerControllerService src/nodes/PickerControllerService.cpp)
+add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
+add_executable(ParameterService src/nodes/ParameterService.cpp)
@@ -404,17 +406,17 @@ if(VICON_LIBRARY)
add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
endif()
-add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(BatteryMonitor d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(DemoControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(StudentControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(MpcControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(RemoteControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(TuningControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(PickerControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(BatteryMonitor d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(DemoControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(StudentControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(MpcControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(RemoteControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(TuningControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(PickerControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -455,21 +457,21 @@ if(VICON_LIBRARY)
target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
endif()
-target_link_libraries(PPSClient ${catkin_LIBRARIES})
-target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
-target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
-target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
-target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
+target_link_libraries(PPSClient ${catkin_LIBRARIES})
+target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
+target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
+target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
+target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
if(Eigen3_FOUND)
target_link_libraries(MpcControllerService ${catkin_LIBRARIES} Eigen3::Eigen)
else()
target_link_libraries(MpcControllerService ${catkin_LIBRARIES})
endif()
-target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
-target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
-target_link_libraries(PickerControllerService ${catkin_LIBRARIES})
-target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
-target_link_libraries(ParameterService ${catkin_LIBRARIES})
+target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
+target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
+target_link_libraries(PickerControllerService ${catkin_LIBRARIES})
+target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
+target_link_libraries(ParameterService ${catkin_LIBRARIES})
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h
index 96a9f12bc5654d80e9f2cfa115303467457f675e..ef7d7920ba8efbc9d366a90e336b5ccb4b0e187a 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h
@@ -42,15 +42,22 @@
#ifdef CATKIN_MAKE
#include "rosNodeThread_for_managerGUI.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 "marker.h"
#include "crazyFly.h"
#include "CFLinker.h"
+// Include the DFALL service types
#include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieEntry.h"
-#include <std_msgs/Int32.h>
-
// The constants that are sent to the agents in order to
// "command" changes in their operation state
@@ -235,7 +242,7 @@ private:
std::string ros_namespace;
- ros::Publisher DBChangedPublisher;
+ //ros::Publisher DBChangedPublisher;
ros::Publisher emergencyStopPublisher;
// Publsher for sending "commands" from here (the master) to all
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
index 8cd1e74cc8c9262b120c10e80bba33f3697608f3..2f790f7e41b589cb8d2058b027963b71ee6f91b5 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
@@ -275,8 +275,11 @@ void MainGUIWindow::_init()
ros_namespace = ros::this_node::getNamespace();
ros::NodeHandle nodeHandle("~");
- DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
- emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
+ //DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+ emergencyStopPublisher = nodeHandle_dfall_root.advertise<d_fall_pps::IntWithHeader>("emergencyStop", 1);
// Initialise the publisher for sending "commands" from here (the master)
// to all of the agent nodes
@@ -883,9 +886,9 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
fill_database_file();
// Now also publish a ROS message stating that we changed the DB, so the nodes can update it
- std_msgs::Int32 msg;
- msg.data = 1;
- this->DBChangedPublisher.publish(msg);
+ //std_msgs::Int32 msg;
+ //msg.data = 1;
+ //this->DBChangedPublisher.publish(msg);
}
void MainGUIWindow::clear_database_file()
@@ -1105,10 +1108,11 @@ void MainGUIWindow::on_all_land_button_clicked()
// > MOTORS OFF
void MainGUIWindow::on_all_motors_off_button_clicked()
{
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
- commandAllAgentsPublisher.publish(msg);
- //emergencyStopPublisher.publish(msg);
+ msg.shouldCheckForID = false;
+ //commandAllAgentsPublisher.publish(msg);
+ emergencyStopPublisher.publish(msg);
}
// > ENABLE SAFE CONTROLLER
void MainGUIWindow::on_all_enable_safe_controller_button_clicked()
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 8e5ef66452562e6573b2fd28f71c00492ca8ecf9..75410c452c402d2f696fa852d20e408e48a01396 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
@@ -38,17 +38,23 @@
#include <QMutex>
#ifdef CATKIN_MAKE
-#include <std_msgs/Int32.h>
-#include <std_msgs/Float32.h>
-
#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"
+
+
using namespace d_fall_pps;
#endif
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 147dec5b187d453919a3d286ecf64f96510736da..1d757b1cd3018bb30f8f72f4965978cbbf48cee7 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
@@ -4,17 +4,26 @@
#include <QWidget>
#ifdef CATKIN_MAKE
-#include <std_msgs/Int32.h>
-#include <std_msgs/Float32.h>
-
#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 the DFALL service types
// #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"
+
// using namespace d_fall_pps;
#endif
@@ -65,9 +74,28 @@ private:
// --------------------------------------------------- //
// PRIVATE VARIABLES FOR ROS
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
// PUBLISHERS AND SUBSRIBERS
- // > For Crazyradio commands based on button clicks
- ros::Publisher commandAllPublisher;
+ // > For {take-off,land,motors-off} and controller selection
+ ros::Publisher commandPublisher;
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS FOR ROS
+
+ // Fill the head for a message
+ void fillIntMessageHeader( d_fall_pps::IntWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+
+
+
#endif
};
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h
index f61703c4f3562b8cb3fef7f933c4a8d619531654..8281e4868c736196ea6b096cb5b3f814f75624cb 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h
@@ -41,6 +41,21 @@
#include <ros/network.h>
#include <ros/package.h>
#include "rosNodeThread_for_flyingAgentGUI.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/StringWithHeader.h"
+
+#include "nodes/Constants.h"
+
+// Namespacing the package
+using namespace d_fall_pps;
+//using namespace std;
+
#endif
@@ -68,6 +83,19 @@ private:
#ifdef CATKIN_MAKE
rosNodeThread* m_rosNodeThread;
+
+ // Variables for the type and ID
+ // The type of this node, i.e., agent or a coordinator,
+ // specififed as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID = 0;
+
+ // The namespace into which this parameter service loads yaml parameters
+ std::string m_parameter_service_namespace;
+
+ ros::Publisher m_requestLoadYamlFilenamePublisher;
#endif
};
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 1f073f9b767a700020a3ae8cf72d1e6e13170fe7..68c7e58cbd3d78604edfa6ad6734145674da2a30 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
@@ -125,7 +125,7 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
// > For updating the current battery state
batteryStateSubscriber = base_nodeHandle.subscribe("PPSClient/batteryState", 1, &CoordinatorRow::batteryStateChangedCallback, this);
// > For Flying state commands based on button clicks
- flyingStateCommandPublisher = base_nodeHandle.advertise<std_msgs::Int32>("PPSClient/Command", 1);
+ 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
@@ -712,7 +712,8 @@ void CoordinatorRow::on_rf_disconnect_button_clicked()
void CoordinatorRow::on_enable_flying_button_clicked()
{
#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
+ msg.shouldCheckForID = false;
msg.data = CMD_CRAZYFLY_TAKE_OFF;
this->flyingStateCommandPublisher.publish(msg);
#endif
@@ -721,7 +722,8 @@ void CoordinatorRow::on_enable_flying_button_clicked()
void CoordinatorRow::on_disable_flying_button_clicked()
{
#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
+ msg.shouldCheckForID = false;
msg.data = CMD_CRAZYFLY_LAND;
this->flyingStateCommandPublisher.publish(msg);
#endif
@@ -730,7 +732,8 @@ void CoordinatorRow::on_disable_flying_button_clicked()
void CoordinatorRow::on_motors_off_button_clicked()
{
#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
+ msg.shouldCheckForID = false;
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
this->flyingStateCommandPublisher.publish(msg);
#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 e844d06067352607dbd55ec3bb01315f3d5d50b7..14df7d63bb4897b9ade22bc2479a68768b2aaef2 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
@@ -10,10 +10,27 @@ EnableControllerLoadYamlBar::EnableControllerLoadYamlBar(QWidget *parent) :
#ifdef CATKIN_MAKE
//ros::init();
- // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
- ros::NodeHandle dfall_root_nodeHandle("/dfall");
- commandAllPublisher = dfall_root_nodeHandle.advertise<std_msgs::Int32>("/my_GUI/commandAllAgents", 1);
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[FLYING AGENT GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[FLYING AGENT GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE COMMAND PUBLISHER
+ commandPublisher = nodeHandle_for_this_gui.advertise<d_fall_pps::IntWithHeader>("PPSClient/Command", 1);
#endif
}
@@ -31,9 +48,10 @@ EnableControllerLoadYamlBar::~EnableControllerLoadYamlBar()
void EnableControllerLoadYamlBar::on_enable_safe_button_clicked()
{
#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
+ fillIntMessageHeader(msg);
msg.data = CMD_USE_SAFE_CONTROLLER;
- this->commandAllPublisher.publish(msg);
+ this->commandPublisher.publish(msg);
ROS_INFO("[FLYING AGENT GUI] Enable Safe Controller");
#endif
}
@@ -41,9 +59,10 @@ void EnableControllerLoadYamlBar::on_enable_safe_button_clicked()
void EnableControllerLoadYamlBar::on_enable_demo_button_clicked()
{
#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
+ fillIntMessageHeader(msg);
msg.data = CMD_USE_DEMO_CONTROLLER;
- this->commandAllPublisher.publish(msg);
+ this->commandPublisher.publish(msg);
ROS_INFO("[FLYING AGENT GUI] Enable Demo Controller");
#endif
}
@@ -51,9 +70,10 @@ void EnableControllerLoadYamlBar::on_enable_demo_button_clicked()
void EnableControllerLoadYamlBar::on_enable_student_button_clicked()
{
#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
+ d_fall_pps::IntWithHeader msg;
+ fillIntMessageHeader(msg);
msg.data = CMD_USE_STUDENT_CONTROLLER;
- this->commandAllPublisher.publish(msg);
+ this->commandPublisher.publish(msg);
ROS_INFO("[FLYING AGENT GUI] Enable Student Controller");
#endif
}
@@ -85,3 +105,122 @@ void EnableControllerLoadYamlBar::on_load_yaml_default_button_clicked()
{
}
+
+
+#ifdef CATKIN_MAKE
+// Fill the head for a message
+void EnableControllerLoadYamlBar::fillIntMessageHeader( d_fall_pps::IntWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ msg.shouldCheckForID = true;
+ msg.agentIDs.push_back(7);
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForID = true;
+ ROS_ERROR("[FLYING AGENT GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+#ifdef CATKIN_MAKE
+bool EnableControllerLoadYamlBar::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("[FLYING AGENT GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[FLYING AGENT GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[FLYING AGENT GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[FLYING AGENT GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[FLYING AGENT GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[FLYING AGENT GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[FLYING AGENT GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
index 544d3ab371db351c9e55ae7b97ab95168e64475e..a0e08224aeb438f3af25dc529e22aaa3ff168e85 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
@@ -50,7 +50,105 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
// > For "kill GUI node", press "CTRL+C" while the GUI window is the focus
m_close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
+#ifdef CATKIN_MAKE
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+ // Get the value of the "type" parameter into a local string variable
+ std::string type_string;
+ if(!nodeHandle.getParam("type", type_string))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[FLYING AGENT GUI] Failed to get type");
+ }
+
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ ROS_ERROR("[FLYING AGENT GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // 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
+ ROS_ERROR("[FLYING AGENT GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[FLYING AGENT GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ ROS_ERROR("[FLYING AGENT GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[FLYING AGENT GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ ROS_ERROR("[FLYING AGENT GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+
+
+ // Get the namespace of this "ParameterService" node
+ std::string this_node_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[FLYING AGENT GUI] ros::this_node::getNamespace() = " << this_node_namespace);
+
+ // Construct the string to the namespace of this Paramater Service
+ m_parameter_service_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[FLYING AGENT GUI] parameter service is: " << m_parameter_service_namespace);
+
+ // Get the node handle to this parameter service
+ ros::NodeHandle nodeHandle_to_parameter_service(m_parameter_service_namespace);
+ m_requestLoadYamlFilenamePublisher = nodeHandle_to_parameter_service.advertise<StringWithHeader>("requestLoadYamlFilename", 1);
+
+ // Remove the show/hide coordinator menu item if launch as an agent
+ if (m_type==TYPE_AGENT)
+ {
+ // Hide the coordinator part of the GUI
+ ui->customWidget_coordinator->hide();
+ // And make the menu item unavailable
+ ui->actionShowHide_Coordinator->setEnabled(false);
+
+ }
+#endif
}
@@ -81,8 +179,17 @@ void MainWindow::on_actionShowHide_Coordinator_triggered()
void MainWindow::on_action_LoadYAML_BatteryMonitor_triggered()
{
#ifdef CATKIN_MAKE
- // Send a message that the "BatteryMonitor" Yaml should be loaded
- // by the appropriate Parameter Service
+ // Inform the user that the menu item was selected
+ ROS_INFO("[FLYING AGENT GUI] Load Battery Monitor YAML was clicked.");
+
+ // Create a local variable for the message
+ StringWithHeader yaml_filename_msg;
+ // Specify the data
+ yaml_filename_msg.data = "BatteryMonitor";
+ // Set for whom this applies to
+ yaml_filename_msg.shouldCheckForID = false;
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
#endif
}
@@ -90,7 +197,16 @@ void MainWindow::on_action_LoadYAML_BatteryMonitor_triggered()
void MainWindow::on_action_LoadYAML_ClientConfig_triggered()
{
#ifdef CATKIN_MAKE
- // Send a message that the "ClientConfig" Yaml should be loaded
- // by the appropriate Parameter Service
+ // Inform the user that the menu item was selected
+ ROS_INFO("[FLYING AGENT GUI] Load Client Config YAML was clicked.");
+
+ // Create a local variable for the message
+ StringWithHeader yaml_filename_msg;
+ // Specify the data
+ yaml_filename_msg.data = "ClientConfig";
+ // Set for whom this applies to
+ yaml_filename_msg.shouldCheckForID = false;
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
#endif
}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 8562b558a21c7446598ecffaaa02bea001f802de..d12f08d228b2ed21c062f748195791ab9cb6950d 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -35,8 +35,9 @@
import roslib; roslib.load_manifest('d_fall_pps')
import rospy
-from d_fall_pps.msg import ControlCommand
from std_msgs.msg import Int32
+from d_fall_pps.msg import ControlCommand
+from d_fall_pps.msg import IntWithHeader
# General import
@@ -124,7 +125,7 @@ class PPSRadioClient:
self.link_uri = ""
self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
- self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', Int32, queue_size=1)
+ self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', IntWithHeader, queue_size=1)
time.sleep(1.0)
# Initialize the CrazyFlie and add callbacks
@@ -168,7 +169,10 @@ class PPSRadioClient:
print "Motors OFF"
self._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
- self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+ msg = IntWithHeader()
+ msg.shouldCheckForID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ self.PPSClient_command_pub.publish(msg)
time.sleep(0.1)
print "Disconnecting from %s" % self.link_uri
self._cf.close_link()
@@ -225,7 +229,10 @@ class PPSRadioClient:
"""
self.change_status_to(CONNECTED)
# change state to motors OFF
- cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+ msg = IntWithHeader()
+ msg.shouldCheckForID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ cf_client.PPSClient_command_pub.publish(msg)
rospy.loginfo("Connection to %s successful: " % link_uri)
# Config for Logging
@@ -252,7 +259,10 @@ class PPSRadioClient:
rospy.logwarn("Disconnected from %s" % link_uri)
# change state to motors OFF
- self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+ msg = IntWithHeader()
+ msg.shouldCheckForID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ self.PPSClient_command_pub.publish(msg)
self.logconf.stop()
rospy.loginfo("logconf stopped")
@@ -359,7 +369,10 @@ if __name__ == '__main__':
cf_client._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
- cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+ msg = IntWithHeader()
+ msg.shouldCheckForID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ cf_client.PPSClient_command_pub.publish(msg)
#wait for client to send its commands
time.sleep(1.0)
diff --git a/pps_ws/src/d_fall_pps/include/classes/GetParamtersAndNamespaces.h b/pps_ws/src/d_fall_pps/include/classes/GetParamtersAndNamespaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..f92f393de1ae9e6fdfb1d3d22184c45e258fd1b9
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/classes/GetParamtersAndNamespaces.h
@@ -0,0 +1,132 @@
+// 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 service that manages the loading of YAML parameters
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+#include <stdlib.h>
+#include <iostream>
+#include <string>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <ros/network.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 the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+using namespace d_fall_pps;
+//using namespace std;
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+
+// GET YAML PARAMETER FUNCTIONS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+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);
+
+
+
+// FUNCTIONS FOR GETTING IDs AND NAMESPACES
+
+// Get the "agentID" and "coordID" from the client node
+bool getAgentIDandCoordIDfromClientNode( std::string client_namespace , int * agentID_ref , int * coordID_ref);
+
+// Construct the namespaces for the coordinator
+void constructNamespaceForCoordinator( int coordID, std::string & coord_namespace );
+
+// Construct the namespaces for the coordinator's parameter services
+void constructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace );
+
+// Check the header of a message for whether it is relevant
+bool checkMessageHeader( int agentID , bool shouldCheckForID , const std::vector<uint> & agentIDs );
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h b/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
index d7eff9d1840e4c7c45c3673561430e5d4d9fa3c6..1739c05beeae98b64a82f0ae4c2360810794163f 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
@@ -59,6 +59,7 @@
// Include the shared definitions
#include "nodes/Constants.h"
+#include "classes/GetParamtersAndNamespaces.h"
// SPECIFY THE PACKAGE NAMESPACE
using namespace d_fall_pps;
@@ -75,18 +76,18 @@ using namespace d_fall_pps;
// ----------------------------------------------------------------------------------
// Battery levels
-#define BATTERY_LEVEL_000 0
-#define BATTERY_LEVEL_010 1
-#define BATTERY_LEVEL_020 2
-#define BATTERY_LEVEL_030 3
-#define BATTERY_LEVEL_040 4
-#define BATTERY_LEVEL_050 5
-#define BATTERY_LEVEL_060 6
-#define BATTERY_LEVEL_070 7
-#define BATTERY_LEVEL_080 8
-#define BATTERY_LEVEL_090 9
-#define BATTERY_LEVEL_100 10
-#define BATTERY_LEVEL_UNAVAILABLE -1
+// #define BATTERY_LEVEL_000 0
+// #define BATTERY_LEVEL_010 1
+// #define BATTERY_LEVEL_020 2
+// #define BATTERY_LEVEL_030 3
+// #define BATTERY_LEVEL_040 4
+// #define BATTERY_LEVEL_050 5
+// #define BATTERY_LEVEL_060 6
+// #define BATTERY_LEVEL_070 7
+// #define BATTERY_LEVEL_080 8
+// #define BATTERY_LEVEL_090 9
+// #define BATTERY_LEVEL_100 10
+// #define BATTERY_LEVEL_UNAVAILABLE -1
// Battery states
#define BATTERY_STATE_NORMAL 0
@@ -97,13 +98,6 @@ using namespace d_fall_pps;
#define TYPE_COORDINATOR 1
#define TYPE_AGENT 2
-
-// CrazyRadio states:
-#define CRAZY_RADIO_STATE_CONNECTED 0
-#define CRAZY_RADIO_STATE_CONNECTING 1
-#define CRAZY_RADIO_STATE_DISCONNECTED 2
-
-
// Flying states
#define AGENT_OPERATING_STATE_MOTORS_OFF 1
#define AGENT_OPERATING_STATE_TAKE_OFF 2
@@ -187,9 +181,14 @@ int yaml_battery_delay_threshold_to_change_state = 5;
// P R R OOO T OOO T Y P EEEEE SSSS
// ----------------------------------------------------------------------------------
+// Callback for the raw voltage data
void newBatteryVoltageCallback(const std_msgs::Float32& msg);
-
+// The selector for the filter
float newBatteryVoltageForFilter(float new_value);
+// Moving average filter
+float movingAverageBatteryFilter(float new_input);
+
+
// > For converting a voltage to a percentage,
// depending on the current "my_flying_state" value
float fromVoltageToPercent(float voltage , float operating_state);
@@ -201,31 +200,5 @@ void updateBatteryStateBasedOnLevel(int level);
// LOAD YAML PARAMETER FUNCTIONS
-void fetchYamlParameters(ros::NodeHandle& nodeHandle);
-void processFetchedParameters();
-
-void yamlReadyForFetchCallback(const IntWithHeader & msg);
-
-
-
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
-
-
-
-
-
-
-
-
-
-// FUNCTIONS FOR TEMPLATING A GET STUFF CLASS
-
-// Get the "agentID" and "coordID" from the client node
-bool getAgentIDandCoordIDfromClientNode( std::string client_namespace , int * agentID_ref , int * coordID_ref);
-
-// Construct the namespaces for the parameter services
-void getConstructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace );
-
-// Check the header of a message for whether it is relevant
-bool checkMessageHeader( int agentID , bool shouldCheckForID , const std::vector<uint> & agentIDs );
\ No newline at end of file
+void isReadyBatteryMonitorYamlCallback(const IntWithHeader & msg);
+void fetchBatteryMonitorYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h b/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h
index 9d7f81e8e9323f5eb9f46c6031e7688f3e9efcdf..db93514fa18e3330e7be65f2a633af1598101e77 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h
@@ -43,14 +43,23 @@
#include <stdlib.h>
#include <ros/ros.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/CrazyflieContext.h"
#include "d_fall_pps/CrazyflieDB.h"
+// Include the DFALL service types
#include "d_fall_pps/CMRead.h"
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
+// Include other classes
#include "CrazyflieIO.h"
@@ -103,6 +112,8 @@ using namespace std;
CrazyflieDB crazyflieDB;
+ros::Publisher m_databaseChangedPublisher;
+
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 91e1f664c7d370a25892dd7a4100b0c8f4f82e8f..ac3ecb916be33a14eac4881c7415b5f3cec6cf16 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,71 @@
// ----------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------
+// U U
+// U U
+// U U
+// U U
+// UUU
+// ----------------------------------------------------------------------------------
+
+
+// Conversions between degrees and radians
+#define RAD2DEG 180.0/PI
+#define DEG2RAD PI/180.0
+
+// PI
+#define PI 3.141592653589
+
+
+
+
+// Types PPS 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
+#define STUDENT_CONTROLLER 3
+#define MPC_CONTROLLER 4
+#define REMOTE_CONTROLLER 5
+#define TUNING_CONTROLLER 6
+#define PICKER_CONTROLLER 7
+
+// The constants that "command" changes in the
+// operation state of this agent
+#define CMD_USE_SAFE_CONTROLLER 1
+#define CMD_USE_DEMO_CONTROLLER 2
+#define CMD_USE_STUDENT_CONTROLLER 3
+#define CMD_USE_MPC_CONTROLLER 4
+#define CMD_USE_REMOTE_CONTROLLER 5
+#define CMD_USE_TUNING_CONTROLLER 6
+#define CMD_USE_PICKER_CONTROLLER 7
+
+
+#define CMD_CRAZYFLY_TAKE_OFF 11
+#define CMD_CRAZYFLY_LAND 12
+#define CMD_CRAZYFLY_MOTORS_OFF 13
+
+// Flying states
+#define STATE_MOTORS_OFF 1
+#define STATE_TAKE_OFF 2
+#define STATE_FLYING 3
+#define STATE_LAND 4
+
+
+// Commands for CrazyRadio
+#define CMD_RECONNECT 0
+#define CMD_DISCONNECT 1
+
+
+// CrazyRadio states:
+#define CRAZY_RADIO_STATE_CONNECTED 0
+#define CRAZY_RADIO_STATE_CONNECTING 1
+#define CRAZY_RADIO_STATE_DISCONNECTED 2
+
@@ -74,6 +139,37 @@
// For where to load the yaml file from
#define LOAD_YAML_FROM_AGENT 1
#define LOAD_YAML_FROM_COORDINATOR 2
+
+
+
+
+
+
+// The types, i.e., agent or coordinator
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 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 9441c08c84fa509a9dee21039ca800e81a1ba421..7d9f825411a1ebde9a4e217496246e43b9116445 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
@@ -77,9 +77,6 @@
// These constants are defined to make the code more readable and adaptable.
-// Universal constants
-#define PI 3.1415926535
-
// 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.
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 f9baaca80dcac8b8cc601c6e7bcbce99fa664b5a..2ab71e041acdb8188504dfd78482777b99a1ca84 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -46,20 +46,29 @@
#include <std_msgs/String.h>
#include <ros/package.h>
-#include "d_fall_pps/Controller.h"
-#include "d_fall_pps/CMQuery.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/ViconData.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/Setpoint.h"
-#include "std_msgs/Int32.h"
-#include "std_msgs/Float32.h"
+
+// Include the DFALL service types
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/CMQuery.h"
// Include the Parameter Service shared definitions
#include "nodes/Constants.h"
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>
@@ -77,58 +86,7 @@
// DDDD EEEEE F III N N EEEEE SSSS
// ----------------------------------------------------------------------------------
-// Types PPS 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
-#define STUDENT_CONTROLLER 3
-#define MPC_CONTROLLER 4
-#define REMOTE_CONTROLLER 5
-#define TUNING_CONTROLLER 6
-#define PICKER_CONTROLLER 7
-
-// The constants that "command" changes in the
-// operation state of this agent
-#define CMD_USE_SAFE_CONTROLLER 1
-#define CMD_USE_DEMO_CONTROLLER 2
-#define CMD_USE_STUDENT_CONTROLLER 3
-#define CMD_USE_MPC_CONTROLLER 4
-#define CMD_USE_REMOTE_CONTROLLER 5
-#define CMD_USE_TUNING_CONTROLLER 6
-#define CMD_USE_PICKER_CONTROLLER 7
-
-
-#define CMD_CRAZYFLY_TAKE_OFF 11
-#define CMD_CRAZYFLY_LAND 12
-#define CMD_CRAZYFLY_MOTORS_OFF 13
-// Flying states
-#define STATE_MOTORS_OFF 1
-#define STATE_TAKE_OFF 2
-#define STATE_FLYING 3
-#define STATE_LAND 4
-
-// Battery states
-#define BATTERY_STATE_NORMAL 0
-#define BATTERY_STATE_LOW 1
-
-// Commands for CrazyRadio
-#define CMD_RECONNECT 0
-#define CMD_DISCONNECT 1
-
-
-// CrazyRadio states:
-#define CONNECTED 0
-#define CONNECTING 1
-#define DISCONNECTED 2
-
-
-// Universal constants
-#define PI 3.141592653589
// Namespacing the package
using namespace d_fall_pps;
@@ -145,8 +103,18 @@ using namespace d_fall_pps;
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
-// "agentID", gives namespace and identifier in CentralManagerService
-int agentID;
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
// The safe controller specified in the ClientConfig.yaml
ros::ServiceClient safeController;
@@ -165,27 +133,36 @@ ros::ServiceClient pickerController;
//values for safteyCheck
-bool strictSafety;
-float angleMargin;
+bool yaml_strictSafety;
+float yaml_angleMargin;
+
+
+
// battery threshold
-float m_battery_threshold_while_flying;
-float m_battery_threshold_while_motors_off;
+//float m_battery_threshold_while_flying;
+//float m_battery_threshold_while_motors_off;
// battery values
-int m_battery_state;
-float m_battery_voltage;
+//int m_battery_state;
+//float m_battery_voltage;
+
+
+
+
Setpoint controller_setpoint;
+std::vector<float> yaml_default_setpoint = {0.0f, 0.0f, 0.0f, 0.0f};
+
// variables for linear trayectory
Setpoint current_safe_setpoint;
double distance;
double unit_vector[3];
bool was_in_threshold = false;
-double distance_threshold; //to be loaded from yaml
+double yaml_distance_threshold; //to be loaded from yaml
ros::ServiceClient centralManager;
@@ -232,12 +209,12 @@ int controller_used;
ros::Publisher controllerUsedPublisher;
-std::string ros_namespace;
-float take_off_distance;
-float landing_distance;
-float duration_take_off;
-float duration_landing;
+
+float yaml_take_off_distance;
+float yaml_landing_distance;
+float yaml_duration_take_off;
+float yaml_duration_landing;
bool finished_take_off = false;
bool finished_land = false;
@@ -271,7 +248,7 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
-void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
+
@@ -279,26 +256,25 @@ void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
void viconCallback(const ViconData& viconData);
-// > For the LOADING of YAML PARAMETERS
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
-void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
+
// > For the {dis/re}-connect command received from the coordinator
+//void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
-// > For the BATTERY
-int getBatteryState();
-void setBatteryStateTo(int new_battery_state);
-float movingAverageBatteryFilter(float new_input);
-void CFBatteryCallback(const std_msgs::Float32& msg);
+
+
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
+void commandCallback(const IntWithHeader & commandMsg);
+
+
void loadSafeController();
@@ -316,6 +292,22 @@ void setControllerUsed(int controller);
int getControllerUsed();
+// > For the BATTERY
+//int getBatteryState();
+//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);
+
+
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
index 665071442bbaf24072ab46e31148bf3565bfa33a..011ff0c73baa4186b05bb15e610f950c15b17d3a 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
@@ -71,9 +71,9 @@
// ----------------------------------------------------------------------------------
// The types, i.e., agent or coordinator
-#define TYPE_INVALID -1
-#define TYPE_COORDINATOR 1
-#define TYPE_AGENT 2
+//#define TYPE_INVALID -1
+//#define TYPE_COORDINATOR 1
+//#define TYPE_AGENT 2
// Namespacing the package
@@ -95,8 +95,7 @@ using namespace d_fall_pps;
// "*.launch" file
int m_type = 0;
-// The ID of this agent, i.e., the ID of this computer in the case that this computer is
-// and agent
+// The ID of this node
int m_ID = 0;
// The namespace into which this parameter service loads yaml parameters
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
index 8b51e35eaf4f0fd8d178126083f448973de9dfa7..6964798373fa15505da00a26d9d0ba5ee951b2b3 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
@@ -79,14 +79,6 @@
// These constants are defined to make the code more readable and adaptable.
-// Universal constants
-#define PI 3.1415926535
-
-#define RAD2DEG 180.0/PI
-#define DEG2RAD PI/180.0
-
-
-
// FOR WHICH BUTTON WAS PRESSED IN THE PICKER CONTOLLER
#define PICKER_BUTTON_GOTOSTART 1
#define PICKER_BUTTON_ATTACH 2
diff --git a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
index 33d91be9f75ea34a59ce2ca6a61e13ec8dbfaf23..8e9f783cb4907327e0643262d157bf66db6d645f 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
@@ -82,9 +82,6 @@
// These constants are defined to make the code more readable and adaptable.
-// Universal constants
-#define PI 3.1415926535
-
// 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.
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 684b813434e2b3434b296f50111cc3d5d9893044..6ead6b7fdb980a1a65aeb0e25420b00646d007ff 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
@@ -73,9 +73,6 @@
// These constants are defined to make the code more readable and adaptable.
-// Universal constants
-#define PI 3.1415926535
-
// 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.
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 478c94ff1e63ccc2a9bd524b0c51f493fa162f05..203680119d3b0bc19231e7caebfac5978a0defa6 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
@@ -77,9 +77,6 @@
// These constants are defined to make the code more readable and adaptable.
-// Universal constants
-#define PI 3.1415926535
-
// 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.
diff --git a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
index 1b950747f99ea0aa0bd04d2f26e6204529fffe1e..8701bc89541548e12685fd2c25fb44f3b7d55d16 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
@@ -82,9 +82,6 @@
// These constants are defined to make the code more readable and adaptable.
-// Universal constants
-#define PI 3.1415926535
-
// 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.
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index f4d2d83d017dd982380d067413f974f3feab64ee..868bb5319675b7e5aa21b83898488df1563fb275 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -119,7 +119,7 @@
type = "ParameterService"
>
<param name="type" type="str" value="agent" />
- <param name="agentID" type="str" value="$(arg agentID)" />
+ <param name="agentID" value="$(arg agentID)" />
<rosparam
command = "load"
file = "$(find d_fall_pps)/param/BatteryMonitor.yaml"
@@ -167,9 +167,12 @@
<group if="$(arg withGUI)">
<node
pkg = "d_fall_pps"
- name = "student_GUI"
+ name = "flyingAgentGUI"
output = "screen"
- type = "student_GUI">
+ type = "flyingAgentGUI"
+ >
+ <param name="type" type="str" value="agent" />
+ <param name="agentID" value="$(arg agentID)" />
</node>
</group>
diff --git a/pps_ws/src/d_fall_pps/launch/Config.sh b/pps_ws/src/d_fall_pps/launch/Config.sh
index 3a8fb3872789b05cf070f8c169a3483a6b52e58c..26f36eae875096698b77ba69c1c1a589487b6740 100755
--- a/pps_ws/src/d_fall_pps/launch/Config.sh
+++ b/pps_ws/src/d_fall_pps/launch/Config.sh
@@ -1,7 +1,7 @@
# TO RUN THE SYSTEM FULLY ON THE LOCAL COMPUTER:
-# export ROS_MASTER_URI=http://localhost:11311
+export ROS_MASTER_URI=http://localhost:11311
# TO RUN THE SYSTEM ON A DEFAULT CONFIGURATION OF THE NETWROK:
-export ROS_MASTER_URI=http://teacher:11311
+# export ROS_MASTER_URI=http://teacher:11311
# OTHER NECESSARY ENVIRONMENT VARIABLES:
export ROS_IP=$(hostname -I | awk '{print $1;}')
export DFALL_DEFAULT_AGENT_ID=$(cat /etc/dfall_default_agent_id)
diff --git a/pps_ws/src/d_fall_pps/launch/Coordinator.launch b/pps_ws/src/d_fall_pps/launch/Coordinator.launch
index 59b085301c78d2fffd9c495ff879f2ab7c8f4d3a..290bd237fb886713c1de222b691589d111426c91 100755
--- a/pps_ws/src/d_fall_pps/launch/Coordinator.launch
+++ b/pps_ws/src/d_fall_pps/launch/Coordinator.launch
@@ -39,44 +39,14 @@
<param name="coordID" value="$(arg coordID)" />
<rosparam
command = "load"
- file = "$(find d_fall_pps)/param/YamlFileNames.yaml"
+ file = "$(find d_fall_pps)/param/BatteryMonitor.yaml"
ns = "YamlFileNames"
/>
<rosparam
command = "load"
- file = "$(find d_fall_pps)/param/SafeController.yaml"
+ file = "$(find d_fall_pps)/param/ClientConfig.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"
- ns = "RemoteController"
- />
- <rosparam
- command = "load"
- file = "$(find d_fall_pps)/param/TuningController.yaml"
- ns = "TuningController"
- />
- <rosparam
- command = "load"
- file = "$(find d_fall_pps)/param/PickerController.yaml"
- ns = "PickerController"
- />
</node>
</group>
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 8b15a220a9b8c73b84a1b27bdf84a53946a7705c..d348c138a2211c0ce0cb7e34b9fef7f5d6d586c0 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -6,6 +6,7 @@ remoteController: "RemoteControllerService/RemoteController"
tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
+# Flag for whether to use angle for switching to the Safe Controller
strictSafety: false
angleMargin: 0.8
diff --git a/pps_ws/src/d_fall_pps/src/classes/GetParamtersAndNamespaces.cpp b/pps_ws/src/d_fall_pps/src/classes/GetParamtersAndNamespaces.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d8d7af9c915b0c02de490223009edd0409ba1b49
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/classes/GetParamtersAndNamespaces.cpp
@@ -0,0 +1,234 @@
+// 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:
+// A class for having the standard functions in one place
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "classes/GetParamtersAndNamespaces.h"
+
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 ( )
+// ----------------------------------------------------------------------------------
+
+
+
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+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");
+ }
+}
+
+
+
+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");
+ }
+}
+
+
+
+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();
+}
+
+
+
+
+bool getAgentIDandCoordIDfromClientNode( std::string client_namespace , int * agentID_ref , int * coordID_ref)
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Create a node handle to the client
+ ros::NodeHandle client_nodeHandle(client_namespace);
+
+ // Get the value of the "agentID" parameter
+ int agentID_fetched;
+ if(!client_nodeHandle.getParam("agentID", agentID_fetched))
+ {
+ return_was_successful = false;
+ }
+ else
+ {
+ *agentID_ref = agentID_fetched;
+ }
+
+ // Get the value of the "coordID" parameter
+ int coordID_fetched;
+ if(!client_nodeHandle.getParam("coordID", coordID_fetched))
+ {
+ return_was_successful = false;
+ }
+ else
+ {
+ *coordID_ref = coordID_fetched;
+ }
+
+ // Return
+ return return_was_successful;
+}
+
+
+
+
+
+void constructNamespaceForCoordinator( int coordID, std::string & coord_namespace )
+{
+ // Convert the ID to a zero padded string
+ std::ostringstream str_stream;
+ str_stream << std::setw(3) << std::setfill('0') << coordID;
+ std::string coordID_as_string(str_stream.str());
+ // Construct the namespace
+ coord_namespace = "/dfall/coord" + coordID_as_string;
+}
+
+
+void constructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace )
+{
+ // Convert the ID to a zero padded string
+ std::ostringstream str_stream;
+ str_stream << std::setw(3) << std::setfill('0') << coordID;
+ std::string coordID_as_string(str_stream.str());
+ // Construct the namespace
+ coord_param_service_namespace = "/dfall/coord" + coordID_as_string + "/ParameterService";
+}
+
+
+
+
+
+// Check the header of a message for whether it is relevant
+bool checkMessageHeader( int agentID , bool shouldCheckForID , const std::vector<uint> & agentIDs )
+{
+ // The messag is by default relevant if the "shouldCheckForID"
+ // flag is false
+ if (!shouldCheckForID)
+ {
+ return true;
+ }
+ else
+ {
+ // Iterate through the vector of IDs
+ for ( int i_ID=0 ; i_ID < agentIDs.size() ; i_ID++ )
+ {
+ if ( agentIDs[i_ID] == agentID )
+ {
+ return true;
+ }
+ }
+ }
+ // If the function made it to here, then the message is
+ // NOT relevant, hence return false
+ return false;
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp b/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
index 15deb2f6bd20c10cf0ede2611010dc28f72c5ea7..fbcf4cee4a12b1f6ffd776a268893a5d65265a9f 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
@@ -128,7 +128,32 @@ void newBatteryVoltageCallback(const std_msgs::Float32& msg)
// > For filtering the battery voltage
float newBatteryVoltageForFilter(float new_value)
{
- return 0.0f;
+ return movingAverageBatteryFilter( new_value);
+}
+
+float movingAverageBatteryFilter(float new_input)
+{
+ const int N = 7;
+ static float previous_output = 4.2f;
+ static float inputs[N] = {4.2f,4.2f,4.2f,4.2f,4.2f,4.2f,4.2f};
+
+
+ // imagine an array of an even number of samples, we will output the one
+ // in the middle averaged with information from all of them.
+ // For that, we only need to store some past of the signal
+ float output = previous_output + new_input/N - inputs[N-1]/N;
+
+ // update array of inputs
+ for(int i = N - 1; i > 0; i--)
+ {
+ inputs[i] = inputs[i-1];
+ }
+ inputs[0] = new_input;
+
+
+ // update previous output
+ previous_output = output;
+ return output;
}
@@ -281,94 +306,62 @@ void updateBatteryStateBasedOnLevel(int level)
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void yamlReadyForFetchCallback(const IntWithHeader & msg)
+
+void isReadyBatteryMonitorYamlCallback(const IntWithHeader & msg)
{
// Check whether the message is relevant
bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
// Continue if the message is relevant
- if (isRevelant)
+ if (isRevelant)
{
// Extract the data
int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
// Load from the respective parameter service
- // Switch between fetching for the different controllers and from different locations
switch(parameter_service_to_load_from)
{
-
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
case LOAD_YAML_FROM_AGENT:
{
- // Let the user know that this message was received
- ROS_INFO("[BATTERY MONITOR] Now fetching the YAML parameter values from this agent.");
- // Create a node handle to the parameter service of this agent
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
- // Call the function that fetches the parameters
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ ROS_INFO("[BATTERY MONITOR] Now fetching the BatteryMonitor 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:
{
- // Let the user know that this message was received
- ROS_INFO("[BATTERY MONITOR] Now fetching the YAML parameter values from this agent's coordinator.");
- // Create a node handle to the parameter service of this agent's coordinator
- ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
- // Call the function that fetches the parameters
- fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ ROS_INFO("[BATTERY MONITOR] Now fetching the BatteryMonitor 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 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.");
+ ROS_ERROR("[BATTERY MONITOR] 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
+ fetchBatteryMonitorYamlParameters(nodeHandle_to_use);
}
}
-// Check the header of a message for whether it is relevant
-bool checkMessageHeader( int agentID , bool shouldCheckForID , const std::vector<uint> & agentIDs )
-{
- // The messag is by default relevant if the "shouldCheckForID"
- // flag is false
- if (!shouldCheckForID)
- {
- return true;
- }
- else
- {
- // Iterate through the vector of IDs
- for ( int i_ID=0 ; i_ID < agentIDs.size() ; i_ID++ )
- {
- if ( agentIDs[i_ID] == agentID )
- {
- return true;
- }
- }
- }
- // If the function made it to here, then the message is
- // NOT relevant, hence return false
- return false;
-}
-// 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 fetchBatteryMonitorYamlParameters(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:
+ // BatteryMonitor.yaml
- // Add the "StudentController" namespace to the "nodeHandle"
+ // Add the "BatteryMonitor" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "BatteryMonitor");
@@ -391,114 +384,11 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[BATTERY MONITOR] DEBUGGING: the fetched BatteryMonitor/battery_polling_period = " << yaml_battery_voltage_threshold_lower_while_flying);
-
- // Call the function that computes details an values that are needed from these
- // parameters loaded above
- processFetchedParameters();
-}
-
+ ROS_INFO_STREAM("[BATTERY MONITOR] DEBUGGING: the fetched BatteryMonitor/battery_voltage_threshold_lower_while_flying = " << yaml_battery_voltage_threshold_lower_while_flying);
-
-// 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);
-}
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
-{
- int val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-
-
-
-
-bool getAgentIDandCoordIDfromClientNode( std::string client_namespace , int * agentID_ref , int * coordID_ref)
-{
- // Initialise the return variable as success
- bool return_was_successful = true;
-
- // Create a node handle to the client
- ros::NodeHandle client_nodeHandle(client_namespace);
-
- // Get the value of the "agentID" parameter
- int agentID_fetched;
- if(!client_nodeHandle.getParam("agentID", agentID_fetched))
- {
- return_was_successful = false;
- }
- else
- {
- *agentID_ref = agentID_fetched;
- }
-
- // Get the value of the "coordID" parameter
- int coordID_fetched;
- if(!client_nodeHandle.getParam("coordID", coordID_fetched))
- {
- return_was_successful = false;
- }
- else
- {
- *coordID_ref = coordID_fetched;
- }
-
- // Return
- return return_was_successful;
-}
-
-
-
-void getConstructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace )
-{
- // 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
- // Convert the agent ID to a zero padded string
- std::ostringstream str_stream;
- str_stream << std::setw(3) << std::setfill('0') << coordID;
- std::string coordID_as_string(str_stream.str());
- coord_param_service_namespace = "/dfall/coord" + coordID_as_string + "/ParameterService";
+ // PROCESS ANY OF THE FETCHED PARAMETERS AS NECESSARY
}
@@ -533,7 +423,7 @@ int main(int argc, char* argv[])
// Stall the node IDs are not valid
if ( !isValid_IDs )
{
- ROS_ERROR("[BATTERY SERVICE] Node NOT FUNCTIONING :-)");
+ ROS_ERROR("[BATTERY MONITOR] Node NOT FUNCTIONING :-)");
ros::spin();
}
else
@@ -551,10 +441,9 @@ int main(int argc, char* argv[])
// Set the class variable "m_namespace_to_coordinator_parameter_service",
// i.e., the namespace of parameter service for this agent's coordinator
- getConstructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
// Inform the user of what namespaces are being used
- ROS_INFO_STREAM("[BATTERY MONITOR] The namespace string for accessing the Paramter Services are:");
ROS_INFO_STREAM("[BATTERY MONITOR] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
ROS_INFO_STREAM("[BATTERY MONITOR] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
@@ -568,14 +457,14 @@ int main(int argc, char* argv[])
// The parameters service publish messages with names of the form:
// /dfall/.../ParameterService/<filename with .yaml extension>
- ros::Subscriber batteryMonitor_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "BatteryMonitor", 1, yamlReadyForFetchCallback);
- ros::Subscriber batteryMonitor_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("BatteryMonitor", 1, yamlReadyForFetchCallback);
+ ros::Subscriber batteryMonitor_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "BatteryMonitor", 1, isReadyBatteryMonitorYamlCallback);
+ ros::Subscriber batteryMonitor_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("BatteryMonitor", 1, isReadyBatteryMonitorYamlCallback);
// FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
// Call the class function that loads the parameters for this class.
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ fetchBatteryMonitorYamlParameters(nodeHandle_to_own_agent_parameter_service);
diff --git a/pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp
index 0034c54e0751c3d280e52dff4ec5aa433d328ab7..375766ad1fe9eaa7d255eddbec9fe35c26b64d16 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp
@@ -157,6 +157,9 @@ bool cmCommand(CMCommand::Request &request, CMCommand::Response &response)
case CMD_SAVE:
{
writeCrazyflieDB(crazyflieDB);
+ std_msgs::Int32 msg;
+ msg.data = 1;
+ m_databaseChangedPublisher.publish(msg);
return true;
}
@@ -164,6 +167,9 @@ bool cmCommand(CMCommand::Request &request, CMCommand::Response &response)
{
crazyflieDB.crazyflieEntries.clear();
readCrazyflieDB(crazyflieDB);
+ std_msgs::Int32 msg;
+ msg.data = 1;
+ m_databaseChangedPublisher.publish(msg);
return true;
}
@@ -195,14 +201,8 @@ int main(int argc, char* argv[])
ros::ServiceServer updateService = nodeHandle.advertiseService("Update", cmUpdate);
ros::ServiceServer commandService = nodeHandle.advertiseService("Command", cmCommand);
- // Get the handle to the namespace in which this coordinator is launched
- //ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
-
- // Subscriber for requests that the controller parameters should be re-loaded from
- // the .YAML files on the coordinators machine, and then all the agents should be
- // request to fetch the parameters from itself, i.e., fetch parameters from the
- // coordinator.
- //ros::Subscriber controllerYamlReadyForFetchSubscriber = namespaceNodeHandle.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, controllerYamlReadyForFetchCallback);
+ // Publisher for when the database is saved
+ m_databaseChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
ROS_INFO("CentralManagerService ready");
diff --git a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
index 031d7bdd5857b34d02022ca21822f41953a9ab14..eb5b88ce6113018a72c375fbf325efff88f22f7b 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
@@ -76,9 +76,6 @@
// These constants are defined to make the code more readable and adaptable.
-// Universal constants
-#define PI 3.1415926535
-
// 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.
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 2d656f30998e3321cf2444ace2661a2e06f12526..0754860eead138793fef4e6c97fd30deb9097190 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -80,12 +80,12 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
//attitude check
//if strictSafety is set to true in ClientConfig.yaml the SafeController takes also over if the roll and pitch angles get to large
//the angleMargin is a value in the range (0,1). The closer to 1, the closer to 90 deg are the roll and pitch angles allowed to become before the safeController takes over
- if(strictSafety){
- if((data.roll > PI/2*angleMargin) or (data.roll < -PI/2*angleMargin)) {
+ if(yaml_strictSafety){
+ if((data.roll > PI/2*yaml_angleMargin) or (data.roll < -PI/2*yaml_angleMargin)) {
ROS_INFO_STREAM("[PPS CLIENT] roll too big.");
return false;
}
- if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
+ if((data.pitch > PI/2*yaml_angleMargin) or (data.pitch < -PI/2*yaml_angleMargin)) {
ROS_INFO_STREAM("[PPS CLIENT] pitch too big.");
return false;
}
@@ -133,7 +133,7 @@ void takeOffCF(CrazyflieData& current_local_coordinates) //local because the set
Setpoint setpoint_msg;
setpoint_msg.x = current_local_coordinates.x; // previous one
setpoint_msg.y = current_local_coordinates.y; //previous one
- setpoint_msg.z = current_local_coordinates.z + take_off_distance; //previous one plus some offset
+ setpoint_msg.z = current_local_coordinates.z + yaml_take_off_distance; //previous one plus some offset
// setpoint_msg.yaw = current_local_coordinates.yaw; //previous one
setpoint_msg.yaw = 0.0;
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
@@ -158,7 +158,7 @@ void landCF(CrazyflieData& current_local_coordinates)
Setpoint setpoint_msg;
setpoint_msg.x = current_local_coordinates.x; // previous one
setpoint_msg.y = current_local_coordinates.y; //previous one
- setpoint_msg.z = landing_distance; //previous one plus some offset
+ setpoint_msg.z = yaml_landing_distance; //previous one plus some offset
setpoint_msg.yaw = current_local_coordinates.yaw; //previous one
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
@@ -170,7 +170,7 @@ void landCF(CrazyflieData& current_local_coordinates)
void changeFlyingStateTo(int new_state)
{
- if(crazyradio_status == CONNECTED)
+ if(crazyradio_status == CRAZY_RADIO_STATE_CONNECTED)
{
ROS_INFO("[PPS CLIENT] Change state to: %d", new_state);
flying_state = new_state;
@@ -236,7 +236,7 @@ void viconCallback(const ViconData& viconData)
takeOffCF(local);
finished_take_off = false;
ROS_INFO("[PPS CLIENT] STATE_TAKE_OFF");
- timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_take_off), takeOffTimerCallback, true);
+ timer_takeoff = nodeHandle.createTimer(ros::Duration(yaml_duration_take_off), takeOffTimerCallback, true);
}
if(finished_take_off)
{
@@ -261,7 +261,7 @@ void viconCallback(const ViconData& viconData)
landCF(local);
finished_land = false;
ROS_INFO("[PPS CLIENT] STATE_LAND");
- timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_landing), landTimerCallback, true);
+ timer_takeoff = nodeHandle.createTimer(ros::Duration(yaml_duration_landing), landTimerCallback, true);
}
if(finished_land)
{
@@ -334,16 +334,16 @@ void viconCallback(const ViconData& viconData)
calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
// ROS_INFO_STREAM("distance: " << distance);
// here, detect if euclidean distance between setpoint and current position is higher than a threshold
- if(distance > distance_threshold)
+ if(distance > yaml_distance_threshold)
{
// DEBUGGING: display a message that the crazyflie is inside the thresholds
//ROS_INFO("inside threshold");
// Declare a local variable for the "setpoint message" to be published
Setpoint setpoint_msg;
// here, where we are now, or where we were in the beginning?
- setpoint_msg.x = local.x + distance_threshold * unit_vector[0];
- setpoint_msg.y = local.y + distance_threshold * unit_vector[1];
- setpoint_msg.z = local.z + distance_threshold * unit_vector[2];
+ setpoint_msg.x = local.x + yaml_distance_threshold * unit_vector[0];
+ setpoint_msg.y = local.y + yaml_distance_threshold * unit_vector[1];
+ setpoint_msg.z = local.z + yaml_distance_threshold * unit_vector[2];
// yaw is better divided by the number of steps?
setpoint_msg.yaw = current_safe_setpoint.yaw;
@@ -393,8 +393,8 @@ void viconCallback(const ViconData& viconData)
void loadCrazyflieContext() {
CMQuery contextCall;
- contextCall.request.studentID = agentID;
- ROS_INFO_STREAM("[PPS CLIENT] AgentID:" << agentID);
+ contextCall.request.studentID = m_agentID;
+ ROS_INFO_STREAM("[PPS CLIENT] AgentID:" << m_agentID);
CrazyflieContext new_context;
@@ -434,91 +434,100 @@ void loadCrazyflieContext() {
-void commandCallback(const std_msgs::Int32& commandMsg) {
- int cmd = commandMsg.data;
+void commandCallback(const IntWithHeader & msg) {
- switch(cmd) {
- case CMD_USE_SAFE_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_SAFE_CONTROLLER Command received");
- setControllerUsed(SAFE_CONTROLLER);
- break;
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
- case CMD_USE_DEMO_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_DEMO_CONTROLLER Command received");
- setControllerUsed(DEMO_CONTROLLER);
- break;
-
- case CMD_USE_STUDENT_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_STUDENT_CONTROLLER Command received");
- setControllerUsed(STUDENT_CONTROLLER);
- break;
-
- case CMD_USE_MPC_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_MPC_CONTROLLER Command received");
- setControllerUsed(MPC_CONTROLLER);
- break;
-
- case CMD_USE_REMOTE_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_REMOTE_CONTROLLER Command received");
- setControllerUsed(REMOTE_CONTROLLER);
- break;
-
- case CMD_USE_TUNING_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_TUNING_CONTROLLER Command received");
- setControllerUsed(TUNING_CONTROLLER);
- break;
-
- case CMD_USE_PICKER_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_PICKER_CONTROLLER Command received");
- setControllerUsed(PICKER_CONTROLLER);
- break;
-
- case CMD_CRAZYFLY_TAKE_OFF:
- if(flying_state == STATE_MOTORS_OFF)
- {
- changeFlyingStateTo(STATE_TAKE_OFF);
- }
- break;
-
- case CMD_CRAZYFLY_LAND:
- if(flying_state != STATE_MOTORS_OFF)
- {
- changeFlyingStateTo(STATE_LAND);
- }
- break;
- case CMD_CRAZYFLY_MOTORS_OFF:
- changeFlyingStateTo(STATE_MOTORS_OFF);
- break;
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int cmd = msg.data;
+
+ switch(cmd) {
+ case CMD_USE_SAFE_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_SAFE_CONTROLLER Command received");
+ setControllerUsed(SAFE_CONTROLLER);
+ break;
+
+ case CMD_USE_DEMO_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_DEMO_CONTROLLER Command received");
+ setControllerUsed(DEMO_CONTROLLER);
+ break;
+
+ case CMD_USE_STUDENT_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_STUDENT_CONTROLLER Command received");
+ setControllerUsed(STUDENT_CONTROLLER);
+ break;
+
+ case CMD_USE_MPC_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_MPC_CONTROLLER Command received");
+ setControllerUsed(MPC_CONTROLLER);
+ break;
+
+ case CMD_USE_REMOTE_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_REMOTE_CONTROLLER Command received");
+ setControllerUsed(REMOTE_CONTROLLER);
+ break;
+
+ case CMD_USE_TUNING_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_TUNING_CONTROLLER Command received");
+ setControllerUsed(TUNING_CONTROLLER);
+ break;
+
+ case CMD_USE_PICKER_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_PICKER_CONTROLLER Command received");
+ setControllerUsed(PICKER_CONTROLLER);
+ break;
+
+ case CMD_CRAZYFLY_TAKE_OFF:
+ if(flying_state == STATE_MOTORS_OFF)
+ {
+ changeFlyingStateTo(STATE_TAKE_OFF);
+ }
+ break;
- default:
- ROS_ERROR_STREAM("[PPS CLIENT] unexpected command number: " << cmd);
- break;
- }
+ case CMD_CRAZYFLY_LAND:
+ if(flying_state != STATE_MOTORS_OFF)
+ {
+ changeFlyingStateTo(STATE_LAND);
+ }
+ break;
+ case CMD_CRAZYFLY_MOTORS_OFF:
+ changeFlyingStateTo(STATE_MOTORS_OFF);
+ break;
+
+ default:
+ ROS_ERROR_STREAM("[PPS CLIENT] unexpected command number: " << cmd);
+ break;
+ }
+ }
}
-void DBChangedCallback(const std_msgs::Int32& msg)
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
{
loadCrazyflieContext();
}
-void emergencyStopCallback(const std_msgs::Int32& msg)
+void emergencyStopCallback(const IntWithHeader & msg)
{
commandCallback(msg);
}
-void commandAllAgentsCallback(const std_msgs::Int32& msg)
-{
- commandCallback(msg);
-}
+//void commandAllAgentsCallback(const std_msgs::Int32& msg)
+//{
+// commandCallback(msg);
+//}
void crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
crazyradio_status = msg.data;
// RESET THE BATTERY STATE IF DISCONNECTED
- if (crazyradio_status == DISCONNECTED)
- {
- setBatteryStateTo(BATTERY_STATE_NORMAL);
- }
+ //if (crazyradio_status == CRAZY_RADIO_STATE_DISCONNECTED)
+ //{
+ // setBatteryStateTo(BATTERY_STATE_NORMAL);
+ //}
}
void controllerSetPointCallback(const Setpoint& newSetpoint)
@@ -540,125 +549,6 @@ void safeSetPointCallback(const Setpoint& newSetpoint)
-// ----------------------------------------------------------------------------------
-// L OOO A DDDD
-// L O O A A D D
-// L O O A A D D
-// L O O AAAAA D D
-// LLLLL OOO A A DDDD
-//
-// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
-// P P A A R R A A MM MM E T E R R S
-// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
-// P AAAAA R R AAAAA M M E T E R R S
-// P A A R R A A M M EEEEE T EEEEE R R SSSS
-// ----------------------------------------------------------------------------------
-
-
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
-{
- // Extract from the "msg" for which controller the and from where to fetch the YAML
- // parameters
- int controller_to_fetch_yaml = msg.data;
-
- // Switch between fetching for the different controllers and from different locations
- switch(controller_to_fetch_yaml)
- {
-
- // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT:
- {
- // Let the user know that this message was received
- ROS_INFO("[PPS CLIENT] Received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from this machine.");
- // 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
- fetchYamlParametersForSafeController(nodeHandle_to_own_agent_parameter_service);
- break;
- }
-
- // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
- {
- // Let the user know that this message was received
- // > and also from where the paramters are being fetched
- ROS_INFO("[PPS CLIENT] Received the message that YAML parameters were (re-)loaded for the Safe Controller. > 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
- fetchYamlParametersForSafeController(nodeHandle_to_coordinator_parameter_service);
- break;
- }
-
- default:
- {
- // Let the user know that the command was not relevant
- //ROS_INFO("The PPSClient received the message that YAML parameters were (re-)loaded.\r> However the parameters do not relate to this service, hence nothing will be fetched.");
- break;
- }
- }
-}
-
-
-
-void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle)
-{
- // Here we load the parameters that are specified in the SafeController.yaml file
-
- // Add the "SafeController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
-
- if(!nodeHandle_for_safeController.getParam("takeOffDistance", take_off_distance))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get takeOffDistance");
- }
-
- if(!nodeHandle_for_safeController.getParam("landingDistance", landing_distance))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get landing_distance");
- }
-
- if(!nodeHandle_for_safeController.getParam("durationTakeOff", duration_take_off))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get duration_take_off");
- }
-
- if(!nodeHandle_for_safeController.getParam("durationLanding", duration_landing))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get duration_landing");
- }
-
- if(!nodeHandle_for_safeController.getParam("distanceThreshold", distance_threshold))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get distance_threshold");
- }
-}
-
-
-// > Load the paramters from the Client Config YAML file
-void fetchClientConfigParameters(ros::NodeHandle& nodeHandle)
-{
- if(!nodeHandle.getParam("agentID", agentID)) {
- ROS_ERROR("[PPS CLIENT] Failed to get agentID");
- }
- if(!nodeHandle.getParam("strictSafety", strictSafety)) {
- ROS_ERROR("[PPS CLIENT] Failed to get strictSafety param");
- return;
- }
- if(!nodeHandle.getParam("angleMargin", angleMargin)) {
- ROS_ERROR("[PPS CLIENT] Failed to get angleMargin param");
- return;
- }
- if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
- ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_flying param");
- return;
- }
- if(!nodeHandle.getParam("battery_threshold_while_motors_off", m_battery_threshold_while_motors_off)) {
- ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_motors_off param");
- return;
- }
-}
-
@@ -687,95 +577,6 @@ void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg)
-int getBatteryState()
-{
- return m_battery_state;
-}
-
-
-void setBatteryStateTo(int new_battery_state)
-{
- switch(new_battery_state)
- {
- case BATTERY_STATE_NORMAL:
- m_battery_state = BATTERY_STATE_NORMAL;
- ROS_INFO("[PPS CLIENT] changed battery state to normal");
- break;
- case BATTERY_STATE_LOW:
- m_battery_state = BATTERY_STATE_LOW;
- ROS_INFO("[PPS CLIENT] changed battery state to low");
- if(flying_state != STATE_MOTORS_OFF)
- changeFlyingStateTo(STATE_LAND);
- break;
- default:
- ROS_INFO("[PPS CLIENT] Unknown battery state command, set to normal");
- m_battery_state = BATTERY_STATE_NORMAL;
- break;
- }
-
- std_msgs::Int32 battery_state_msg;
- battery_state_msg.data = getBatteryState();
- batteryStatePublisher.publish(battery_state_msg);
-}
-
-float movingAverageBatteryFilter(float new_input)
-{
- const int N = 7;
- static float previous_output = 4.2f;
- static float inputs[N] = {4.2f,4.2f,4.2f,4.2f,4.2f,4.2f,4.2f};
-
-
- // imagine an array of an even number of samples, we will output the one
- // in the middle averaged with information from all of them.
- // For that, we only need to store some past of the signal
- float output = previous_output + new_input/N - inputs[N-1]/N;
-
- // update array of inputs
- for(int i = N - 1; i > 0; i--)
- {
- inputs[i] = inputs[i-1];
- }
- inputs[0] = new_input;
-
-
- // update previous output
- previous_output = output;
- return output;
-}
-
-
-void CFBatteryCallback(const std_msgs::Float32& msg)
-{
- m_battery_voltage = msg.data;
- // filter and check if inside limits, and if, change status
- // need to do the filtering first
- float filtered_battery_voltage = movingAverageBatteryFilter(m_battery_voltage); //need to perform filtering here
-
- // ROS_INFO_STREAM("filtered data: " << filtered_battery_voltage);
- if(
- (flying_state != STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_flying))
- ||
- (flying_state == STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_motors_off))
- )
- {
- if(getBatteryState() != BATTERY_STATE_LOW)
- {
- setBatteryStateTo(BATTERY_STATE_LOW);
- ROS_INFO("[PPS CLIENT] low level battery triggered");
- }
-
- }
- else
- {
- // TO AVOID BEING ABLE TO IMMEDIATELY TAKE-OFF AFTER A
- // "BATTERY LOW" EVENT IS TRIGGERED, WE DO NOT SET THE
- // BATTERY STATE BACK TO NORMAL
- // if(getBatteryState() != BATTERY_STATE_NORMAL)
- // {
- // setBatteryStateTo(BATTERY_STATE_NORMAL);
- // }
- }
-}
@@ -786,6 +587,13 @@ void CFBatteryCallback(const std_msgs::Float32& msg)
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
@@ -959,110 +767,505 @@ int getControllerUsed()
-
-
-
// ----------------------------------------------------------------------------------
-// M M A III N N
-// MM MM A A I NN N
-// M M M A A I N N N
-// M M AAAAA I N NN
-// M M A A III N N
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
// ----------------------------------------------------------------------------------
-int main(int argc, char* argv[])
-{
- ros::init(argc, argv, "PPSClient");
- ros::NodeHandle nodeHandle("~");
- ros_namespace = ros::this_node::getNamespace();
-
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
-
- // > Load the paramters from the Client Config YAML file
- fetchClientConfigParameters(nodeHandle);
-
- // Get the namespace of this "SafeControllerService" node
- std::string m_namespace = ros::this_node::getNamespace();
-
-
- // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN 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 = "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);
-
-
- // 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);
-
- // 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("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
-
-
- // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
-
- // Call the class function that loads the parameters for this class.
- fetchYamlParametersForSafeController(nodeHandle_to_own_agent_parameter_service);
-
- // *********************************************************************************
- // Load default setpoint from the "SafeController" namespace of the "ParameterService"
- std::vector<float> default_setpoint(4);
- ros::NodeHandle nodeHandle_for_safeController(nodeHandle_to_own_agent_parameter_service, "SafeController");
+void batteryMonitorStateChangedCallback(std_msgs::Int32 msg)
+{
+ // Extract the data
+ int new_battery_state = msg.data;
- if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
+ // Take action if changed to low battery state
+ if ((flying_state != STATE_MOTORS_OFF) && (new_battery_state == BATTERY_STATE_LOW))
+ {
+ ROS_INFO("[PPS CLIENT] low level battery triggered, now landing.");
+ changeFlyingStateTo(STATE_LAND);
+ }
+ else if ((flying_state == STATE_MOTORS_OFF) && (new_battery_state == BATTERY_STATE_LOW))
{
- ROS_ERROR_STREAM("[PPS CLIENT] Could not find parameter 'defaultSetpoint', as called from main(...)");
+ ROS_INFO("[PPS CLIENT] low level battery triggered, please turn off the Crazyflie.");
}
+}
- // Copy the default setpoint into the class variable
- controller_setpoint.x = default_setpoint[0];
- controller_setpoint.y = default_setpoint[1];
- controller_setpoint.z = default_setpoint[2];
- controller_setpoint.yaw = default_setpoint[3];
- //ros::service::waitForService("/CentralManagerService/CentralManager");
- // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
- ros::NodeHandle dfall_root_nodeHandle("/dfall");
- centralManager = dfall_root_nodeHandle.serviceClient<CMQuery>("CentralManagerService/Query", false);
- loadCrazyflieContext();
- //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
- ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
- ROS_INFO_STREAM("[PPS CLIENT] successfully subscribed to ViconData");
+/*
+int getBatteryState()
+{
+ return m_battery_state;
+}
- //ros::Publishers to advertise the control output
- controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
+
+void setBatteryStateTo(int new_battery_state)
+{
+ switch(new_battery_state)
+ {
+ case BATTERY_STATE_NORMAL:
+ m_battery_state = BATTERY_STATE_NORMAL;
+ ROS_INFO("[PPS CLIENT] changed battery state to normal");
+ break;
+ case BATTERY_STATE_LOW:
+ m_battery_state = BATTERY_STATE_LOW;
+ ROS_INFO("[PPS CLIENT] changed battery state to low");
+ if(flying_state != STATE_MOTORS_OFF)
+ changeFlyingStateTo(STATE_LAND);
+ break;
+ default:
+ ROS_INFO("[PPS CLIENT] Unknown battery state command, set to normal");
+ m_battery_state = BATTERY_STATE_NORMAL;
+ break;
+ }
+
+ std_msgs::Int32 battery_state_msg;
+ battery_state_msg.data = getBatteryState();
+ batteryStatePublisher.publish(battery_state_msg);
+}
+
+float movingAverageBatteryFilter(float new_input)
+{
+ const int N = 7;
+ static float previous_output = 4.2f;
+ static float inputs[N] = {4.2f,4.2f,4.2f,4.2f,4.2f,4.2f,4.2f};
+
+
+ // imagine an array of an even number of samples, we will output the one
+ // in the middle averaged with information from all of them.
+ // For that, we only need to store some past of the signal
+ float output = previous_output + new_input/N - inputs[N-1]/N;
+
+ // update array of inputs
+ for(int i = N - 1; i > 0; i--)
+ {
+ inputs[i] = inputs[i-1];
+ }
+ inputs[0] = new_input;
+
+
+ // update previous output
+ previous_output = output;
+ return output;
+}
+
+
+void CFBatteryCallback(const std_msgs::Float32& msg)
+{
+ m_battery_voltage = msg.data;
+ // filter and check if inside limits, and if, change status
+ // need to do the filtering first
+ float filtered_battery_voltage = movingAverageBatteryFilter(m_battery_voltage); //need to perform filtering here
+
+ // ROS_INFO_STREAM("filtered data: " << filtered_battery_voltage);
+ if(
+ (flying_state != STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_flying))
+ ||
+ (flying_state == STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_motors_off))
+ )
+ {
+ if(getBatteryState() != BATTERY_STATE_LOW)
+ {
+ setBatteryStateTo(BATTERY_STATE_LOW);
+ ROS_INFO("[PPS CLIENT] low level battery triggered");
+ }
+
+ }
+ else
+ {
+ // TO AVOID BEING ABLE TO IMMEDIATELY TAKE-OFF AFTER A
+ // "BATTERY LOW" EVENT IS TRIGGERED, WE DO NOT SET THE
+ // BATTERY STATE BACK TO NORMAL
+ // if(getBatteryState() != BATTERY_STATE_NORMAL)
+ // {
+ // setBatteryStateTo(BATTERY_STATE_NORMAL);
+ // }
+ }
+}
+*/
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+
+void isReadySafeControllerYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[PPS CLIENT] Now fetching the SafeController 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("[PPS CLIENT] Now fetching the SafeController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[PPS CLIENT] 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
+ fetchSafeControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+
+void fetchSafeControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // SafeController.yaml
+
+ // Add the "SafeController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "SafeController");
+
+ // take off and landing parameters (in meters and seconds)
+ yaml_take_off_distance = getParameterFloat(nodeHandle_for_paramaters,"takeOffDistance");
+ yaml_landing_distance = getParameterFloat(nodeHandle_for_paramaters,"landingDistance");
+ yaml_duration_take_off = getParameterFloat(nodeHandle_for_paramaters,"durationTakeOff");
+ yaml_duration_landing = getParameterFloat(nodeHandle_for_paramaters,"durationLanding");
+ yaml_distance_threshold = getParameterFloat(nodeHandle_for_paramaters,"distanceThreshold");
+
+ // setpoint in meters (x, y, z, yaw)
+ getParameterFloatVector(nodeHandle_for_paramaters,"defaultSetpoint",yaml_default_setpoint,4);
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[PPS CLIENT] DEBUGGING: the fetched SafeController/durationTakeOff = " << yaml_duration_take_off);
+
+ /*
+ // Here we load the parameters that are specified in the SafeController.yaml file
+
+ // Add the "SafeController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
+
+ if(!nodeHandle_for_safeController.getParam("takeOffDistance", take_off_distance))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get takeOffDistance");
+ }
+
+ if(!nodeHandle_for_safeController.getParam("landingDistance", landing_distance))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get landing_distance");
+ }
+
+ if(!nodeHandle_for_safeController.getParam("durationTakeOff", duration_take_off))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get duration_take_off");
+ }
+
+ if(!nodeHandle_for_safeController.getParam("durationLanding", duration_landing))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get duration_landing");
+ }
+
+ if(!nodeHandle_for_safeController.getParam("distanceThreshold", distance_threshold))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get distance_threshold");
+ }
+ */
+}
+
+
+
+
+void isReadyClientConfigYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[PPS CLIENT] Now fetching the ClientConfig 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("[PPS CLIENT] Now fetching the ClientConfig YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[PPS CLIENT] 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
+ fetchClientConfigYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+
+// > Load the paramters from the Client Config YAML file
+void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // ClientConfig.yaml
+
+ // Add the "ClientConfig" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "ClientConfig");
+
+ // Flag for whether to use angle for switching to the Safe Controller
+ yaml_strictSafety = getParameterBool(nodeHandle_for_paramaters,"strictSafety");
+ yaml_angleMargin = getParameterFloat(nodeHandle_for_paramaters,"angleMargin");
+
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[PPS CLIENT] DEBUGGING: the fetched ClientConfig/strictSafety = " << yaml_strictSafety);
+
+
+ /*
+ if(!nodeHandle.getParam("strictSafety", strictSafety)) {
+ ROS_ERROR("[PPS CLIENT] Failed to get strictSafety param");
+ return;
+ }
+ if(!nodeHandle.getParam("angleMargin", angleMargin)) {
+ ROS_ERROR("[PPS CLIENT] Failed to get angleMargin param");
+ return;
+ }
+
+
+ if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
+ ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_flying param");
+ return;
+ }
+ if(!nodeHandle.getParam("battery_threshold_while_motors_off", m_battery_threshold_while_motors_off)) {
+ ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_motors_off param");
+ return;
+ }
+ */
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+int main(int argc, char* argv[])
+{
+ // Starting the ROS-node
+ ros::init(argc, argv, "PPSClient");
+
+ // Create a "ros::NodeHandle" type local variable named "nodeHandle",
+ // the "~" indcates that "self" is the node handle assigned.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PPS Client] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // 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("[PPS Client] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[PPS Client] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // 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("[PPS Client] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[PPS Client] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameters service publish 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);
+
+ ros::Subscriber safeController_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "SafeController", 1, isReadySafeControllerYamlCallback);
+ ros::Subscriber safeController_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("SafeController", 1, isReadySafeControllerYamlCallback);
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ fetchClientConfigYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ fetchSafeControllerYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+
+
+
+
+
+
+ // Copy the default setpoint into the class variable
+ controller_setpoint.x = yaml_default_setpoint[0];
+ controller_setpoint.y = yaml_default_setpoint[1];
+ controller_setpoint.z = yaml_default_setpoint[2];
+ controller_setpoint.yaw = yaml_default_setpoint[3];
+
+
+
+
+
+
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+
+
+
+
+ // SERVICE CLIENT FOR THE ALLOWED FLYING ZONE AND OTHER CONTEXT DETAILS
+
+ //ros::service::waitForService("/CentralManagerService/CentralManager");
+ centralManager = nodeHandle_dfall_root.serviceClient<CMQuery>("CentralManagerService/Query", false);
+ loadCrazyflieContext();
+
+ // Subscriber for when the Flying Zone Database changed
+ ros::Subscriber databaseChangedSubscriber = nodeHandle_dfall_root.subscribe("CentralManagerService/DBChanged", 1, crazyflieContextDatabaseChangedCallback);
+
+
+
+
+
+ // EMERGENCY STOP OF THE WHOLE "D-FaLL-System"
+ ros::Subscriber emergencyStopSubscriber = nodeHandle_dfall_root.subscribe("emergencyStop", 1, emergencyStopCallback);
+
+
+
+
+
+ // LOCALISATION DATA FROM MOTION CAPTURE SYSTEM
+
+ //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
+ ros::Subscriber viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, viconCallback);
+
+
+
+
+ // PUBLISHER FOR COMMANDING THE CRAZYFLIE
+ // i.e., motorCmd{1,2,3,4}, roll, pitch, yaw, and onboardControllerType
+ //ros::Publishers to advertise the control output
+ controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
//this topic lets the PPSClient listen to the terminal commands
- commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
- ros::Subscriber commandSubscriber = nodeHandle.subscribe("Command", 1, commandCallback);
+ //commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
+
+
+
+
+
+ // CREATE A NODE HANDLE TO THE COORDINATOR
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+
+
+
+
+
+ // SUBSCRIBER FOR {TAKE-OFF,LAND,MOTORS-OFF} AND CONTROLLER SELECTION
+ // > for the agent GUI
+ ros::Subscriber commandSubscriber_to_agent = nodeHandle.subscribe("Command", 1, commandCallback);
+ // > for the coord GUI
+ ros::Subscriber commandSubscriber_to_coord = nodeHandle_to_coordinator.subscribe("PPSClient/Command", 1, commandCallback);
+
+
+
+
//this topic lets us use the terminal to communicate with crazyRadio node.
crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
@@ -1070,13 +1273,14 @@ int main(int argc, char* argv[])
// this topic will publish flying state whenever it changes.
flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
- // will publish battery state when it changes
- batteryStatePublisher = nodeHandle.advertise<std_msgs::Int32>("batteryState", 1);
-
+
controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
+
+
+
// crazy radio status
- crazyradio_status = DISCONNECTED;
+ crazyradio_status = CRAZY_RADIO_STATE_DISCONNECTED;
// publish first flying state data
std_msgs::Int32 flying_state_msg;
@@ -1089,14 +1293,11 @@ int main(int argc, char* argv[])
ros::Subscriber controllerSetpointSubscriber = namespaceNodeHandle.subscribe("student_GUI/ControllerSetpoint", 1, controllerSetPointCallback);
ros::Subscriber safeSetpointSubscriber = namespaceNodeHandle.subscribe("SafeControllerService/Setpoint", 1, safeSetPointCallback);
- // subscriber for DBChanged
- ros::Subscriber DBChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, DBChangedCallback);
-
- // subscriber for emergencyStop
- ros::Subscriber emergencyStopSubscriber = namespaceNodeHandle.subscribe("/my_GUI/emergencyStop", 1, emergencyStopCallback);
+
+
// Subscriber for "commandAllAgents" commands that are sent from the coordinator node
- ros::Subscriber commandAllAgentsSubscriber = namespaceNodeHandle.subscribe("/my_GUI/commandAllAgents", 1, commandAllAgentsCallback);
+ //ros::Subscriber commandAllAgentsSubscriber = namespaceNodeHandle.subscribe("/my_GUI/commandAllAgents", 1, commandAllAgentsCallback);
// crazyradio status. Connected, connecting or disconnected
ros::Subscriber crazyRadioStatusSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);
@@ -1104,20 +1305,34 @@ int main(int argc, char* argv[])
// Subscriber for "crazyRadioCommandAllAgents" commands that are sent from the coordinator node
ros::Subscriber crazyRadioCommandAllAgentsSubscriber = namespaceNodeHandle.subscribe("/my_GUI/crazyRadioCommandAllAgents", 1, crazyRadioCommandAllAgentsCallback);
- // know the battery level of the CF
- ros::Subscriber CFBatterySubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CFBattery", 1, CFBatteryCallback);
+
+
+
+
+ // will publish battery state when it changes
+ //batteryStatePublisher = nodeHandle.advertise<std_msgs::Int32>("batteryState", 1);
// INITIALISE THE BATTERY VOLTAGE TO SOMETHING CLOSE TO FULL
// > This is used to prevent the "Low Battery" being trigged when the
// first battery voltage data is received
- m_battery_voltage = 4.2f;
+ //m_battery_voltage = 4.2f;
+ // know the battery level of the CF
+ //ros::Subscriber CFBatterySubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CFBattery", 1, CFBatteryCallback);
+
+ //setBatteryStateTo(BATTERY_STATE_NORMAL); //initialize battery state
+
+ // Subscribe to the battery state change message from the Battery Monitor node
+ std::string namespace_to_battery_monitor = m_namespace + "/BatteryMonitor";
+ ros::NodeHandle nodeHandle_to_battery_monitor(namespace_to_battery_monitor);
+ ros::Subscriber CFBatterySubscriber = nodeHandle_to_battery_monitor.subscribe("ChangedStateTo", 1, batteryMonitorStateChangedCallback);
+
//start with safe controller
flying_state = STATE_MOTORS_OFF;
setControllerUsed(SAFE_CONTROLLER);
setInstantController(SAFE_CONTROLLER); //initialize this also, so we notify GUI
- setBatteryStateTo(BATTERY_STATE_NORMAL); //initialize battery state
+
// Open a ROS "bag" to store data for post-analysis
// std::string package_path;
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 934e978d9a642cdd531cbd70d6483ba8762e4d58..0aa0aefed9e04232e1c30116b78bbf8dc4989d90 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,8 @@ 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<StringWithHeader>(yaml_filename_to_load, 1);
+ ros::Publisher yamlParametersReadyForFetchPublisher = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1);
+ //ros::spinOnce();
// Create a local variable for the message
IntWithHeader yaml_ready_msg;
// Specify with the data the "type" of this parameter service
@@ -277,7 +278,7 @@ void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_lo
}
case TYPE_COORDINATOR:
{
- yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ yaml_ready_msg.data = LOAD_YAML_FROM_COORDINATOR;
break;
}
default:
@@ -294,14 +295,18 @@ void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_lo
// Copy across the vector of IDs
if (yaml_filename_to_load_with_header.shouldCheckForID)
{
- int length_of_IDs = yaml_filename_to_load_with_header.agentIDs.size();
- for ( int i_ID=0 ; i_ID<length_of_IDs ; i_ID++ )
+ for ( int i_ID=0 ; i_ID<yaml_filename_to_load_with_header.agentIDs.size() ; i_ID++ )
{
yaml_ready_msg.agentIDs.push_back(yaml_filename_to_load_with_header.agentIDs[i_ID]);
}
}
+ // Sleep to make the publisher known to ROS (in seconds)
+ ros::Duration(1.0).sleep();
// Send the message
yamlParametersReadyForFetchPublisher.publish(yaml_ready_msg);
+
+ // Inform the user that this was published
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
}
@@ -472,7 +477,7 @@ int main(int argc, char* argv[])
// loads yaml parameters
bool isValid_namespaces = constructNamespaces();
- // Stall the parameter service is the TYPE and ID are not valid
+ // Stall if the TYPE and ID are not valid
if ( !( isValid_type_and_ID && isValid_namespaces ) )
{
ROS_ERROR("[PARAMETER SERVICE] Service NOT FUNCTIONING :-)");