Commit c61f7547 authored by beuchatp's avatar beuchatp
Browse files

Have setup the basic framework of the coordinator connections. Next step to...

Have setup the basic framework of the coordinator connections. Next step to connect all GUI buttons for (dis-)connect, start,stop,off,and select controller. And then test.
parent 1f3225f7
......@@ -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})
......
......@@ -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
......
......@@ -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()
......
......@@ -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
......
......@@ -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
};
......
......@@ -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
};
......
......@@ -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
......
......@@ -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
......@@ -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