diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 08a1abf807c90cebdc13b201025a4d0d94cfa34d..3e92c2eb1509ae3176e774a68ff1bbbff0bb5dc5 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -134,6 +134,8 @@ add_message_files(
CrazyflieDB.msg
#----------------------------------------------------------------------
DebugMsg.msg
+ CustomControllerYAML.msg
+ CustomButton.msg
)
## Generate services in the 'srv' folder
@@ -238,6 +240,7 @@ add_executable(PPSClient src/PPSClient.cpp)
add_executable(SafeControllerService src/SafeControllerService.cpp)
add_executable(CustomControllerService src/CustomControllerService.cpp)
add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
+add_executable(ParameterService src/ParameterService.cpp)
add_executable(CircleControllerService src/CircleControllerService.cpp)
add_executable(FollowCrazyflieService src/FollowCrazyflieService.cpp)
add_executable(FollowN_1Service src/FollowN_1Service.cpp)
@@ -286,6 +289,7 @@ add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TA
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CustomControllerService 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(CircleControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(FollowCrazyflieService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(FollowN_1Service d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -324,6 +328,8 @@ target_link_libraries(CustomControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
+target_link_libraries(ParameterService ${catkin_LIBRARIES})
+
target_link_libraries(CircleControllerService ${catkin_LIBRARIES})
target_link_libraries(FollowCrazyflieService ${catkin_LIBRARIES})
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user
index 13d53214d559b938f2acc8a24ac64a7286603676..c3bd5196277cff69c0be75d8070cd6cb7bc53467 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 3.5.1, 2017-09-08T11:50:18. -->
+<!-- Written by QtCreator 3.5.1, 2017-12-05T07:54:15. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
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 9e2fd35149985a824038b533c13d41a2210786ec..e9a8b5b6a268bc1c44a44732109aa870bbeb6d90 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
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
//
// This file is part of D-FaLL-System.
//
@@ -48,11 +48,31 @@
#include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieEntry.h"
+#include "d_fall_pps/CustomControllerYAML.h"
#include <std_msgs/Int32.h>
+// The constants that are sent to the agents in order to
+// "command" changes in their operation state
+#define CMD_USE_SAFE_CONTROLLER 1
+#define CMD_USE_CUSTOM_CONTROLLER 2
+#define CMD_CRAZYFLY_TAKE_OFF 3
+#define CMD_CRAZYFLY_LAND 4
#define CMD_CRAZYFLY_MOTORS_OFF 5
+
+// The constants that are sent to the agents in order to
+// adjust their radio connection
+#define CMD_RECONNECT 0
+#define CMD_DISCONNECT 1
+
+// For which controller parameters to load
+#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
+#define LOAD_YAML_CUSTOM_CONTROLLER_AGENT 2
+#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 3
+#define LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+
+
using namespace d_fall_pps;
#endif
@@ -166,14 +186,47 @@ private slots:
void on_comboBoxCFs_currentTextChanged(const QString &arg1);
+
+ // For the buttons that "command" all of the agent nodes
+ // > For the radio connection
+ void on_all_connect_button_clicked();
+ void on_all_disconnect_button_clicked();
+ // > For changing the operation state
+ void on_all_take_off_button_clicked();
+ void on_all_land_button_clicked();
void on_all_motors_off_button_clicked();
+ void on_all_enable_safe_controller_button_clicked();
+ void on_all_enable_custom_controller_button_clicked();
+ // > For loading the parameter
+ void on_all_load_safe_controller_yaml_own_agent_button_clicked();
+ void on_all_load_custom_controller_yaml_own_agent_button_clicked();
+ // > For sending a message with updated parameters
+ void on_all_load_safe_controller_yaml_coordinator_button_clicked();
+ void on_all_load_custom_controller_yaml_coordinator_button_clicked();
+
private:
Ui::MainGUIWindow *ui;
myGraphicsScene* scene;
+
+ ros::Timer m_timer_yaml_file_for_safe_controller;
+ ros::Timer m_timer_yaml_file_for_custom_controller;
+
void _init();
+ void safeYamlFileTimerCallback(const ros::TimerEvent&);
+ void customYamlFileTimerCallback(const ros::TimerEvent&);
+
+ void customSendYamlAsMessageTimerCallback(const ros::TimerEvent&);
+
+ float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+ void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+ int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+ void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+ int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+
#ifdef CATKIN_MAKE
rosNodeThread* _rosNodeThread;
@@ -181,8 +234,29 @@ private:
std::vector<crazyFly*> crazyflies_vector;
CFLinker* cf_linker;
+ std::string ros_namespace;
+
ros::Publisher DBChangedPublisher;
ros::Publisher emergencyStopPublisher;
+
+ // Publsher for sending "commands" from here (the master) to all
+ // of the agent nodes (where a "command" is the integer that
+ // gives the directive to "take-off", "land, "motors-off", etc...)
+ ros::Publisher commandAllAgentsPublisher;
+
+ // Publisher for sending a request from here (the master) to all "Parameter Service" nodes
+ // that it should re-load parameters from the YAML file for the controllers.
+ // > This is recieved and acted on by both Coordinate and Agent type "Parameter Services",
+ // > A coordinator type "Parameter Service" will subsequently request the agents to fetch
+ // the parameters from itself.
+ // > A agent type "Parameter Service" will subsequently request its own agent to fetch
+ // the parameters from itself.
+ ros::Publisher requestLoadControllerYamlPublisher;
+
+ // Publisher for sending a request from here (the master) to all
+ // of the agents nodes that they should (re/dis)-connect from
+ // the Crazy-Radio
+ ros::Publisher crazyRadioCommandAllAgentsPublisher;
#endif
void updateComboBoxesCFs();
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 cc48cdca6537d2c679139890f4d7eb5e3eca5f69..7bb024784a2180f2d788138a63eacab83475ffe2 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
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
//
// This file is part of D-FaLL-System.
//
@@ -229,9 +229,30 @@ void MainGUIWindow::_init()
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
QObject::connect(cf_linker, SIGNAL(updateComboBoxes()), this, SLOT(updateComboBoxes()));
+
+ 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);
+
+ // Initialise the publisher for sending "commands" from here (the master)
+ // to all of the agent nodes
+ commandAllAgentsPublisher = nodeHandle.advertise<std_msgs::Int32>("commandAllAgents", 1);
+
+ // Publisher for sending a request from here (the master) to all "Parameter Service" nodes
+ // that it should re-load parameters from the YAML file for the controllers.
+ // > This is recieved and acted on by both Coordinate and Agent type "Parameter Services",
+ // > A coordinator type "Parameter Service" will subsequently request the agents to fetch
+ // the parameters from itself.
+ // > A agent type "Parameter Service" will subsequently request its own agent to fetch
+ // the parameters from itself.
+ requestLoadControllerYamlPublisher = nodeHandle.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
+
+ // Initialise the publisher for sending a request from here (the master)
+ // to all of the agents nodes that they should (re/dis)-connect from
+ // the Crazy-Radio
+ crazyRadioCommandAllAgentsPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommandAllAgents", 1);
#endif
}
@@ -962,9 +983,316 @@ void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
}
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC OOO M M M M A N N DDDD A L L
+// C O O MM MM MM MM A A NN N D D A A L L
+// C O O M M M M M M A A N N N D D A A L L
+// C O O M M M M AAAAA N NN D D AAAAA L L
+// CCCC OOO M M M M A A N N DDDD A A LLLLL LLLLL
+//
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+// For the buttons that commands all of the agent nodes to:
+// > (RE)CONNECT THE RADIO
+void MainGUIWindow::on_all_connect_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_RECONNECT;
+ crazyRadioCommandAllAgentsPublisher.publish(msg);
+}
+// > DISCONNECT THE RADIO
+void MainGUIWindow::on_all_disconnect_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_DISCONNECT;
+ crazyRadioCommandAllAgentsPublisher.publish(msg);
+}
+// > TAKE-OFF
+void MainGUIWindow::on_all_take_off_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_CRAZYFLY_TAKE_OFF;
+ commandAllAgentsPublisher.publish(msg);
+}
+// > LAND
+void MainGUIWindow::on_all_land_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_CRAZYFLY_LAND;
+ commandAllAgentsPublisher.publish(msg);
+}
+// > MOTORS OFF
void MainGUIWindow::on_all_motors_off_button_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
- emergencyStopPublisher.publish(msg);
+ commandAllAgentsPublisher.publish(msg);
+ //emergencyStopPublisher.publish(msg);
+}
+// > ENABLE SAFE CONTROLLER
+void MainGUIWindow::on_all_enable_safe_controller_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_SAFE_CONTROLLER;
+ commandAllAgentsPublisher.publish(msg);
+}
+// > ENABLE SAFE CONTROLLER
+void MainGUIWindow::on_all_enable_custom_controller_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_CUSTOM_CONTROLLER;
+ commandAllAgentsPublisher.publish(msg);
+}
+// > LOAD THE YAML PARAMETERS FOR THE SAFE CONTROLLER
+void MainGUIWindow::on_all_load_safe_controller_yaml_own_agent_button_clicked()
+{
+ // Disable the button
+ ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(false);
+ ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(false);
+
+ // Send the message that the YAML paremters should be loaded
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
+ requestLoadControllerYamlPublisher.publish(msg);
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ ros::NodeHandle nodeHandle("~");
+ m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::safeYamlFileTimerCallback, this, true);
+}
+// > LOAD THE YAML PARAMETERS FOR THE CUSTOM CONTROLLER
+void MainGUIWindow::on_all_load_custom_controller_yaml_own_agent_button_clicked()
+{
+ // Disable the button
+ ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(false);
+ ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(false);
+
+ // Send the message that the YAML paremters should be loaded
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT;
+ requestLoadControllerYamlPublisher.publish(msg);
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ ros::NodeHandle nodeHandle("~");
+ m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
+
+}
+// > SEND THE YAML PARAMETERS FOR THE SAFE CONTROLLER
+void MainGUIWindow::on_all_load_safe_controller_yaml_coordinator_button_clicked()
+{
+ // Disable the button
+ ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(false);
+ ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(false);
+
+ // Send the message that the YAML paremters should be loaded
+ // by the coordinator (and then the agent informed)
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_SAFE_CONTROLLER_COORDINATOR;
+ requestLoadControllerYamlPublisher.publish(msg);
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ ros::NodeHandle nodeHandle("~");
+ m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::safeYamlFileTimerCallback, this, true);
+}
+// > SEND THE YAML PARAMETERS FOR THE CUSTOM CONTROLLER
+void MainGUIWindow::on_all_load_custom_controller_yaml_coordinator_button_clicked()
+{
+ // Disable the button
+ ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(false);
+ ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(false);
+
+ // Send the message that the YAML paremters should be loaded
+ // by the coordinator (and then the agent informed)
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR;
+ requestLoadControllerYamlPublisher.publish(msg);
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ ros::NodeHandle nodeHandle("~");
+ m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
+
+}
+// > CALLBACK TO RE-ENABLE THE SAFE CONTROLLER YAML BUTTONS
+void MainGUIWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load" and the "send" safe controller YAML button again
+ ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(true);
+ ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(true);
+}
+// > CALLBACK TO RE-ENABLE THE CUSTOM CONTROLLER YAML BUTTONS
+void MainGUIWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load" and the "send" custom controller YAML button again
+ ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(true);
+ ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(true);
+}
+
+/*
+// > CALLBACK TO SEND THE CUSTOM CONTROLLER YAML PARAMETERS AS A MESSAGE
+void MainGUIWindow::customSendYamlAsMessageTimerCallback(const ros::TimerEvent&)
+{
+ // Load the CUSTOM controller YAML parameters from file into a message for
+ // sending directly to the "CustonControllerService" node of every agent
+
+ ros::NodeHandle nodeHandle("~");
+
+ // Instaniate a local variable of the message type
+ CustomControllerYAML customControllerYamlMessage;
+
+ // Load the data directly from the YAML file into the message
+
+ // > For the mass
+ customControllerYamlMessage.mass = MainGUIWindow::getParameterFloat(nodeHandle, "mass");
+
+ // Debugging this this works
+ ROS_INFO_STREAM("DEBUGGING: mass loaded from the custom controller YAML = " << customControllerYamlMessage.mass );
+
+ // > For the control_frequency
+ customControllerYamlMessage.control_frequency = MainGUIWindow::getParameterFloat(nodeHandle, "control_frequency");
+
+ // > For the motorPoly coefficients
+ std::vector<float> temp_motorPoly(3);
+ MainGUIWindow::getParameterFloatVector(nodeHandle, "motorPoly", temp_motorPoly, 3);
+ // Copy the loaded data into the message
+ for ( int i=0 ; i<3 ; i++ )
+ {
+ customControllerYamlMessage.motorPoly.push_back( temp_motorPoly[i] );
+ }
+
+ // > For the boolean about whether to execute the convert into body frame function
+ customControllerYamlMessage.shouldPerformConvertIntoBodyFrame = MainGUIWindow::getParameterBool(nodeHandle, "shouldPerformConvertIntoBodyFrame");
+
+ // > For the boolean about publishing the agents current position
+ customControllerYamlMessage.shouldPublishCurrent_xyz_yaw = MainGUIWindow::getParameterBool(nodeHandle, "shouldPublishCurrent_xyz_yaw");
+
+ // > For the boolean about following another agent
+ customControllerYamlMessage.shouldFollowAnotherAgent = MainGUIWindow::getParameterBool(nodeHandle, "shouldFollowAnotherAgent");
+
+ // > For the ordered agent ID's for following eachother in a line
+ std::vector<int> temp_follow_in_a_line_agentIDs(100);
+ int temp_number_of_agents_in_a_line = MainGUIWindow::getParameterIntVectorWithUnknownLength(nodeHandle, "follow_in_a_line_agentIDs", temp_follow_in_a_line_agentIDs);
+ // > Double check that the sizes agree
+ if ( temp_number_of_agents_in_a_line != temp_follow_in_a_line_agentIDs.size() )
+ {
+ // Update the user if the sizes don't agree
+ ROS_ERROR_STREAM("parameter 'follow_in_a_line_agentIDs' was loaded with two different lengths, " << temp_number_of_agents_in_a_line << " versus " << temp_follow_in_a_line_agentIDs.size() );
+ }
+ // Copy the loaded data into the message
+ for ( int i=0 ; i<temp_number_of_agents_in_a_line ; i++ )
+ {
+ customControllerYamlMessage.follow_in_a_line_agentIDs.push_back( temp_follow_in_a_line_agentIDs[i] );
+ }
+
+ // Publish the message containing the loaded YAML parameters
+ customYAMLasMessagePublisher.publish(customControllerYamlMessage);
+
+ // Start a timer which will enable the button in its callback
+ m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(0.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
+}
+*/
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD Y Y A M M L
+// L O O A A D D Y Y A A MM MM L
+// L O O A A D D Y A A M M M L
+// L O O AAAAA D D Y AAAAA M M L
+// LLLLL OOO A A DDDD Y A A M M LLLLL
+//
+// 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
+// ----------------------------------------------------------------------------------
+
+// load parameters from corresponding YAML file
+//
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float MainGUIWindow::getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void MainGUIWindow::getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int MainGUIWindow::getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void MainGUIWindow::getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int MainGUIWindow::getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+bool MainGUIWindow::getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.ui
index 725dbd1e12d9720528e67b6643f4fbd65ff6c58d..128a1cb90506437117454638422083c2831abe9a 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>866</width>
- <height>820</height>
+ <width>1176</width>
+ <height>1180</height>
</rect>
</property>
<property name="windowTitle">
@@ -250,6 +250,24 @@
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QPushButton" name="all_motors_off_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
<property name="text">
<string>All motors OFF</string>
</property>
@@ -452,6 +470,316 @@
</item>
</layout>
</widget>
+ <widget class="QWidget" name="tab">
+ <attribute name="title">
+ <string>Command all</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="1" column="0">
+ <widget class="QPushButton" name="all_disconnect_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Disconnect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QPushButton" name="all_connect_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Reconnect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0" colspan="2">
+ <widget class="QLabel" name="all_flying_state_label">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>20</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>FLYING STATE</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignBottom|Qt::AlignHCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QPushButton" name="all_take_off_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Take off</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QPushButton" name="all_enable_safe_controller_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Enable Safe</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QPushButton" name="all_land_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Land</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QPushButton" name="all_enable_custom_controller_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Enable Custom</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="1">
+ <widget class="QPushButton" name="all_load_custom_controller_yaml_own_agent_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Load Custom YAML</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0" colspan="2">
+ <widget class="QLabel" name="all_radio_label">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>20</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="lineWidth">
+ <number>1</number>
+ </property>
+ <property name="text">
+ <string>CRAZYRADIO</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignBottom|Qt::AlignHCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0" colspan="2">
+ <widget class="QLabel" name="all_yaml_label">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>20</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>LOAD YAML PARAMETERS</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignBottom|Qt::AlignHCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="0">
+ <widget class="QPushButton" name="all_load_safe_controller_yaml_coordinator_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Load Safe YAML</string>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="1">
+ <widget class="QPushButton" name="all_load_custom_controller_yaml_coordinator_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Load Custom YAML</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0" colspan="2">
+ <widget class="QLabel" name="all_yaml_agent_label">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>30</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>30</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>> From agenet's local file</string>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="0" colspan="2">
+ <widget class="QLabel" name="all_yaml_coordinator_label">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>30</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>30</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>> From coordinator's file</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="0">
+ <widget class="QPushButton" name="all_load_safe_controller_yaml_own_agent_button">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Load Safe YAML</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
</widget>
</item>
</layout>
@@ -501,7 +829,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>866</width>
+ <width>1176</width>
<height>25</height>
</rect>
</property>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index 13a572c9e5dbfc39b98351a4836f371400ecb678..be4888a000e4ac892adc5da572790d1404c9a286 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
//
// This file is part of D-FaLL-System.
//
@@ -45,21 +45,23 @@
#include "d_fall_pps/Setpoint.h"
-// tipes of controllers being used:
+// Types of controllers being used:
#define SAFE_CONTROLLER 0
#define CUSTOM_CONTROLLER 1
-// commands for CrazyRadio
+// Commands for CrazyRadio
#define CMD_RECONNECT 0
#define CMD_DISCONNECT 1
-
// CrazyRadio states:
#define CONNECTED 0
#define CONNECTING 1
#define DISCONNECTED 2
-// Commands for PPSClient
+// The constants that "command" changes in the
+// operation state of this agent. These "commands"
+// are sent from this GUI node to the "PPSClient"
+// node where the command is enacted
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2
#define CMD_CRAZYFLY_TAKE_OFF 3
@@ -72,12 +74,17 @@
#define STATE_FLYING 3
#define STATE_LAND 4
-// battery states
-
+// Battery states
#define BATTERY_STATE_NORMAL 0
#define BATTERY_STATE_LOW 1
+// For which controller parameters to load
+#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
+#define LOAD_YAML_CUSTOM_CONTROLLER_AGENT 2
+#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 3
+#define LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+// Universal constants
#define PI 3.141592653589
#define RAD2DEG 180.0/PI
@@ -117,6 +124,9 @@ private slots:
void on_en_safe_controller_clicked();
+ void on_customButton_1_clicked();
+ void on_customButton_2_clicked();
+ void on_customButton_3_clicked();
private:
Ui::MainWindow *ui;
@@ -127,7 +137,8 @@ private:
std::string m_ros_namespace;
- ros::Timer m_custom_timer_yaml_file;
+ ros::Timer m_timer_yaml_file_for_safe_controller;
+ ros::Timer m_timer_yaml_file_for_custom_controlller;
int m_student_id;
CrazyflieContext m_context;
@@ -150,10 +161,22 @@ private:
ros::Publisher customSetpointPublisher;
ros::Subscriber customSetpointSubscriber;
+ ros::Publisher PPSClientStudentCustomButtonPublisher;
+
ros::Subscriber DBChangedSubscriber;
- ros::Publisher customYAMLloadedPublisher;
- ros::Publisher safeYAMLloadedPublisher;
+
+
+ // > For publishing a message that requests the
+ // YAML parameters to be re-loaded from file
+ // > The message contents specify which controller
+ // the parameters should be re-loaded for
+ ros::Publisher requestLoadControllerYamlPublisher;
+
+ // Subscriber for locking the load the controller YAML
+ // parameters when the Coordintor GUI requests a load
+ ros::Subscriber requestLoadControllerYaml_from_my_GUI_Subscriber;
+
ros::Subscriber controllerUsedSubscriber;
@@ -168,6 +191,7 @@ private:
void DBChangedCallback(const std_msgs::Int32& msg);
void customYamlFileTimerCallback(const ros::TimerEvent&);
void safeYamlFileTimerCallback(const ros::TimerEvent&);
+ void requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg);
void controllerUsedChangedCallback(const std_msgs::Int32& msg);
void batteryStateChangedCallback(const std_msgs::Int32& msg);
@@ -184,6 +208,9 @@ private:
void highlightSafeControllerTab();
void highlightCustomControllerTab();
+ bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
+ Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
+
const std::vector<float> m_cutoff_voltages {3.1966, 3.2711, 3.3061, 3.3229, 3.3423, 3.3592, 3.3694, 3.385, 3.4006, 3.4044, 3.4228, 3.4228, 3.4301, 3.4445, 3.4531, 3.4677, 3.4705, 3.4712, 3.4756, 3.483, 3.4944, 3.5008, 3.5008, 3.5084, 3.511, 3.5122, 3.5243, 3.5329, 3.5412, 3.5529, 3.5609, 3.5625, 3.5638, 3.5848, 3.6016, 3.6089, 3.6223, 3.628, 3.6299, 3.6436, 3.6649, 3.6878, 3.6983, 3.7171, 3.7231, 3.7464, 3.7664, 3.7938, 3.8008, 3.816, 3.8313, 3.8482, 3.866, 3.8857, 3.8984, 3.9159, 3.9302, 3.9691, 3.997, 4.14 };
};
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index 2fc1e60f5fdf52141bded27293b286c33d8f250e..d86a3dd007e7c682d4b1049f6f46d01f54b9a2a1 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
//
// This file is part of D-FaLL-System.
//
@@ -42,6 +42,8 @@
#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/CustomButton.h"
+
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow),
@@ -56,7 +58,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
setCrazyRadioStatus(DISCONNECTED);
m_ros_namespace = ros::this_node::getNamespace();
- ROS_INFO("namespace: %s", m_ros_namespace.c_str());
+ ROS_INFO("Student GUI node namespace: %s", m_ros_namespace.c_str());
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
@@ -83,15 +85,27 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ros::NodeHandle my_nodeHandle("~");
controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
- customYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("customYAMLloaded", 1);
- safeYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("safeYAMLloaded", 1);
// communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
- ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
+ //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
+ ros::NodeHandle nh_PPSClient("PPSClient");
crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
+ PPSClientStudentCustomButtonPublisher = nh_PPSClient.advertise<CustomButton>("StudentCustomButton", 1);
+
+
+ // > For publishing a message that requests the
+ // YAML parameters to be re-loaded from file
+ // > The message contents specify which controller
+ // the parameters should be re-loaded for
+ requestLoadControllerYamlPublisher = nh_PPSClient.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
+
+
+ // Subscriber for locking the load the controller YAML
+ // parameters when the Coordintor GUI requests a load
+ requestLoadControllerYaml_from_my_GUI_Subscriber = nodeHandle.subscribe("/my_GUI/requestLoadControllerYaml", 1, &MainWindow::requestLoadControllerYaml_from_my_GUI_Callback, this);
// First get student ID
if(!nh_PPSClient.getParam("studentID", m_student_id))
@@ -105,27 +119,32 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
- std::vector<float> default_setpoint(4);
- ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService");
- ROS_INFO_STREAM(m_ros_namespace << "/SafeControllerService");
+ // Load default setpoint from the "SafeController" namespace of the "ParameterService"
+ std::vector<float> default_setpoint(4);
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service("ParameterService");
+ ros::NodeHandle nodeHandle_for_safeController(nodeHandle_to_own_agent_parameter_service, "SafeController");
- if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
+ if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
{
- ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
+ ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
}
-
+ // Copy the default setpoint into respective text fields of the GUI
ui->current_setpoint_x->setText(QString::number(default_setpoint[0]));
ui->current_setpoint_y->setText(QString::number(default_setpoint[1]));
ui->current_setpoint_z->setText(QString::number(default_setpoint[2]));
ui->current_setpoint_yaw->setText(QString::number(default_setpoint[3]));
+
disableGUI();
highlightSafeControllerTab();
ui->label_battery->setStyleSheet("QLabel { color : red; }");
m_battery_state = BATTERY_STATE_NORMAL;
+ ui->error_label->setStyleSheet("QLabel { color : red; }");
+ ui->error_label->clear();
+
initialize_custom_setpoint();
}
@@ -445,12 +464,43 @@ void MainWindow::on_motors_OFF_button_clicked()
void MainWindow::on_set_setpoint_button_clicked()
{
Setpoint msg_setpoint;
- msg_setpoint.x = (ui->new_setpoint_x->text()).toFloat();
- msg_setpoint.y = (ui->new_setpoint_y->text()).toFloat();
- msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();
- msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
+
+ // initialize setpoint to previous one
+
+ msg_setpoint.x = (ui->current_setpoint_x->text()).toFloat();
+ msg_setpoint.y = (ui->current_setpoint_y->text()).toFloat();
+ msg_setpoint.z = (ui->current_setpoint_z->text()).toFloat();
+ msg_setpoint.yaw = (ui->current_setpoint_yaw->text()).toFloat();
+
+ if(!ui->new_setpoint_x->text().isEmpty())
+ msg_setpoint.x = (ui->new_setpoint_x->text()).toFloat();
+
+ if(!ui->new_setpoint_y->text().isEmpty())
+ msg_setpoint.y = (ui->new_setpoint_y->text()).toFloat();
+
+ if(!ui->new_setpoint_z->text().isEmpty())
+ msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();
+
+ if(!ui->new_setpoint_yaw->text().isEmpty())
+ msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
+
+
+ if(!setpointInsideBox(msg_setpoint, m_context))
+ {
+ ROS_INFO("Corrected setpoint, was out of bounds");
+
+ // correct the setpoint given the box size
+ msg_setpoint = correctSetpointBox(msg_setpoint, m_context);
+ ui->error_label->setText("Setpoint is outside safety box");
+ }
+ else
+ {
+ ui->error_label->clear();
+ }
this->controllerSetpointPublisher.publish(msg_setpoint);
+
+ ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
}
void MainWindow::initialize_custom_setpoint()
@@ -467,12 +517,24 @@ void MainWindow::initialize_custom_setpoint()
void MainWindow::on_set_setpoint_button_2_clicked()
{
Setpoint msg_setpoint;
- msg_setpoint.x = (ui->new_setpoint_x_2->text()).toFloat();
- msg_setpoint.y = (ui->new_setpoint_y_2->text()).toFloat();
- msg_setpoint.z = (ui->new_setpoint_z_2->text()).toFloat();
- msg_setpoint.yaw = (ui->new_setpoint_yaw_2->text()).toFloat() * DEG2RAD;
+
+ msg_setpoint.x = (ui->current_setpoint_x_2->text()).toFloat();
+ msg_setpoint.y = (ui->current_setpoint_y_2->text()).toFloat();
+ msg_setpoint.z = (ui->current_setpoint_z_2->text()).toFloat();
+ msg_setpoint.yaw = (ui->current_setpoint_yaw_2->text()).toFloat();
+
+ if(!ui->new_setpoint_x_2->text().isEmpty())
+ msg_setpoint.x = (ui->new_setpoint_x_2->text()).toFloat();
+ if(!ui->new_setpoint_y_2->text().isEmpty())
+ msg_setpoint.y = (ui->new_setpoint_y_2->text()).toFloat();
+ if(!ui->new_setpoint_z_2->text().isEmpty())
+ msg_setpoint.z = (ui->new_setpoint_z_2->text()).toFloat();
+ if(!ui->new_setpoint_yaw_2->text().isEmpty())
+ msg_setpoint.yaw = (ui->new_setpoint_yaw_2->text()).toFloat() * DEG2RAD;
this->customSetpointPublisher.publish(msg_setpoint);
+
+ ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
}
void MainWindow::on_RF_disconnect_button_clicked()
@@ -483,68 +545,124 @@ void MainWindow::on_RF_disconnect_button_clicked()
ROS_INFO("command disconnect published");
}
-void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
-{
- // send msg that says that parameters have changed in YAML file
- std_msgs::Int32 msg;
- msg.data = 1;
- this->safeYAMLloadedPublisher.publish(msg);
- ROS_INFO("YALMloaded published");
- ui->load_safe_yaml_button->setEnabled(true);
-}
+
+
void MainWindow::on_load_safe_yaml_button_clicked()
{
+ // Set the "load safe yaml" button to be disabled
ui->load_safe_yaml_button->setEnabled(false);
+
+ // Send a message requesting the parameters from the YAML
+ // file to be reloaded for the safe controller
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
+ this->requestLoadControllerYamlPublisher.publish(msg);
+ ROS_INFO("Request load of safe controller YAML published");
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
ros::NodeHandle nodeHandle("~");
- m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::safeYamlFileTimerCallback, this, true);
+ m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
+}
- std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
- ROS_INFO_STREAM(d_fall_pps_path);
+void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load safe yaml" button again
+ ui->load_safe_yaml_button->setEnabled(true);
+}
- // first, reload the name of the custom controller:
- std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
- system(cmd.c_str());
- ROS_INFO_STREAM(cmd);
- // then, reload the parameters of the custom controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeControllerService";
- system(cmd.c_str());
- ROS_INFO_STREAM(cmd);
-}
-void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
+void MainWindow::on_load_custom_yaml_button_clicked()
{
- // send msg that says that parameters have changed in YAML file
+ // Set the "load custom yaml" button to be disabled
+ ui->load_custom_yaml_button->setEnabled(false);
+
+ // Send a message requesting the parameters from the YAML
+ // file to be reloaded for the custom controller
std_msgs::Int32 msg;
- msg.data = 1;
- this->customYAMLloadedPublisher.publish(msg);
- ROS_INFO("YALMloaded published");
+ msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT;
+ this->requestLoadControllerYamlPublisher.publish(msg);
+ ROS_INFO("Request load of custom controller YAML published");
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ ros::NodeHandle nodeHandle("~");
+ m_timer_yaml_file_for_custom_controlller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::customYamlFileTimerCallback, this, true);
+}
+
+void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load custom yaml" button again
ui->load_custom_yaml_button->setEnabled(true);
}
-void MainWindow::on_load_custom_yaml_button_clicked()
+
+
+
+
+void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
{
- ui->load_custom_yaml_button->setEnabled(false);
+ // Extract from the "msg" for which controller the YAML
+ // parameters should be loaded
+ int controller_to_load_yaml = msg.data;
+
+ // Create the "nodeHandle" needed in the switch cases below
ros::NodeHandle nodeHandle("~");
- m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::customYamlFileTimerCallback, this, true);
- std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
- ROS_INFO_STREAM(d_fall_pps_path);
+ // Switch between loading for the different controllers
+ switch(controller_to_load_yaml)
+ {
+ case LOAD_YAML_SAFE_CONTROLLER_AGENT:
+ case LOAD_YAML_SAFE_CONTROLLER_COORDINATOR:
+ // Set the "load safe yaml" button to be disabled
+ ui->load_safe_yaml_button->setEnabled(false);
+
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
+
+ break;
+
+ case LOAD_YAML_CUSTOM_CONTROLLER_AGENT:
+ case LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR:
+ // Set the "load custom yaml" button to be disabled
+ ui->load_custom_yaml_button->setEnabled(false);
- // first, reload the name of the custom controller:
- std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
- system(cmd.c_str());
- ROS_INFO_STREAM(cmd);
+ // Start a timer which will enable the button in its callback
+ // > This is required because the agent node waits some time between
+ // re-loading the values from the YAML file and then assigning then
+ // to the local variable of the agent.
+ // > Thus we use this timer to prevent the user from clicking the
+ // button in the GUI repeatedly.
+ m_timer_yaml_file_for_custom_controlller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::customYamlFileTimerCallback, this, true);
- // then, reload the parameters of the custom controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomControllerService";
- system(cmd.c_str());
- ROS_INFO_STREAM(cmd);
+ break;
+
+ default:
+ ROS_INFO("Unknown 'all controllers to load yaml' command, thus nothing will be disabled");
+ break;
+ }
}
+
+
+
void MainWindow::on_en_custom_controller_clicked()
{
std_msgs::Int32 msg;
@@ -559,3 +677,80 @@ void MainWindow::on_en_safe_controller_clicked()
msg.data = CMD_USE_SAFE_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
}
+
+void MainWindow::on_customButton_1_clicked()
+{
+ CustomButton msg_custom_button;
+ msg_custom_button.button_index = 1;
+ msg_custom_button.command_code = 0;
+ this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
+
+ ROS_INFO("Custom button 1 pressed");
+}
+
+void MainWindow::on_customButton_2_clicked()
+{
+ CustomButton msg_custom_button;
+ msg_custom_button.button_index = 2;
+ msg_custom_button.command_code = 0;
+ this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
+ ROS_INFO("Custom button 2 pressed");
+}
+
+void MainWindow::on_customButton_3_clicked()
+{
+ CustomButton msg_custom_button;
+ msg_custom_button.button_index = 3;
+ msg_custom_button.command_code = (ui->custom_command_3->text()).toFloat();
+ this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
+ ROS_INFO("Custom button 3 pressed");
+}
+
+Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
+{
+ Setpoint corrected_setpoint;
+ corrected_setpoint = setpoint;
+
+ float x_size = context.localArea.xmax - context.localArea.xmin;
+ float y_size = context.localArea.ymax - context.localArea.ymin;
+ float z_size = context.localArea.zmax - context.localArea.zmin;
+
+ if(setpoint.x > x_size/2)
+ corrected_setpoint.x = x_size/2;
+ if(setpoint.y > y_size/2)
+ corrected_setpoint.y = y_size/2;
+ if(setpoint.z > z_size)
+ corrected_setpoint.z = z_size;
+
+ if(setpoint.x < -x_size/2)
+ corrected_setpoint.x = -x_size/2;
+ if(setpoint.y < -y_size/2)
+ corrected_setpoint.y = -y_size/2;
+ if(setpoint.z < 0)
+ corrected_setpoint.z = 0;
+
+ return corrected_setpoint;
+}
+
+bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
+{
+
+ float x_size = context.localArea.xmax - context.localArea.xmin;
+ float y_size = context.localArea.ymax - context.localArea.ymin;
+ float z_size = context.localArea.zmax - context.localArea.zmin;
+ //position check
+ if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
+ ROS_INFO_STREAM("x outside safety box");
+ return false;
+ }
+ if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
+ ROS_INFO_STREAM("y outside safety box");
+ return false;
+ }
+ if((setpoint.z < 0) or (setpoint.z > z_size)) {
+ ROS_INFO_STREAM("z outside safety box");
+ return false;
+ }
+
+ return true;
+}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index 3dd33614610632ae4c773e1caccc487d664b5765..88cf08e5ce445fe31c88bde317df0329f9d9a7f2 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1027</width>
- <height>619</height>
+ <width>1042</width>
+ <height>701</height>
</rect>
</property>
<property name="sizePolicy">
@@ -33,10 +33,145 @@
<string/>
</property>
<layout class="QGridLayout" name="gridLayout_10">
+ <item row="3" column="1">
+ <widget class="QGroupBox" name="groupBox_4">
+ <property name="title">
+ <string/>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QPushButton" name="motors_OFF_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Motors OFF</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="take_off_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Take Off</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="land_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Land</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_battery">
+ <property name="font">
+ <font>
+ <pointsize>9</pointsize>
+ <italic>true</italic>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QGroupBox" name="groupBox">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Maximum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>18</pointsize>
+ </font>
+ </property>
+ <property name="title">
+ <string>StudentID # connected to CF #</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="1" column="0">
+ <widget class="QLabel" name="raw_voltage_label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>18</pointsize>
+ <italic>false</italic>
+ </font>
+ </property>
+ <property name="text">
+ <string>Raw voltage: </string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="voltage_field">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Maximum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLabel" name="flying_state_label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>FlyingState</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
<item row="3" column="0">
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
- <number>1</number>
+ <number>0</number>
</property>
<widget class="QWidget" name="tab_3">
<attribute name="title">
@@ -401,6 +536,19 @@
</property>
</widget>
</item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_setpoint_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
<item row="3" column="0">
<widget class="QLabel" name="label_9">
<property name="sizePolicy">
@@ -427,8 +575,8 @@
</property>
</widget>
</item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="current_setpoint_z">
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="current_setpoint_yaw">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -458,19 +606,6 @@
</property>
</widget>
</item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="current_setpoint_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
<item row="3" column="2">
<widget class="QLineEdit" name="new_setpoint_z">
<property name="sizePolicy">
@@ -515,6 +650,13 @@
</property>
</widget>
</item>
+ <item row="5" column="1">
+ <widget class="QLabel" name="error_label">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
@@ -563,6 +705,27 @@
<string>Custom Controller</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_9">
+ <item row="2" column="1">
+ <widget class="QPushButton" name="customButton_2">
+ <property name="text">
+ <string>Custom Command 2</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QPushButton" name="customButton_3">
+ <property name="text">
+ <string>Custom Command 3</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QPushButton" name="customButton_1">
+ <property name="text">
+ <string>Custom Command 1</string>
+ </property>
+ </widget>
+ </item>
<item row="0" column="0">
<widget class="QGroupBox" name="groupBox_5">
<property name="title">
@@ -1032,7 +1195,7 @@
</layout>
</widget>
</item>
- <item row="1" column="1">
+ <item row="4" column="1">
<widget class="QPushButton" name="en_custom_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -1051,7 +1214,7 @@
</property>
</widget>
</item>
- <item row="1" column="0">
+ <item row="4" column="0">
<widget class="QPushButton" name="load_custom_yaml_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -1070,145 +1233,13 @@
</property>
</widget>
</item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="custom_command_3"/>
+ </item>
</layout>
</widget>
</widget>
</item>
- <item row="3" column="1">
- <widget class="QGroupBox" name="groupBox_4">
- <property name="title">
- <string/>
- </property>
- <layout class="QVBoxLayout" name="verticalLayout">
- <item>
- <widget class="QPushButton" name="motors_OFF_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Motors OFF</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="take_off_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Take Off</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="land_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Land</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_battery">
- <property name="font">
- <font>
- <pointsize>9</pointsize>
- <italic>true</italic>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QGroupBox" name="groupBox">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Maximum" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="font">
- <font>
- <pointsize>18</pointsize>
- </font>
- </property>
- <property name="title">
- <string>StudentID # connected to CF #</string>
- </property>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="1" column="0">
- <widget class="QLabel" name="raw_voltage_label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="font">
- <font>
- <pointsize>18</pointsize>
- <italic>false</italic>
- </font>
- </property>
- <property name="text">
- <string>Raw voltage: </string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="voltage_field">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Maximum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>80</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLabel" name="flying_state_label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>FlyingState</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
</layout>
</widget>
</item>
@@ -1270,7 +1301,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1027</width>
+ <width>1042</width>
<height>25</height>
</rect>
</property>
diff --git a/pps_ws/src/d_fall_pps/launch/Student.launch b/pps_ws/src/d_fall_pps/launch/Student.launch
index c2480b38a9fc1fa1a78315189a943f3576ec8a25..918c01067471dccce718677c983b50457b531112 100755
--- a/pps_ws/src/d_fall_pps/launch/Student.launch
+++ b/pps_ws/src/d_fall_pps/launch/Student.launch
@@ -1,27 +1,34 @@
<launch>
+ <!-- CRAZY RADIO -->
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
+ <!-- PPS CLIENT -->
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
<param name="studentID" value="$(optenv ROS_NAMESPACE)" />
</node>
+ <!-- SAFE CONTROLLER -->
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
+ <!-- CUSTOM CONTROLLER -->
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
- <rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" />
</node>
- <!-- When we have a GUI, this has to be adapted and included -->
- <node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI">
- <!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
+ <!-- PARAMETER SERVICE -->
+ <node pkg="d_fall_pps" name="ParameterService" output="screen" type="ParameterService">
+ <param name="type" type="str" value="agent" />
+ <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" ns="SafeController" />
+ <rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" ns="CustomController" />
</node>
-
+ <!-- AGENT GUI (aka. the "student GUI") -->
+ <node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI">
+ </node>
</launch>
diff --git a/pps_ws/src/d_fall_pps/launch/Teacher.launch b/pps_ws/src/d_fall_pps/launch/Teacher.launch
index 9193b7896c16f8f081e35e489e487093883155aa..1c2e6a9d63045b3969339f7f17d3ac0faf18e2b8 100755
--- a/pps_ws/src/d_fall_pps/launch/Teacher.launch
+++ b/pps_ws/src/d_fall_pps/launch/Teacher.launch
@@ -8,7 +8,14 @@
<!-- When we have a GUI, this has to be adapted and included -->
<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
- <!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
+ </node>
+
+ <!-- PARAMETER SERVICE -->
+ <node pkg="d_fall_pps" name="ParameterService" output="screen" type="ParameterService">
+ <param name="type" type="str" value="coordinator" />
+ <param name="agentID" value="0" />
+ <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" ns="SafeController" />
+ <rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" ns="CustomController" />
</node>
</launch>
diff --git a/pps_ws/src/d_fall_pps/msg/CustomButton.msg b/pps_ws/src/d_fall_pps/msg/CustomButton.msg
new file mode 100644
index 0000000000000000000000000000000000000000..7ba9d563c0d308337be9aa2aa691581a6ec9b6c9
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/CustomButton.msg
@@ -0,0 +1,2 @@
+uint32 button_index
+float64 command_code
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/msg/CustomControllerYAML.msg b/pps_ws/src/d_fall_pps/msg/CustomControllerYAML.msg
new file mode 100644
index 0000000000000000000000000000000000000000..ebe1dee883ffcd744fe81b94023e395d099775fc
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/CustomControllerYAML.msg
@@ -0,0 +1,7 @@
+float64 mass
+float64 control_frequency
+float64[] motorPoly
+bool shouldPerformConvertIntoBodyFrame
+bool shouldPublishCurrent_xyz_yaw
+bool shouldFollowAnotherAgent
+uint32[] follow_in_a_line_agentIDs
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index 766bf5b586942ff2988a44b49199070c024a24e1..5e0a39201708c4aae0f2b97c1f3510773788f738 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -1 +1 @@
-3,PPS_CF01,0/0/2M/E7E7E7E701,0,0.918398,-0.0405601,-0.2,2.27427,0.860455,1.2
+7,PPS_CF01,0/0/2M/E7E7E7E701,0,1.56976,-0.10861,-0.2,2.79907,0.808677,1.2
diff --git a/pps_ws/src/d_fall_pps/param/CustomController.yaml b/pps_ws/src/d_fall_pps/param/CustomController.yaml
index 8f59bc1539c5c13ded4c40c38cf1c0583dc5a2a1..29b9df5e92950cb77a05d44787430a4cac3c6388 100644
--- a/pps_ws/src/d_fall_pps/param/CustomController.yaml
+++ b/pps_ws/src/d_fall_pps/param/CustomController.yaml
@@ -1,8 +1,46 @@
-# mass of the crazyflie
-mass : 30
+# Mass of the crazyflie
+mass : 31
-# frequency of the controller, in hertz
+# Frequency of the controller, in hertz
control_frequency : 200
-#quadratic motor regression equation (a0, a1, a2)
-motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
+# Quadratic motor regression equation (a0, a1, a2)
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# Boolean for whether to execute the convert into body frame function
+shouldPerformConvertIntoBodyFrame : true
+
+# Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
+shouldPublishCurrent_xyz_yaw : false
+
+# Boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
+# an attempt to follow the "my_current_xyz_yaw_topic" from another agent
+shouldFollowAnotherAgent : false
+
+# The order in which agents should follow eachother
+# > This parameter is a vector of integers that specifies agent ID's
+# > The order of the agent ID's is the ordering of the line formation
+# > i.e., the first ID is the leader, the second ID should follow the first ID, and so on
+follow_in_a_line_agentIDs : [1, 2, 3]
+
+# Boolean indiciating whether the "Debug Message" of this agent should be published or not
+shouldPublishDebugMessage : false
+
+# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+shouldDisplayDebugInfo : false
+
+# A flag for which controller to use, defined as:
+# 1 - LQR controller based on the state vector: [position,velocity,angles]
+# 2 - LQR controller based on the state vector: [position,velocity]
+controller_type : 1
+
+# The LQR Controller parameters for "mode = 1"
+gainMatrixRollRate : [ 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00]
+gainMatrixPitchRate : [ 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00]
+gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84]
+gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00]
+
+# The LQR Controller parameters for "mode = 2"
+gainMatrixRollAngle : [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
+gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
+gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.31, 0.00, 0.00, 0.14]
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp b/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
index 9fd5332f33c9026d5580a32ecc3a50d73775cc4f..4a0e58a8ebbe3a75bbbc05ec619dbad05ac2211e 100755
--- a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
@@ -30,6 +30,17 @@
// ----------------------------------------------------------------------------------
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 <ros/ros.h>
#include "d_fall_pps/CrazyflieContext.h"
@@ -43,55 +54,181 @@
#include "CrazyflieIO.h"
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// For which controller parameters to load
+#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
+#define LOAD_YAML_CUSTOM_CONTROLLER_AGENT 2
+#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 3
+#define LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+
+// For which controller parameters and from where to fetch
+#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
+#define FETCH_YAML_CUSTOM_CONTROLLER_FROM_OWN_AGENT 2
+#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 3
+#define FETCH_YAML_CUSTOM_CONTROLLER_FROM_COORDINATOR 4
+
+
using namespace d_fall_pps;
using namespace std;
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
CrazyflieDB crazyflieDB;
-bool cmRead(CMRead::Request &request, CMRead::Response &response) {
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+bool cmRead(CMRead::Request &request, CMRead::Response &response);
+int findEntryByStudID(unsigned int studID);
+bool cmQuery(CMQuery::Request &request, CMQuery::Response &response);
+int findEntryByCF(string name);
+bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response);
+bool cmCommand(CMCommand::Request &request, CMCommand::Response &response);
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// The requesting to load parameters is currently handled by the Paramter Service
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD A TTTTT A BBBB A SSSS EEEEE
+// D D A A T A A B B A A S E
+// D D A A T A A BBBB A A SSS EEE
+// D D AAAAA T AAAAA B B AAAAA S E
+// DDDD A A T A A BBBB A A SSSS EEEEE
+// ----------------------------------------------------------------------------------
+
+bool cmRead(CMRead::Request &request, CMRead::Response &response)
+{
response.crazyflieDB = crazyflieDB;
return true;
}
-int findEntryByStudID(unsigned int studID) {
- for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++) {
+int findEntryByStudID(unsigned int studID)
+{
+ for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++)
+ {
CrazyflieEntry entry = crazyflieDB.crazyflieEntries[i];
- if(entry.studentID == studID) {
+ if(entry.studentID == studID)
+ {
return i;
}
}
return -1;
}
-bool cmQuery(CMQuery::Request &request, CMQuery::Response &response) {
+bool cmQuery(CMQuery::Request &request, CMQuery::Response &response)
+{
int cfIndex = findEntryByStudID(request.studentID);
- if(cfIndex != -1) {
+ if(cfIndex != -1)
+ {
response.crazyflieContext = crazyflieDB.crazyflieEntries[cfIndex].crazyflieContext;
return true;
- } else {
+ }
+ else
+ {
return false;
}
}
-int findEntryByCF(string name) {
- for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++) {
+int findEntryByCF(string name)
+{
+ for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++)
+ {
CrazyflieEntry entry = crazyflieDB.crazyflieEntries[i];
string cfName = entry.crazyflieContext.crazyflieName;
- if(cfName == name) {
+ if(cfName == name)
+ {
return i;
}
}
return -1;
}
-bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response) {
- switch(request.mode) {
- case ENTRY_INSERT_OR_UPDATE: {
+bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response)
+{
+ switch(request.mode)
+ {
+ case ENTRY_INSERT_OR_UPDATE:
+ {
string cfName = request.crazyflieEntry.crazyflieContext.crazyflieName;
int cfIndex = findEntryByCF(cfName);
- if(cfIndex == -1) {
+ if(cfIndex == -1)
+ {
crazyflieDB.crazyflieEntries.push_back(request.crazyflieEntry);
- } else {
+ }
+ else
+ {
crazyflieDB.crazyflieEntries[cfIndex] = request.crazyflieEntry;
}
return true;
@@ -100,9 +237,12 @@ bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response) {
case ENTRY_REMOVE: {
string cfName = request.crazyflieEntry.crazyflieContext.crazyflieName;
int cfIndex = findEntryByCF(cfName);
- if(cfIndex == -1) {
+ if(cfIndex == -1)
+ {
return false;
- } else {
+ }
+ else
+ {
crazyflieDB.crazyflieEntries.erase(crazyflieDB.crazyflieEntries.begin() +cfIndex);
return true;
}
@@ -112,14 +252,18 @@ bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response) {
}
}
-bool cmCommand(CMCommand::Request &request, CMCommand::Response &response) {
- switch(request.command) {
- case CMD_SAVE: {
+bool cmCommand(CMCommand::Request &request, CMCommand::Response &response)
+{
+ switch(request.command)
+ {
+ case CMD_SAVE:
+ {
writeCrazyflieDB(crazyflieDB);
return true;
}
- case CMD_RELOAD: {
+ case CMD_RELOAD:
+ {
crazyflieDB.crazyflieEntries.clear();
readCrazyflieDB(crazyflieDB);
return true;
@@ -129,7 +273,19 @@ bool cmCommand(CMCommand::Request &request, CMCommand::Response &response) {
}
}
-int main(int argc, char* argv[]) {
+
+
+
+// ----------------------------------------------------------------------------------
+// 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[])
+{
ros::init(argc, argv, "CentralManagerService");
ros::NodeHandle nodeHandle("~");
@@ -141,6 +297,16 @@ 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);
+
+
ROS_INFO("CentralManagerService ready");
ros::spin();
diff --git a/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp b/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp
index 8ef32d752add884b534b2ec4192364fd807423dc..e5e0c03f350623f89d39b19b36556a296ef81ec3 100644
--- a/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp
+++ b/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp
@@ -25,7 +25,13 @@
//
//
// DESCRIPTION:
-// This manages the Input/Output stream from the Crazyflie
+// This manages the Input/Output from the text file database that is used to store
+// and commincate an agent's zone between the coordinator node and the agent's node,
+// i.e., the details of:
+// > the linked ( agent ID , Crazyflie ) pair, where the Crazyflie is described by:
+// - the "name", which matches that used by the localisation system
+// - the "address", which is needed to establish a radio connection
+// > the hyper-rectangle area in which the agent is allowed to operate
//
// ----------------------------------------------------------------------------------
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index 048b8375ded87f65266e6943268ad00b4badd2a1..7d4889183a77e57fc4897f4e662a627264c0ca3a 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -56,6 +56,7 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
+#include "d_fall_pps/CustomControllerYAML.h"
#include <std_msgs/Int32.h>
@@ -73,10 +74,10 @@
// These constants are defined to make the code more readable and adaptable.
-// constants
+// Universal constants
#define PI 3.1415926535
-// The constants define the modes that can be used for controller the Crazyflie 2.0,
+// These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the
// firmware running on the Crazyflie 2.0.
// The following is a short description about each mode:
@@ -91,10 +92,28 @@
// body frame roll, pitch, and yaw angles from the PID attitude
// controllers implemented in the Crazyflie 2.0 firmware.
#define MOTOR_MODE 6
-#define RATE_MODE 7
+#define RATE_MODE 7
#define ANGLE_MODE 8
-// namespacing the package
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+// LQR_RATE_MODE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_ANGLE_MODE LQR controller based on the state vector:
+// [position,velocity]
+//
+#define LQR_RATE_MODE 1 // (DEFAULT)
+#define LQR_ANGLE_MODE 2
+
+// Constants for feching the yaml paramters
+#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
+#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2
+#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
+#define FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+
+// Namespacing the package
using namespace d_fall_pps;
@@ -111,27 +130,94 @@ using namespace d_fall_pps;
// Variables for controller
float cf_mass; // Crazyflie mass in grams
-std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
+std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
float control_frequency; // Frequency at which the controller is running
float gravity_force; // The weight of the Crazyflie in Newtons, i.e., mg
-CrazyflieData previous_stateErrorInertial; // The location error of the Crazyflie at the "previous" time step
+float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
-// The LQR Controller parameters
-const float gainMatrixRoll[9] = {0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0};
-const float gainMatrixPitch[9] = {1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0};
-const float gainMatrixYaw[9] = {0, 0, 0, 0, 0, 0, 0, 0, 2.843099534};
-const float gainMatrixThrust[9] = {0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0};
+
+// The controller type to run in the "calculateControlOutput" function
+int controller_type = LQR_RATE_MODE;
+
+// The LQR Controller parameters for "LQR_RATE_MODE"
+std::vector<float> gainMatrixRollRate = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
+std::vector<float> gainMatrixPitchRate = { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
+std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
+std::vector<float> gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
+// The LQR Controller parameters for "LQR_ANGLE_MODE"
+std::vector<float> gainMatrixRollAngle = { 0.00,-0.20, 0.00, 0.00,-0.20, 0.00};
+std::vector<float> gainMatrixPitchAngle = { 0.20, 0.00, 0.00, 0.20, 0.00, 0.00};
+std::vector<float> gainMatrixThrust_SixStateVector = { 0.00, 0.00, 0.31, 0.00, 0.00, 0.14};
+
+// ROS Publisher for debugging variables
ros::Publisher debugPublisher;
+// Variable for the namespaces for the paramter services
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+
+
+// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
+
+// Boolean whether to execute the convert into body frame function
+bool shouldPerformConvertIntoBodyFrame = false;
+
+
+// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool shouldDisplayDebugInfo = false;
+
+
+// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
+// POSITION
+
+// The ID of this agent, i.e., the ID of this compute
+int my_agentID = 0;
+
+// Boolean indicating whether the (x,y,z,yaw) of this agent should be published or not
+// > The default behaviour is: do not publish,
+// > This varaible is changed based on parameters loaded from the YAML file
+bool shouldPublishCurrent_xyz_yaw = false;
+
+// Boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
+// an attempt to follow the "my_current_xyz_yaw_topic" from another agent
+// > The default behaviour is: do not follow,
+// > This varaible is changed based on parameters loaded from the YAML file
+bool shouldFollowAnotherAgent = false;
+
+// The order in which agents should follow eachother
+// > This parameter is a vector of integers that specifies agent ID's
+// > The order of the agent ID's is the ordering of the line formation
+// > i.e., the first ID is the leader, the second ID should follow the first ID, and so on
+std::vector<int> follow_in_a_line_agentIDs(100);
+
+// Integer specifying the ID of the agent that will be followed by this agent
+// > The default behaviour not to follow any agent, indicated by ID zero
+// > This varaible is changed based on parameters loaded from the YAML file
+int agentID_to_follow = 0;
+
+// ROS Publisher for my current (x,y,z,yaw) position
+ros::Publisher my_current_xyz_yaw_publisher;
+
+// ROS Subscriber for my position
+ros::Subscriber xyz_yaw_to_follow_subscriber;
+
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
-// The "CrazyflieData" type used for the "previous_stateErrorInertial" variable is a
+// The "CrazyflieData" type used for the "request" variable is a
// structure as defined in the file "CrazyflieData.msg" which has the following
// properties:
// string crazyflieName The name given to the Crazyflie in the Vicon software
@@ -168,6 +254,11 @@ ros::Publisher debugPublisher;
// written below in an order that ensure each function is implemented before it is
// called from another function, hence why the "main" function is at the bottom.
+// CONTROLLER COMPUTATIONS
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforRates(float stateErrorBody[9], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAngles(float stateErrorBody[9], Controller::Request &request, Controller::Response &response);
+
// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
@@ -176,12 +267,20 @@ float computeMotorPolyBackward(float thrust);
// SETPOINT CHANGE CALLBACK
void setpointCallback(const Setpoint& newSetpoint);
+void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint);
// LOAD PARAMETERS
-void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
-float getFloatParameter(ros::NodeHandle& nodeHandle, std::string name);
-void loadPPSTemplateParameters(ros::NodeHandle& nodeHandle);
-void customYAMLloadedCallback(const std_msgs::Int32& msg);
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
@@ -304,254 +403,451 @@ void customYAMLloadedCallback(const std_msgs::Int32& msg);
// (CCW) | M3 | | M2 | (CW)
// \____/ \____/
//
-//
+//
//
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
-bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
- // This is the "start" of the outer loop controller, add all your control
- // computation here, or you may find it convienient to create functions
- // to keep you code cleaner
-
-
- // Define a local array to fill in with the state error
- float stateErrorInertial[9];
-
- // Fill in the (x,y,z) position error
- stateErrorInertial[0] = request.ownCrazyflie.x - setpoint[0];
- stateErrorInertial[1] = request.ownCrazyflie.y - setpoint[1];
- stateErrorInertial[2] = request.ownCrazyflie.z - setpoint[2];
-
- // Compute an estimate of the velocity
- // > As simply the derivative between the current and previous position
- stateErrorInertial[3] = (stateErrorInertial[0] - previous_stateErrorInertial.x) * control_frequency;
- stateErrorInertial[4] = (stateErrorInertial[1] - previous_stateErrorInertial.y) * control_frequency;
- stateErrorInertial[5] = (stateErrorInertial[2] - previous_stateErrorInertial.z) * control_frequency;
-
- // Fill in the roll and pitch angle measurements directly
- stateErrorInertial[6] = request.ownCrazyflie.roll;
- stateErrorInertial[7] = request.ownCrazyflie.pitch;
-
- // Fill in the yaw angle error
- // > This error should be "unwrapped" to be in the range
- // ( -pi , pi )
- // > First, get the yaw error into a local variable
- float yawError = request.ownCrazyflie.yaw - setpoint[3];
- // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
- while(yawError > PI) {yawError -= 2 * PI;}
- while(yawError < -PI) {yawError += 2 * PI;}
- // > Third, put the "yawError" into the "stateError" variable
- stateErrorInertial[8] = yawError;
+ // This is the "start" of the outer loop controller, add all your control
+ // computation here, or you may find it convienient to create functions
+ // to keep you code cleaner
+
+
+ // Define a local array to fill in with the state error
+ float stateErrorInertial[9];
+
+ // Fill in the (x,y,z) position error
+ stateErrorInertial[0] = request.ownCrazyflie.x - setpoint[0];
+ stateErrorInertial[1] = request.ownCrazyflie.y - setpoint[1];
+ stateErrorInertial[2] = request.ownCrazyflie.z - setpoint[2];
+
+ // Compute an estimate of the velocity
+ // > As simply the derivative between the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - previous_stateErrorInertial[0]) * control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - previous_stateErrorInertial[1]) * control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - previous_stateErrorInertial[2]) * control_frequency;
+
+ // Fill in the roll and pitch angle measurements directly
+ stateErrorInertial[6] = request.ownCrazyflie.roll;
+ stateErrorInertial[7] = request.ownCrazyflie.pitch;
+
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > First, get the yaw error into a local variable
+ float yawError = request.ownCrazyflie.yaw - setpoint[3];
+ // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // > Third, put the "yawError" into the "stateError" variable
+ stateErrorInertial[8] = yawError;
+
+
+ // CONVERSION INTO BODY FRAME
+ // Conver the state erorr from the Inertial frame into the Body frame
+ // > Note: the function "convertIntoBodyFrame" is implemented in this file
+ // and by default does not perform any conversion. The equations to convert
+ // the state error into the body frame should be implemented in that function
+ // for successful completion of the PPS exercise
+ float stateErrorBody[9];
+ convertIntoBodyFrame(stateErrorInertial, stateErrorBody, request.ownCrazyflie.yaw);
+
+
+ // SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
+ // > as we have already used previous error we can now update it update it
+ for(int i = 0; i < 9; ++i)
+ {
+ previous_stateErrorInertial[i] = stateErrorInertial[i];
+ }
+
+
+
+ // SWITCH BETWEEN THE DIFFERENT CONTROLLER TYPES:
+ switch (controller_type)
+ {
+ // LQR controller based on the state vector: [position,velocity,angles]
+ case LQR_RATE_MODE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector: [position,velocity]
+ case LQR_ANGLE_MODE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAngles(stateErrorBody,request,response);
+ break;
+ }
+
+ default:
+ {
+ // Display that the "controller_type" was not recognised
+ ROS_INFO_STREAM("ERROR: in the 'calculateControlOutput' function of the 'CustomControllerService': the 'controller_type' is not recognised.");
+ // Set everything in the response to zero
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
+ response.controlOutput.motorCmd1 = 0;
+ response.controlOutput.motorCmd2 = 0;
+ response.controlOutput.motorCmd3 = 0;
+ response.controlOutput.motorCmd4 = 0;
+ response.controlOutput.onboardControllerType = MOTOR_MODE;
+ break;
+ }
+
+
+
+ }
+
+
+
+
+
+
+
+ // ************************************************************************************************
+ // PPPP U U BBBB L III SSSS H H X X Y Y ZZZZZ Y Y A W W
+ // P P U U B B L I S H H X X Y Y Z Y Y A A W W
+ // PPPP U U BBBB L I SSS HHHH X Y Z Y A A W W
+ // P U U B B L I S H H X X Y Z Y AAAAA W W W
+ // P UUU BBBB LLLLL III SSSS H H X X Y ZZZZZ Y A A W W
+
+ // PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
+ if (shouldPublishCurrent_xyz_yaw)
+ {
+ // publish setpoint for FollowController of another student group
+ Setpoint temp_current_xyz_yaw;
+ // Fill in the x,y,z, and yaw info directly from the data provided to this
+ // function
+ temp_current_xyz_yaw.x = request.ownCrazyflie.x;
+ temp_current_xyz_yaw.y = request.ownCrazyflie.y;
+ temp_current_xyz_yaw.z = request.ownCrazyflie.z;
+ temp_current_xyz_yaw.yaw = request.ownCrazyflie.yaw;
+ my_current_xyz_yaw_publisher.publish(temp_current_xyz_yaw);
+ }
+
+
+
+
+
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+}
-
- // CONVERSION INTO BODY FRAME
- // Conver the state erorr from the Inertial frame into the Body frame
- // > Note: the function "convertIntoBodyFrame" is implemented in this file
- // and by default does not perform any conversion. The equations to convert
- // the state error into the body frame should be implemented in that function
- // for successful completion of the PPS exercise
- float stateErrorBody[9];
- convertIntoBodyFrame(stateErrorInertial, stateErrorBody, request.ownCrazyflie.yaw);
- // **********************
- // Y Y A W W
- // Y Y A A W W
- // Y A A W W
- // Y AAAAA W W W
- // Y A A W W
- //
- // YAW CONTROLLER
+void calculateControlOutput_viaLQRforRates(float stateErrorBody[9], Controller::Request &request, Controller::Response &response)
+{
+ // **********************
+ // Y Y A W W
+ // Y Y A A W W
+ // Y A A W W
+ // Y AAAAA W W W
+ // Y A A W W
+ //
+ // YAW CONTROLLER
- // Instantiate the local variable for the yaw rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float yawRate_forResponse = 0;
+ // Instantiate the local variable for the yaw rate that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+ float yawRate_forResponse = 0;
- // Perform the "-Kx" LQR computation for the yaw rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- yawRate_forResponse -= gainMatrixYaw[i] * stateErrorBody[i];
- }
+ // Perform the "-Kx" LQR computation for the yaw rate to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ }
- // Put the computed yaw rate into the "response" variable
- response.controlOutput.yaw = yawRate_forResponse;
+ // Put the computed yaw rate into the "response" variable
+ response.controlOutput.yaw = yawRate_forResponse;
- // **************************************
- // BBBB OOO DDDD Y Y ZZZZZ
- // B B O O D D Y Y Z
- // BBBB O O D D Y Z
- // B B O O D D Y Z
- // BBBB OOO DDDD Y ZZZZZ
- //
- // ALITUDE CONTROLLER (i.e., z-controller)
+ // **************************************
+ // BBBB OOO DDDD Y Y ZZZZZ
+ // B B O O D D Y Y Z
+ // BBBB O O D D Y Z
+ // B B O O D D Y Z
+ // BBBB OOO DDDD Y ZZZZZ
+ //
+ // ALITUDE CONTROLLER (i.e., z-controller)
+
+ // Instantiate the local variable for the thrust adjustment that will be
+ // requested from the Crazyflie's on-baord "inner-loop" controller
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the thrust adjustment to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ }
+
+ // Put the computed thrust adjustment into the "response" variable,
+ // as well as adding the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ // > NOTE: the "gravity_force" value was already divided by 4 when is was
+ // loaded from the .yaml file via the "fetchYamlParameters"
+ // class function in this file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- // Instantiate the local variable for the thrust adjustment that will be
- // requested from the Crazyflie's on-baord "inner-loop" controller
- float thrustAdjustment = 0;
- // Perform the "-Kx" LQR computation for the thrust adjustment to respoond with
- for(int i = 0; i < 9; ++i)
- {
- thrustAdjustment -= gainMatrixThrust[i] * stateErrorBody[i];
- }
- // Put the computed thrust adjustment into the "response" variable,
- // as well as adding the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
- // > NOTE: the "gravity_force" value was already divided by 4 when is was
- // loaded from the .yaml file via the "loadPPSTemplateParameters"
- // class function in this file.
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
-
-
-
-
- // **************************************
- // BBBB OOO DDDD Y Y X X
- // B B O O D D Y Y X X
- // BBBB O O D D Y X
- // B B O O D D Y X X
- // BBBB OOO DDDD Y X X
- //
- // BODY FRAME X CONTROLLER
-
- // Instantiate the local variable for the pitch rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float pitchRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the pitch rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- pitchRate_forResponse -= gainMatrixPitch[i] * stateErrorBody[i];
- }
- // Put the computed pitch rate into the "response" variable
- response.controlOutput.pitch = pitchRate_forResponse;
-
+ // **************************************
+ // BBBB OOO DDDD Y Y X X
+ // B B O O D D Y Y X X
+ // BBBB O O D D Y X
+ // B B O O D D Y X X
+ // BBBB OOO DDDD Y X X
+ //
+ // BODY FRAME X CONTROLLER
+
+ // Instantiate the local variable for the pitch rate that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+ float pitchRate_forResponse = 0;
+
+ // Perform the "-Kx" LQR computation for the pitch rate to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ }
+
+ // Put the computed pitch rate into the "response" variable
+ response.controlOutput.pitch = pitchRate_forResponse;
+
// **************************************
- // BBBB OOO DDDD Y Y Y Y
- // B B O O D D Y Y Y Y
- // BBBB O O D D Y Y
- // B B O O D D Y Y
- // BBBB OOO DDDD Y Y
- //
- // BODY FRAME Y CONTROLLER
-
- // Instantiate the local variable for the roll rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float rollRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the roll rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- rollRate_forResponse -= gainMatrixRoll[i] * stateErrorBody[i];
- }
+ // BBBB OOO DDDD Y Y Y Y
+ // B B O O D D Y Y Y Y
+ // BBBB O O D D Y Y
+ // B B O O D D Y Y
+ // BBBB OOO DDDD Y Y
+ //
+ // BODY FRAME Y CONTROLLER
- // Put the computed roll rate into the "response" variable
- response.controlOutput.roll = rollRate_forResponse;
-
-
+ // Instantiate the local variable for the roll rate that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ // Perform the "-Kx" LQR computation for the roll rate to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
+ }
+ // Put the computed roll rate into the "response" variable
+ response.controlOutput.roll = rollRate_forResponse;
- // PREPARE AND RETURN THE VARIABLE "response"
- /*choosing the Crazyflie onBoard controller type.
- it can either be Motor, Rate or Angle based */
- // response.controlOutput.onboardControllerType = MOTOR_MODE;
- response.controlOutput.onboardControllerType = RATE_MODE;
- // response.controlOutput.onboardControllerType = ANGLE_MODE;
-
- // SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
- previous_stateErrorInertial = request.ownCrazyflie; // we have already used previous location, update it
- // Adjust (x,y,z) for the stepoint
- previous_stateErrorInertial.x = request.ownCrazyflie.x - setpoint[0];
- previous_stateErrorInertial.y = request.ownCrazyflie.y - setpoint[1];
- previous_stateErrorInertial.z = request.ownCrazyflie.z - setpoint[2];
+ // PREPARE AND RETURN THE VARIABLE "response"
- // Adjust yaw for the stepoint
- previous_stateErrorInertial.yaw = stateErrorInertial[8];
+ /*choosing the Crazyflie onBoard controller type.
+ it can either be Motor, Rate or Angle based */
+ // response.controlOutput.onboardControllerType = MOTOR_MODE;
+ response.controlOutput.onboardControllerType = RATE_MODE;
+ // response.controlOutput.onboardControllerType = ANGLE_MODE;
+ // ***********************************************************
+ // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
+ // D D E B B U U G MM MM S G
+ // D D EEE BBBB U U G M M M SSS G
+ // D D E B B U U G G M M S G G
+ // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
// DEBUGGING CODE:
// As part of the D-FaLL-System we have defined a message type names"DebugMsg".
// By fill this message with data and publishing it you can display the data in
// real time using rpt plots. Instructions for using rqt plots can be found on
// the wiki of the D-FaLL-System repository
+ if (shouldPublishDebugMessage)
+ {
+ // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
+ // (located in the "msg" folder) to see the full structure of this message.
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ debugMsg.vicon_x = request.ownCrazyflie.x;
+ debugMsg.vicon_y = request.ownCrazyflie.y;
+ debugMsg.vicon_z = request.ownCrazyflie.z;
+ debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // Fill in the debugging message with any other data you would like to display
+ // in real time. For example, it might be useful to display the thrust
+ // adjustment computed by the z-altitude controller.
+ // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
+ // type "float64" that you can fill in with data you would like to plot in
+ // real-time.
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ // Publish the "debugMsg"
+ debugPublisher.publish(debugMsg);
+ }
+
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+ if (shouldDisplayDebugInfo)
+ {
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
+
+ // An example of "printing out" the control actions computed.
+ ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
+ ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
+ ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
+ ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+
+ // An example of "printing out" the per motor 16-bit command computed.
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforAngles(float stateErrorBody[9], Controller::Request &request, Controller::Response &response)
+{
+ // **********************
+ // Y Y A W W
+ // Y Y A A W W
+ // Y A A W W
+ // Y AAAAA W W W
+ // Y A A W W
+ //
+ // YAW CONTROLLER
+
+ // Put the yaw setpoint directly as the response
+ response.controlOutput.yaw = setpoint[3];
+
+
+
+
+ // **************************************
+ // BBBB OOO DDDD Y Y ZZZZZ
+ // B B O O D D Y Y Z
+ // BBBB O O D D Y Z
+ // B B O O D D Y Z
+ // BBBB OOO DDDD Y ZZZZZ
+ //
+ // ALITUDE CONTROLLER (i.e., z-controller)
+
+ // Instantiate the local variable for the thrust adjustment that will be
+ // requested from the Crazyflie's on-baord "inner-loop" controller
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the thrust adjustment to respoond with
+ for(int i = 0; i < 6; ++i)
+ {
+ thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
+ }
+
+ // Put the computed thrust adjustment into the "response" variable,
+ // as well as adding the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ // > NOTE: the "gravity_force" value was already divided by 4 when is was
+ // loaded from the .yaml file via the "fetchYamlParameters"
+ // class function in this file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
+
+
+
+
+ // **************************************
+ // BBBB OOO DDDD Y Y X X
+ // B B O O D D Y Y X X
+ // BBBB O O D D Y X
+ // B B O O D D Y X X
+ // BBBB OOO DDDD Y X X
+ //
+ // BODY FRAME X CONTROLLER
+
+ // Instantiate the local variable for the pitch angle that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+ float pitchAngle_forResponse = 0;
+
+ // Perform the "-Kx" LQR computation for the pitch angle to respoond with
+ for(int i = 0; i < 6; ++i)
+ {
+ pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
+ }
+
+ // Put the computed pitch angle into the "response" variable
+ response.controlOutput.pitch = pitchAngle_forResponse;
+
- // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
- // (located in the "msg" folder) to see the full structure of this message.
- DebugMsg debugMsg;
-
- // Fill the debugging message with the data provided by Vicon
- debugMsg.vicon_x = request.ownCrazyflie.x;
- debugMsg.vicon_y = request.ownCrazyflie.y;
- debugMsg.vicon_z = request.ownCrazyflie.z;
- debugMsg.vicon_roll = request.ownCrazyflie.roll;
- debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
- debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
-
- // Fill in the debugging message with any other data you would like to display
- // in real time. For example, it might be useful to display the thrust
- // adjustment computed by the z-altitude controller.
- // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
- // type "float64" that you can fill in with data you would like to plot in
- // real-time.
- // debugMsg.value_1 = thrustAdjustment;
- // ......................
- // debugMsg.value_10 = your_variable_name;
-
- // Publish the "debugMsg"
- debugPublisher.publish(debugMsg);
-
-
- // An alternate debugging technique is to print out data directly to the
- // command line from which this node was launched.
-
- // An example of "printing out" the data from the "request" argument to the
- // command line. This might be useful for debugging.
- // ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
- // ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
- // ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
- // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
- // ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
- // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
- // ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
-
- // An example of "printing out" the control actions computed.
- // ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- // ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- // ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- // ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
-
- // An example of "printing out" the "thrust-to-command" conversion parameters.
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
-
- // An example of "printing out" the per motor 16-bit command computed.
- // ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
- // ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
- // ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
- // ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
-
- // Return "true" to indicate that the control computation was performed successfully
- return true;
+
+
+ // **************************************
+ // BBBB OOO DDDD Y Y Y Y
+ // B B O O D D Y Y Y Y
+ // BBBB O O D D Y Y
+ // B B O O D D Y Y
+ // BBBB OOO DDDD Y Y
+ //
+ // BODY FRAME Y CONTROLLER
+
+ // Instantiate the local variable for the roll angle that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+ float rollAngle_forResponse = 0;
+
+ // Perform the "-Kx" LQR computation for the roll angle to respoond with
+ for(int i = 0; i < 6; ++i)
+ {
+ rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
+ }
+
+ // Put the computed roll angle into the "response" variable
+ response.controlOutput.roll = rollAngle_forResponse;
+
+
+
+
+ // PREPARE AND RETURN THE VARIABLE "response"
+
+ /*choosing the Crazyflie onBoard controller type.
+ it can either be Motor, Rate or Angle based */
+ //response.controlOutput.onboardControllerType = MOTOR_MODE;
+ //response.controlOutput.onboardControllerType = RATE_MODE;
+ response.controlOutput.onboardControllerType = ANGLE_MODE;
}
@@ -599,20 +895,43 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
{
- // Fill in the (x,y,z) position estimates to be returned
- stateBody[0] = stateInertial[0];
- stateBody[1] = stateInertial[1];
- stateBody[2] = stateInertial[2];
-
- // Fill in the (x,y,z) velocity estimates to be returned
- stateBody[3] = stateInertial[3];
- stateBody[4] = stateInertial[4];
- stateBody[5] = stateInertial[5];
-
- // Fill in the (roll,pitch,yaw) estimates to be returned
- stateBody[6] = stateInertial[6];
- stateBody[7] = stateInertial[7];
- stateBody[8] = stateInertial[8];
+ if (shouldPerformConvertIntoBodyFrame)
+ {
+ float sinYaw = sin(yaw_measured);
+ float cosYaw = cos(yaw_measured);
+
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0] * cosYaw + stateInertial[1] * sinYaw;
+ stateBody[1] = stateInertial[1] * cosYaw - stateInertial[0] * sinYaw;
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3] * cosYaw + stateInertial[4] * sinYaw;
+ stateBody[4] = stateInertial[4] * cosYaw - stateInertial[3] * sinYaw;
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+ }
+ else
+ {
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0];
+ stateBody[1] = stateInertial[1];
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3];
+ stateBody[4] = stateInertial[4];
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+ }
}
@@ -668,6 +987,24 @@ void setpointCallback(const Setpoint& newSetpoint)
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// > This function is called anytime a message is published on the topic to which the
+// class instance variable "xyz_yaw_to_follow_subscriber" is subscribed
+void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint)
+{
+ //ROS_INFO("DEBUGGING: Received new setpoint from another agent");
+ // The setpoint should only be updated if allow by the respective booelan
+ if (shouldFollowAnotherAgent)
+ {
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+ }
+}
+
+
+
// ----------------------------------------------------------------------------------
@@ -684,27 +1021,277 @@ void setpointCallback(const Setpoint& newSetpoint)
// P A A R R A A M M EEEEE T EEEEE R R SSSS
// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+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_CUSTOM_CONTROLLER_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not relevant
+ //ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+
+// 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)
+{
+ // Here we load the parameters that are specified in the CustomController.yaml file
+
+ // Add the "CustomController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_customController(nodeHandle, "CustomController");
+
+ // > The mass of the crazyflie
+ cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
+
+ // Display one of the YAML parameters to debug if it is working correctly
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ control_frequency = getParameterFloat(nodeHandle_for_customController, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_customController, "motorPoly", motorPoly, 3);
+
+ // > The boolean for whether to execute the convert into body frame function
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_customController, "shouldPerformConvertIntoBodyFrame");
+
+ // > The boolean indicating whether the (x,y,z,yaw) of this agent should be published
+ // or not
+ shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_customController, "shouldPublishCurrent_xyz_yaw");
+
+ // > The boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
+ // an attempt to follow the "my_current_xyz_yaw_topic" from another agent
+ shouldFollowAnotherAgent = getParameterBool(nodeHandle_for_customController, "shouldFollowAnotherAgent");
+
+ // > The ordered vector for ID's that specifies how the agents should follow eachother
+ follow_in_a_line_agentIDs.clear();
+ int temp_number_of_agents_in_a_line = getParameterIntVectorWithUnknownLength(nodeHandle_for_customController, "follow_in_a_line_agentIDs", follow_in_a_line_agentIDs);
+ // > Double check that the sizes agree
+ if ( temp_number_of_agents_in_a_line != follow_in_a_line_agentIDs.size() )
+ {
+ // Update the user if the sizes don't agree
+ ROS_ERROR_STREAM("parameter 'follow_in_a_line_agentIDs' was loaded with two different lengths, " << temp_number_of_agents_in_a_line << " versus " << follow_in_a_line_agentIDs.size() );
+ }
+
+ // Boolean indiciating whether the "Debug Message" of this agent should be published or not
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_customController, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_customController, "shouldDisplayDebugInfo");
+
+
+ // THE CONTROLLER GAINS:
+
+ // A flag for which controller to use, defined as:
+ controller_type = getParameterInt( nodeHandle_for_customController , "controller_type" );
+
+ // The LQR Controller parameters for "LQR_RATE_MODE"
+ getParameterFloatVector(nodeHandle_for_customController, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_customController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_customController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVector(nodeHandle_for_customController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+
+ // The LQR Controller parameters for "LQR_ANGLE_MODE"
+ getParameterFloatVector(nodeHandle_for_customController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_customController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVector(nodeHandle_for_customController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("DEBUGGING: the fetched SafeController/mass = " << cf_mass);
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+
+}
+
+
// This function CAN BE edited for successful completion of the PPS exercise, and the
// use of parameters loaded from the YAML file is highly recommended to make tuning of
// your controller easier and quicker.
-void loadPPSTemplateParameters(ros::NodeHandle& nodeHandle)
+void processFetchedParameters()
{
- // here we load the parameters that are in the CustomController.yaml
+ // Compute the feed-forward force that we need to counteract gravity (i.e., mg)
+ // > in units of [Newtons]
+ gravity_force = cf_mass * 9.81/(1000*4);
- cf_mass = getFloatParameter(nodeHandle, "mass");
- control_frequency = getFloatParameter(nodeHandle, "control_frequency");
- loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
+ // Look-up which agent should be followed
+ if (shouldFollowAnotherAgent)
+ {
+ // Only bother if the "follow_in_a_line_agentIDs" vector has a non-zero length
+ if (follow_in_a_line_agentIDs.size() > 0)
+ {
+ // Instantiate a local boolean variable for checking whether we found
+ // our own agent ID in the list
+ bool foundMyAgentID = false;
+ // Iterate through the vector of "follow_in_a_line_agentIDs"
+ for ( int i=0 ; i<follow_in_a_line_agentIDs.size() ; i++ )
+ {
+ // Check if the current entry matches the ID of this agent
+ if (follow_in_a_line_agentIDs[i] == my_agentID)
+ {
+ // Set the boolean flag that we have found out own agent ID
+ foundMyAgentID = true;
+ //ROS_INFO_STREAM("DEBUGGING: found my agent ID at index " << i );
+ // If it is the first entry, then this agent is the leader
+ if (i==0)
+ {
+ // The leader does not follow anyone else
+ shouldFollowAnotherAgent = false;
+ agentID_to_follow = 0;
+ }
+ else
+ {
+ // The agent ID to follow is the previous entry
+ agentID_to_follow = follow_in_a_line_agentIDs[i-1];
+ shouldFollowAnotherAgent = true;
+ // Subscribe to the "my_current_xyz_yaw_topic" of the agent ID
+ // that this agent should be following
+ ros::NodeHandle nodeHandle("~");
+ xyz_yaw_to_follow_subscriber = nodeHandle.subscribe("/" + std::to_string(agentID_to_follow) + "/my_current_xyz_yaw_topic", 1, xyz_yaw_to_follow_callback);
+ //ROS_INFO_STREAM("DEBUGGING: subscribed to agent ID = " << agentID_to_follow );
+ }
+ // Break out of the for loop as the assumption is that each agent ID
+ // appears only once in the "follow_in_a_line_agentIDs" vector of ID's
+ break;
+ }
+ }
+ // Check whether we found our own agent ID
+ if (!foundMyAgentID)
+ {
+ //ROS_INFO("DEBUGGING: not following because my ID was not found");
+ // Revert to the default of not following any agent
+ shouldFollowAnotherAgent = false;
+ agentID_to_follow = 0;
+ }
+ }
+ else
+ {
+ // As the "follow_in_a_line_agentIDs" vector has length zero, revert to the
+ // default of not following any agent
+ shouldFollowAnotherAgent = false;
+ agentID_to_follow = 0;
+ //ROS_INFO("DEBUGGING: not following because line vector has length zero");
+ }
+ }
+ else
+ {
+ // Set to its default value the integer specifying the ID of the agent that will
+ // be followed by this agent
+ agentID_to_follow = 0;
+ //ROS_INFO("DEBUGGING: not following because I was asked not to follow");
+ }
+
+ if (shouldFollowAnotherAgent)
+ {
+ ROS_INFO_STREAM("This agent (with ID " << my_agentID << ") will now follow agent ID " << agentID_to_follow );
+ }
+}
- // compute things that we will need after from these parameters
- // force that we need to counteract gravity (mg)
- gravity_force = cf_mass * 9.81/(1000*4); // in N
+/*
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters)
+{
+ // This function puts all the same parameters as the "fetchYamlParameters" function
+ // above into the same variables.
+ // The difference is that this function allows us to send all parameters from one
+ // central coordinator node
+ cf_mass = newCustomControllerParameters.mass;
+ control_frequency = newCustomControllerParameters.control_frequency;
+ for (int i=0;i<3;i++)
+ {
+ motorPoly[i] = newCustomControllerParameters.motorPoly[i];
+ }
+
+ // > The boolean for whether to execute the convert into body frame function
+ shouldPerformConvertIntoBodyFrame = newCustomControllerParameters.shouldPerformConvertIntoBodyFrame;
+
+ shouldPublishCurrent_xyz_yaw = newCustomControllerParameters.shouldPublishCurrent_xyz_yaw;
+ shouldFollowAnotherAgent = newCustomControllerParameters.shouldFollowAnotherAgent;
+ follow_in_a_line_agentIDs.clear();
+ for ( int i=0 ; i<newCustomControllerParameters.follow_in_a_line_agentIDs.size() ; i++ )
+ {
+ follow_in_a_line_agentIDs.push_back( newCustomControllerParameters.follow_in_a_line_agentIDs[i] );
+ }
+
+ // Let the user know that the message was received with new YAML parameters
+ ROS_INFO("Received message containing a new set of Custom Controller YAML parameters");
+
+ // Display one of the YAML parameters to debug if it is working correctly
+ ROS_INFO_STREAM("DEBUGGING: mass received in message = " << newCustomControllerParameters.mass );
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ ros::NodeHandle nodeHandle("~");
+ processFetchedParameters(nodeHandle);
+
}
+*/
-// load parameters from corresponding YAML file
-//
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
+// G E T P P A A R R A A MM MM ( )
+// G EEE T PPPP A A RRRR A A M M M ( )
+// G G E T P AAAAA R R AAAAA M M ( )
+// GGGG EEEEE T P A A R R A A M M ( )
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+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 << "'");
@@ -713,26 +1300,46 @@ void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std
ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
}
}
-
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-float getFloatParameter(ros::NodeHandle& nodeHandle, std::string name)
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
{
-
- float val;
+ int val;
if(!nodeHandle.getParam(name, val))
{
ROS_ERROR_STREAM("missing parameter '" << name << "'");
}
return val;
}
-
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void customYAMLloadedCallback(const std_msgs::Int32& msg)
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
{
- ros::NodeHandle nodeHandle("~");
- ROS_INFO("received msg custom loaded YAML");
- loadPPSTemplateParameters(nodeHandle);
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
}
+
+
@@ -755,8 +1362,86 @@ int main(int argc, char* argv[]) {
// the "~" indcates that "self" is the node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
- // Call the class function that loads the parameters for this class.
- loadPPSTemplateParameters(nodeHandle);
+ // Get the agent ID as the "ROS_NAMESPACE" this computer.
+ // NOTES:
+ // > If you look at the "Student.launch" file in the "launch" folder, you will see
+ // the following line of code:
+ // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "studentID" to the "PPSClient"
+ // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
+ // to the "PPSClient" node within which this controller service is nested.
+ // Get the namespace of this "CustomControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ // Get the handle to the "PPSClient" node
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+ // Get the value of the "studentID" parameter into the instance variable "my_agentID"
+ if(!PPSClient_nodeHandle.getParam("studentID", my_agentID))
+ {
+ // Throw an error if the student ID parameter could not be obtained
+ ROS_ERROR("Failed to get studentID from FollowN_1Service");
+ }
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // 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 = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // 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("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+
+
+ // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // *********************************************************************************
+
+
// Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
// advertises under the name "DebugTopic" and is a message with the structure
@@ -785,12 +1470,11 @@ int main(int argc, char* argv[]) {
// in the "DEFINES" section at the top of this file.
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
- // Instantiate the local variable "customYAMLloadedSubscriber" to be a "ros::Subscriber"
- // type variable that subscribes to the "student_GUI/customYAMLloaded" topic and calls
- // the class function "customYAMLloadedCallback" each time a messaged is received on
- // this topic. Such a message is sent when the button "Load Customcontroller YAML file"
- // is clicked in the student GUI.
- ros::Subscriber customYAMLloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/customYAMLloaded", 1, customYAMLloadedCallback);
+ // Instantiate the instance variable "my_current_xyz_yaw_publisher" to be a "ros::Publisher"
+ // that advertises under the name "<my_agentID>/my_current_xyz_yaw_topic" where <my_agentID>
+ // is filled in with the student ID number of this computer. The messages published will
+ // have the structure defined in the file "Setpoint.msg" (located in the "msg" folder).
+ my_current_xyz_yaw_publisher = nodeHandle.advertise<Setpoint>("/" + std::to_string(my_agentID) + "/my_current_xyz_yaw_topic", 1);
// Print out some information to the user.
ROS_INFO("CustomControllerService ready");
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 0bdf945820e024a937fdaed560f1f630d1429dcd..8f8d560e75e9fc9a9decdcce1c8bbc6beef8e177 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
// This file is part of D-FaLL-System.
//
@@ -30,10 +30,20 @@
// ----------------------------------------------------------------------------------
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 "ros/ros.h"
#include <stdlib.h>
#include <std_msgs/String.h>
-#include <rosbag/bag.h>
#include <ros/package.h>
#include "d_fall_pps/Controller.h"
@@ -47,18 +57,33 @@
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
-
#include "d_fall_pps/ControlCommand.h"
-// types PPS firmware
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// Types PPS firmware
#define TYPE_PPS_MOTORS 6
-#define TYPE_PPS_RATE 7
-#define TYPE_PPS_ANGLE 8
+#define TYPE_PPS_RATE 7
+#define TYPE_PPS_ANGLE 8
-// tipes of controllers being used:
+// Types of controllers being used:
#define SAFE_CONTROLLER 0
#define CUSTOM_CONTROLLER 1
+// The constants that "command" changes in the
+// operation state of this agent
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2
#define CMD_CRAZYFLY_TAKE_OFF 3
@@ -71,12 +96,11 @@
#define STATE_FLYING 3
#define STATE_LAND 4
-// battery states
-
+// Battery states
#define BATTERY_STATE_NORMAL 0
#define BATTERY_STATE_LOW 1
-// commands for CrazyRadio
+// Commands for CrazyRadio
#define CMD_RECONNECT 0
#define CMD_DISCONNECT 1
@@ -86,17 +110,37 @@
#define CONNECTING 1
#define DISCONNECTED 2
-// parameters for take off and landing. Eventually will go in YAML file
-#define TAKE_OFF_OFFSET 1 //in meters
-#define LANDING_DISTANCE 0.15 //in meters
-#define DURATION_TAKE_OFF 3 // seconds
-#define DURATION_LANDING 3 // seconds
+// For which controller parameters to load
+#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
+#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2
+#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
+#define FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+
+// Parameters for take off and landing. Eventually will go in YAML file
+//#define TAKE_OFF_OFFSET 1 //in meters
+//#define LANDING_DISTANCE 0.15 //in meters
+//#define DURATION_TAKE_OFF 3 // seconds
+//#define DURATION_LANDING 3 // seconds
+// Universal constants
#define PI 3.141592653589
+// Namespacing the package
using namespace d_fall_pps;
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
//studentID, gives namespace and identifier in CentralManagerService
int studentID;
@@ -147,7 +191,13 @@ ros::Publisher commandPublisher;
// communication with crazyRadio node. Connect and disconnect
ros::Publisher crazyRadioCommandPublisher;
-rosbag::Bag bag;
+
+// Variable for the namespaces for the paramter services
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
// variables for the states:
int flying_state;
@@ -180,6 +230,51 @@ bool finished_land = false;
ros::Timer timer_takeoff;
ros::Timer timer_land;
+// A ROS "bag" to store data for post-analysis
+//rosbag::Bag bag;
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// > For the LOAD PARAMETERS
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
+void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
+
+
+
+void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
void loadSafeController() {
ros::NodeHandle nodeHandle("~");
@@ -374,7 +469,7 @@ void changeFlyingStateTo(int new_state)
}
else
{
- ROS_INFO("Disconnected and trying to change state. Stays goes to MOTORS OFF");
+ ROS_INFO("Disconnected and trying to change state. State goes to MOTORS OFF");
flying_state = STATE_MOTORS_OFF;
}
@@ -529,8 +624,9 @@ void viconCallback(const ViconData& viconData) {
controlCommandPublisher.publish(controllerCall.response.controlOutput);
- bag.write("ViconData", ros::Time::now(), local);
- bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
+ // Putting data into the ROS "bag" for post-analysis
+ //bag.write("ViconData", ros::Time::now(), local);
+ //bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
}
}
else
@@ -538,63 +634,17 @@ void viconCallback(const ViconData& viconData) {
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = TYPE_PPS_MOTORS; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
- bag.write("ViconData", ros::Time::now(), local);
- bag.write("ControlOutput", ros::Time::now(), zeroOutput);
+
+ // Putting data into the ROS "bag" for post-analysis
+ //bag.write("ViconData", ros::Time::now(), local);
+ //bag.write("ControlOutput", ros::Time::now(), zeroOutput);
}
}
}
}
-void loadParameters(ros::NodeHandle& nodeHandle) {
- if(!nodeHandle.getParam("studentID", studentID)) {
- ROS_ERROR("Failed to get studentID");
- }
- if(!nodeHandle.getParam("strictSafety", strictSafety)) {
- ROS_ERROR("Failed to get strictSafety param");
- return;
- }
- if(!nodeHandle.getParam("angleMargin", angleMargin)) {
- ROS_ERROR("Failed to get angleMargin param");
- return;
- }
- if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
- ROS_ERROR("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("Failed to get battery_threshold_while_motors_off param");
- return;
- }
-}
-
-void loadSafeControllerParameters()
-{
- ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
- if(!nh_safeControllerService.getParam("takeOffDistance", take_off_distance))
- {
- ROS_ERROR("Failed to get takeOffDistance");
- }
-
- if(!nh_safeControllerService.getParam("landingDistance", landing_distance))
- {
- ROS_ERROR("Failed to get landing_distance");
- }
-
- if(!nh_safeControllerService.getParam("durationTakeOff", duration_take_off))
- {
- ROS_ERROR("Failed to get duration_take_off");
- }
- if(!nh_safeControllerService.getParam("durationLanding", duration_landing))
- {
- ROS_ERROR("Failed to get duration_landing");
- }
- if(!nh_safeControllerService.getParam("distanceThreshold", distance_threshold))
- {
- ROS_ERROR("Failed to get distance_threshold");
- }
-}
void loadCrazyflieContext() {
CMQuery contextCall;
@@ -686,6 +736,11 @@ void emergencyStopCallback(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;
@@ -708,12 +763,147 @@ void safeSetPointCallback(const Setpoint& newSetpoint)
}
-void safeYAMLloadedCallback(const std_msgs::Int32& msg)
+
+
+// ----------------------------------------------------------------------------------
+// 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)
{
- ROS_INFO("received msg safe loaded YAML");
- loadSafeControllerParameters();
+ // 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_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("The PPSClient 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_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ // > and also from where the paramters are being fetched
+ ROS_INFO("The PPSClient 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 "CustomController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
+
+ if(!nodeHandle_for_safeController.getParam("takeOffDistance", take_off_distance))
+ {
+ ROS_ERROR("Failed to get takeOffDistance");
+ }
+
+ if(!nodeHandle_for_safeController.getParam("landingDistance", landing_distance))
+ {
+ ROS_ERROR("Failed to get landing_distance");
+ }
+
+ if(!nodeHandle_for_safeController.getParam("durationTakeOff", duration_take_off))
+ {
+ ROS_ERROR("Failed to get duration_take_off");
+ }
+
+ if(!nodeHandle_for_safeController.getParam("durationLanding", duration_landing))
+ {
+ ROS_ERROR("Failed to get duration_landing");
+ }
+
+ if(!nodeHandle_for_safeController.getParam("distanceThreshold", distance_threshold))
+ {
+ ROS_ERROR("Failed to get distance_threshold");
+ }
}
+
+// > Load the paramters from the Client Config YAML file
+void fetchClientConfigParameters(ros::NodeHandle& nodeHandle)
+{
+ if(!nodeHandle.getParam("studentID", studentID)) {
+ ROS_ERROR("Failed to get studentID");
+ }
+ if(!nodeHandle.getParam("strictSafety", strictSafety)) {
+ ROS_ERROR("Failed to get strictSafety param");
+ return;
+ }
+ if(!nodeHandle.getParam("angleMargin", angleMargin)) {
+ ROS_ERROR("Failed to get angleMargin param");
+ return;
+ }
+ if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
+ ROS_ERROR("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("Failed to get battery_threshold_while_motors_off param");
+ return;
+ }
+}
+
+
+
+
+
+
+
+
+void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg)
+{
+ // The "msg" received can be directly published on the "crazyRadioCommandPublisher"
+ // class variable because it is same format message
+ // > NOTE: this may be inefficient to "just" pass on the message,
+ // the intention is that it is more transparent that the "coordinator"
+ // node requests all agents to (re/dis)-connect from, and the
+ // individual agents pass this along to their respective radio node.
+ crazyRadioCommandPublisher.publish(msg);
+}
+
+
+
int getBatteryState()
{
return m_battery_state;
@@ -792,32 +982,92 @@ void CFBatteryCallback(const std_msgs::Float32& msg)
}
}
+
+// ----------------------------------------------------------------------------------
+// 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[])
{
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
ros_namespace = ros::this_node::getNamespace();
- // load default setpoint
- std::vector<float> default_setpoint(4);
- ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+ // > Load the paramters from the Client Config YAML file
+ fetchClientConfigParameters(nodeHandle);
- ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
+ // Get the namespace of this "SafeControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
- if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
+
+ // 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");
+
+ if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
{
- ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
+ ROS_ERROR_STREAM("The PPSClient could not find parameter 'defaultSetpoint', as called from main(...)");
}
+ // 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];
- // load context parameters
- loadParameters(nodeHandle);
- loadSafeControllerParameters();
-
//ros::service::waitForService("/CentralManagerService/CentralManager");
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
@@ -864,10 +1114,14 @@ int main(int argc, char* argv[])
// 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);
+
// crazyradio status. Connected, connecting or disconnected
ros::Subscriber crazyRadioStatusSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);
- ros::Subscriber safeYAMloadedSubscriber = namespaceNodeHandle.subscribe("student_GUI/safeYAMLloaded", 1, safeYAMLloadedCallback);
+ // 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);
@@ -879,13 +1133,17 @@ int main(int argc, char* argv[])
setBatteryStateTo(BATTERY_STATE_NORMAL); //initialize battery state
- std::string package_path;
- package_path = ros::package::getPath("d_fall_pps") + "/";
- ROS_INFO_STREAM(package_path);
- std::string record_file = package_path + "LoggingPPSClient.bag";
- bag.open(record_file, rosbag::bagmode::Write);
+ // Open a ROS "bag" to store data for post-analysis
+ // std::string package_path;
+ // package_path = ros::package::getPath("d_fall_pps") + "/";
+ // ROS_INFO_STREAM(package_path);
+ // std::string record_file = package_path + "LoggingPPSClient.bag";
+ // bag.open(record_file, rosbag::bagmode::Write);
ros::spin();
- bag.close();
+
+ // Close the ROS "bag" that was opened to store data for post-analysis
+ //bag.close();
+
return 0;
}
diff --git a/pps_ws/src/d_fall_pps/src/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/ParameterService.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..cf9c361e14e6fc22ef9ff49c17b7cde5a0071981
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/ParameterService.cpp
@@ -0,0 +1,424 @@
+// 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 <string>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <ros/network.h>
+#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+#include "d_fall_pps/Controller.h"
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// For which controller parameters to load
+#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
+#define LOAD_YAML_CUSTOM_CONTROLLER_AGENT 2
+#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 3
+#define LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+
+#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
+#define FETCH_YAML_CUSTOM_CONTROLLER_FROM_OWN_AGENT 2
+#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 3
+#define FETCH_YAML_CUSTOM_CONTROLLER_FROM_COORDINATOR 4
+
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 2
+
+
+// Namespacing the package
+using namespace d_fall_pps;
+//using namespace std;
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// The type of this node, i.e., agent or a coordinator, specified as a parameter in the
+// "*.launch" file
+int my_type = 0;
+
+// The ID of this agent, i.e., the ID of this computer in the case that this computer is
+// and agent
+int my_agentID = 0;
+
+// Publisher that notifies the relevant nodes when the YAML paramters have been loaded
+// from file into ram/cache, and hence are ready to be fetched
+ros::Publisher controllerYamlReadyForFetchPublihser;
+
+
+std::string m_ros_namespace;
+
+ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+void requestLoadControllerYamlCallback(const std_msgs::Int32& msg);
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
+{
+ // Extract from the "msg" for which controller the YAML
+ // parameters should be loaded
+ int controller_to_load_yaml = msg.data;
+
+ ROS_INFO_STREAM("The Parameter Service node received the message to load YAML parameters from file into cache");
+
+
+ // Instantiate a local varaible to confirm that something can actually be loaded
+ // from a YAML file
+ bool isValidToAttemptLoad = true;
+
+ // Instantiate a local variable for the string that will be passed to the "system"
+ std::string cmd;
+
+ // Get the abolute path to "d_fall_pps"
+ std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
+
+ // Switch between loading for the different controllers
+ if ( (controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR) )
+ {
+ // Re-load the parameters of the safe controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeController";
+ }
+ else if ( (controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT) )
+ {
+ // Re-load the parameters of the safe controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeController";
+ }
+ else if ( (controller_to_load_yaml==LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR) )
+ {
+ // Re-load the parameters of the custom controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomController";
+ }
+ else if ( (controller_to_load_yaml==LOAD_YAML_CUSTOM_CONTROLLER_AGENT) && (my_type==TYPE_AGENT) )
+ {
+ // Re-load the parameters of the custom controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomController";
+ }
+ else
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("> Nothing to load for this parameter service with.");
+ ROS_INFO_STREAM("> The message received has 'controller_to_load_yaml' = " << controller_to_load_yaml);
+ ROS_INFO_STREAM("> And the type of this Parameter Service is 'my_type' = " << my_type);
+ // Set the boolean that prevents the fetch message from being sent
+ isValidToAttemptLoad = false;
+ }
+
+
+ // Only bother with ttempting to loaded the .yaml file, and subseuently send the "ready for fetch"
+ // message if something can actually be loaded from a YAML file
+ if (isValidToAttemptLoad)
+ {
+ // Let the user know what is about to happen
+ ROS_INFO_STREAM("> The following path will be used for locating the .yaml file: " << d_fall_pps_path << " The comand line string sent to the 'system' is: " << cmd );
+
+ // Re-load the parameters by pass the command line string via a "system" call
+ // > i.e., this replicates pasting this string in a new terminal window and pressing enter
+ system(cmd.c_str());
+
+ // Pause breifly to ensure that the yaml file is fully loaded
+ ros::Duration(0.5).sleep();
+
+ // Instantiate a local varaible to confirm that something was actually loaded from
+ // a YAML file
+ bool isReadyForFetch = true;
+
+ // Instantiate a local variable for the fetch message
+ std_msgs::Int32 fetch_msg;
+ // Fill in the data of the fetch message
+ switch(controller_to_load_yaml)
+ {
+ case (LOAD_YAML_SAFE_CONTROLLER_COORDINATOR):
+ fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR;
+ break;
+ case (LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR):
+ fetch_msg.data = FETCH_YAML_CUSTOM_CONTROLLER_FROM_COORDINATOR;
+ break;
+ case (LOAD_YAML_SAFE_CONTROLLER_AGENT):
+ fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT;
+ break;
+ case (LOAD_YAML_CUSTOM_CONTROLLER_AGENT):
+ fetch_msg.data = FETCH_YAML_CUSTOM_CONTROLLER_FROM_OWN_AGENT;
+ break;
+ default:
+ // Let the user know that the command was not recognised
+ ROS_INFO("Unknown 'controller to load yaml' command, thus a 'ready to fetch' message will NOT be published.");
+ // Set the boolean that prevents the fetch message from being sent
+ isReadyForFetch = false;
+ break;
+ }
+ // Send a message that the YAML parameter have been loaded and hence are
+ // ready to be fetched (i.e., using getparam())
+ if (isReadyForFetch)
+ {
+ controllerYamlReadyForFetchPublihser.publish(fetch_msg);
+ }
+ }
+}
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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, "ParameterService");
+
+ // 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("Failed to get type from ParameterService");
+ }
+
+ // Set the "my_type" instance variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ my_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ my_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "my_type" to the value indicating that it is invlid
+ my_type = TYPE_INVALID;
+ ROS_ERROR("The retrieve type parameter was no recognised.");
+ }
+
+
+ // Get the value of the "agentID" parameter into the instance variable "my_agentID"
+ if(!nodeHandle.getParam("agentID", my_agentID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("Failed to get agentID from ParameterService");
+ }
+
+
+ // Publisher that notifies the relevant nodes when the YAML paramters have been loaded
+ // from file into ram/cache, and hence are ready to be fetched
+ controllerYamlReadyForFetchPublihser = nodeHandle.advertise<std_msgs::Int32>("controllerYamlReadyForFetch", 1);
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (my_type)
+ {
+ case TYPE_AGENT:
+ {
+ //m_ros_namespace = ros::this_node::getNamespace();
+ m_ros_namespace = "/" + std::to_string(my_agentID) + '/' + "ParameterService";
+ ROS_INFO_STREAM("This Paramter Sercice will load .yaml file parameters into the 'base' namespace: " << m_ros_namespace);
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ //m_ros_namespace = ros::this_node::getNamespace();
+ m_ros_namespace = "/ParameterService";
+ ROS_INFO_STREAM("This Paramter Sercice will load .yaml file parameters into the 'base' namespace: " << m_ros_namespace);
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("The 'my_type' type parameter was not recognised.");
+ break;
+ }
+ }
+
+
+
+
+ // SUBSCRIBE TO THE APPROPRIATE "request" MESSAGES DEPENDING ON THE "my_type"
+ // Delare the subscribers as local variables here so that they persist for the life
+ // time of this main() function. The varaibles will be assigned in the switch case below
+ // > Subscribers for when this Parameter Service node is: TYPE_AGENT
+ ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
+ ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_coordinator;
+ // > Subscribers for when this Parameter Service node is: TYPE_COORDINATOR
+ ros::Subscriber requestLoadControllerYamlSubscriber_coordinator_to_self;
+
+ // SUBSCRIBE TO THE APPROPRIATE "request" MESSAGES DEPENDING ON THE "my_type"
+ switch (my_type)
+ {
+ // AN AGENT TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ // > The agent's own "PPSClient" node
+ case TYPE_AGENT:
+ {
+ // Subscribing to the agent's own PPSclient
+ // > First: Construct a node handle to the PPSclient
+ ros::NodeHandle nh_PPSClient_for_this_agent("PPSClient");
+ // > Second: Subscribe to the "requestLoadControllerYaml" topic
+ requestLoadControllerYamlSubscriber_agent_to_self = nh_PPSClient_for_this_agent.subscribe("requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
+
+ // Subscribing to the coordinator
+ // > First: construct a node handle to the coordinator
+ ros::NodeHandle nh_coordinator_for_this_agent = ros::NodeHandle();
+ // > Second: Subscribe to the "requestLoadControllerYaml" topic
+ requestLoadControllerYamlSubscriber_agent_to_coordinator = nh_coordinator_for_this_agent.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
+
+ // Inform the user what was subscribed to:
+ ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Subscribing to the coordinator's own "my_GUI"
+ // > First: Get the node handle required
+ ros::NodeHandle nh_coordinator_for_this_coordinator = ros::NodeHandle();
+ // > Second: Subscribe to requests from: the master GUI
+ requestLoadControllerYamlSubscriber_coordinator_to_self = nh_coordinator_for_this_coordinator.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
+
+ // Inform the user what was subscribed to:
+ ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from 'my_GUI'");
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("The 'my_type' type parameter was not recognised.");
+ break;
+ }
+ }
+
+
+ ROS_INFO("CentralManagerService ready");
+ ros::spin();
+
+ return 0;
+}
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index 5b6e14fc093eaf818345c17017e0b61590d41a09..e409c2619935291d8a33af8c6906de1de9c86074 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Cyrill Burgener, Marco Mueller, Philipp Friedli
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
// This file is part of D-FaLL-System.
//
@@ -30,11 +30,23 @@
// ----------------------------------------------------------------------------------
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <std_msgs/String.h>
-#include <rosbag/bag.h>
#include <ros/package.h>
#include "std_msgs/Float32.h"
@@ -42,15 +54,63 @@
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
-// #include "d_fall_pps/Debugging.h" //---------------------------------------------------------------------------
#include <std_msgs/Int32.h>
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// Universal constants
#define PI 3.1415926535
-#define RATE_CONTROLLER 7
+// 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.
+// The following is a short description about each mode:
+// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+#define MOTOR_MODE 6
+#define RATE_MODE 7
+#define ANGLE_MODE 8
+
+// Constants for feching the yaml paramters
+#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
+#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2
+#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
+#define FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+
+// Namespacing the package
using namespace d_fall_pps;
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
std::vector<float> ffThrust(4);
std::vector<float> feedforwardMotor(4);
float cf_mass;
@@ -74,87 +134,80 @@ float saturationThrust;
CrazyflieData previousLocation;
-rosbag::Bag bag;
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-float getFloatParameter(ros::NodeHandle& nodeHandle, std::string name)
-{
-
- float val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
+// Variable for the namespaces for the paramter services
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
-void loadParameterFloatVector(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");
- }
-}
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
-void loadSafeParameters(ros::NodeHandle& nodeHandle) {
- loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
- cf_mass = getFloatParameter(nodeHandle, "mass");
- // force that we need to counteract gravity (mg)
- gravity_force = cf_mass * 9.81/(1000*4); // in N
- saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
- loadParameterFloatVector(nodeHandle, "gainMatrixRoll", gainMatrixRoll, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
+// > For the CONTROL LOOP
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured);
+float computeMotorPolyBackward(float thrust);
+void estimateState(Controller::Request &request, float (&est)[9]);
- loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
- loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
+// > For the LOAD PARAMETERS
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
- loadParameterFloatVector(nodeHandle, "defaultSetpoint", defaultSetpoint, 4);
-}
+// > For the GETPARAM()
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-float computeMotorPolyBackward(float thrust) {
- return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
-}
-//Kalman
-void estimateState(Controller::Request &request, float (&est)[9]) {
- // attitude
- est[6] = request.ownCrazyflie.roll;
- est[7] = request.ownCrazyflie.pitch;
- est[8] = request.ownCrazyflie.yaw;
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
- //velocity & filtering
- float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
- ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
- ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
- ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
- ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
-
- float k_x[6]; //filterGain times state
- k_x[0] = request.ownCrazyflie.x * filterGain[0];
- k_x[1] = request.ownCrazyflie.y * filterGain[1];
- k_x[2] = request.ownCrazyflie.z * filterGain[2];
- k_x[3] = request.ownCrazyflie.x * filterGain[3];
- k_x[4] = request.ownCrazyflie.y * filterGain[4];
- k_x[5] = request.ownCrazyflie.z * filterGain[5];
-
- est[0] = ahat_x[0] + k_x[0];
- est[1] = ahat_x[1] + k_x[1];
- est[2] = ahat_x[2] + k_x[2];
- est[3] = ahat_x[3] + k_x[3];
- est[4] = ahat_x[4] + k_x[4];
- est[5] = ahat_x[5] + k_x[5];
- memcpy(prevEstimate, est, 9 * sizeof(float));
-}
+// ------------------------------------------------------------------------------
+// CCCC OOO N N TTTTT RRRR OOO L L OOO OOO PPPP
+// C O O NN N T R R O O L L O O O O P P
+// C O O N N N T RRRR O O L L O O O O PPPP
+// C O O N NN T R R O O L L O O O O P
+// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
+// ----------------------------------------------------------------------------------
//simple derivative
/*
@@ -173,29 +226,10 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
}
*/
-void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
- float sinYaw = sin(yaw_measured);
- float cosYaw = cos(yaw_measured);
-
- state[0] = est[0] * cosYaw + est[1] * sinYaw;
- state[1] = -est[0] * sinYaw + est[1] * cosYaw;
- state[2] = est[2];
-
- state[3] = est[3] * cosYaw + est[4] * sinYaw;
- state[4] = -est[3] * sinYaw + est[4] * cosYaw;
- state[5] = est[5];
-
- state[6] = est[6];
- state[7] = est[7];
- state[8] = est[8];
-}
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
- // ros::NodeHandle nodeHandle("~");
- // loadSafeParameters(nodeHandle); // do not put this here, cannot control anymore
-
-
+
float yaw_measured = request.ownCrazyflie.yaw;
//move coordinate system to make setpoint origin
@@ -204,7 +238,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
request.ownCrazyflie.z -= setpoint[2];
float yaw = request.ownCrazyflie.yaw - setpoint[3];
-
+
//bag.write("Offset", ros::Time::now(), request.ownCrazyflie);
while(yaw > PI) {yaw -= 2 * PI;}
@@ -213,23 +247,19 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
estimateState(request, est);
-
float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
convertIntoBodyFrame(est, state, yaw_measured);
- std_msgs::Float32 f32;
- f32.data = yaw_measured;
-
//calculate feedback
float outRoll = 0;
float outPitch = 0;
float outYaw = 0;
float thrustIntermediate = 0;
for(int i = 0; i < 9; ++i) {
- outRoll -= gainMatrixRoll[i] * state[i];
- outPitch -= gainMatrixPitch[i] * state[i];
- outYaw -= gainMatrixYaw[i] * state[i];
- thrustIntermediate -= gainMatrixThrust[i] * state[i];
+ outRoll -= gainMatrixRoll[i] * state[i];
+ outPitch -= gainMatrixPitch[i] * state[i];
+ outYaw -= gainMatrixYaw[i] * state[i];
+ thrustIntermediate -= gainMatrixThrust[i] * state[i];
}
// ROS_INFO_STREAM("thrustIntermediate = " << thrustIntermediate);
//INFORMATION: this ugly fix was needed for the older firmware
@@ -255,15 +285,272 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
// ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
- response.controlOutput.onboardControllerType = RATE_CONTROLLER;
+ response.controlOutput.onboardControllerType = RATE_MODE;
previousLocation = request.ownCrazyflie;
- bag.write("ControlOutput", ros::Time::now(), response.controlOutput);
+ return true;
+}
+
+void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured)
+{
+ float sinYaw = sin(yaw_measured);
+ float cosYaw = cos(yaw_measured);
+
+ state[0] = est[0] * cosYaw + est[1] * sinYaw;
+ state[1] = -est[0] * sinYaw + est[1] * cosYaw;
+ state[2] = est[2];
+
+ state[3] = est[3] * cosYaw + est[4] * sinYaw;
+ state[4] = -est[3] * sinYaw + est[4] * cosYaw;
+ state[5] = est[5];
+
+ state[6] = est[6];
+ state[7] = est[7];
+ state[8] = est[8];
+}
+
+
+
+float computeMotorPolyBackward(float thrust)
+{
+ return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+}
+
+
+//Kalman
+void estimateState(Controller::Request &request, float (&est)[9])
+{
+ // attitude
+ est[6] = request.ownCrazyflie.roll;
+ est[7] = request.ownCrazyflie.pitch;
+ est[8] = request.ownCrazyflie.yaw;
+
+ //velocity & filtering
+ float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
+ ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
+ ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
+ ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
+ ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
+
+
+ float k_x[6]; //filterGain times state
+ k_x[0] = request.ownCrazyflie.x * filterGain[0];
+ k_x[1] = request.ownCrazyflie.y * filterGain[1];
+ k_x[2] = request.ownCrazyflie.z * filterGain[2];
+ k_x[3] = request.ownCrazyflie.x * filterGain[3];
+ k_x[4] = request.ownCrazyflie.y * filterGain[4];
+ k_x[5] = request.ownCrazyflie.z * filterGain[5];
+
+ est[0] = ahat_x[0] + k_x[0];
+ est[1] = ahat_x[1] + k_x[1];
+ est[2] = ahat_x[2] + k_x[2];
+ est[3] = ahat_x[3] + k_x[3];
+ est[4] = ahat_x[4] + k_x[4];
+ est[5] = ahat_x[5] + k_x[5];
+
+ memcpy(prevEstimate, est, 9 * sizeof(float));
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+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_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded. > 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
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_SAFE_CONTROLLER_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not relevant
+ //ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded.\r> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+{
+
+ // Here we load the parameters that are specified in the SafeController.yaml file
+
+ // Add the "CustomController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
+
+ // > The mass of the crazyflie
+ cf_mass = getParameterFloat(nodeHandle_for_safeController, "mass");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_safeController, "motorPoly", motorPoly, 3);
+
+
+ // > The row of the LQR matrix that commands body frame roll rate
+ getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixRoll", gainMatrixRoll, 9);
+ // > The row of the LQR matrix that commands body frame pitch rate
+ getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixPitch", gainMatrixPitch, 9);
+ // > The row of the LQR matrix that commands body frame yaw rate
+ getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixYaw", gainMatrixYaw, 9);
+ // > The row of the LQR matrix that commands thrust adjustment
+ getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixThrust", gainMatrixThrust, 9);
+
+ // > The gains for the point-mass Kalman filter
+ getParameterFloatVector(nodeHandle_for_safeController, "filterGain", filterGain, 6);
+ getParameterFloatVector(nodeHandle_for_safeController, "estimatorMatrix", estimatorMatrix, 2);
+
+ // > The defailt setpoint of the controller
+ getParameterFloatVector(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("DEBUGGING: the fetched SafeController/mass = " << cf_mass);
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+}
+
+void processFetchedParameters()
+{
+ // force that we need to counteract gravity (mg)
+ gravity_force = cf_mass * 9.81/(1000*4); // in N
+ // The maximum possible thrust
+ saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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;
+}
+
+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");
+ }
+}
+
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
- return true;
+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 getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// NN N E W W S E T P P O O I NN N T
+// N N N EEE W W SSS EEE T PPPP O O I N N N T
+// N NN E W W W S E T P O O I N NN T
+// N N EEEEE W W SSSS EEEEE T P OOO III N N T
+//
+// GGG U U III CCCC A L L BBBB A CCCC K K
+// G G U U I C A A L L B B A A C K K
+// G U U I C A A L L BBBB A A C KKK
+// G G U U I C AAAAA L L B B AAAAA C K K
+// GGGG UUU III CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
void setpointCallback(const Setpoint& newSetpoint) {
setpoint[0] = newSetpoint.x;
setpoint[1] = newSetpoint.y;
@@ -271,41 +558,108 @@ void setpointCallback(const Setpoint& newSetpoint) {
setpoint[3] = newSetpoint.yaw;
}
-void safeYAMLloadedCallback(const std_msgs::Int32& msg)
-{
- ros::NodeHandle nodeHandle("~");
- ROS_INFO("received msg safe loaded YAML");
- loadSafeParameters(nodeHandle);
-}
-//ros::Publisher pub;
+
+
+// ----------------------------------------------------------------------------------
+// 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[]) {
ros::init(argc, argv, "SafeControllerService");
ros::NodeHandle nodeHandle("~");
- loadSafeParameters(nodeHandle);
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+
+ // Get the namespace of this "SafeControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+
+ // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
+ // namespace string for the parameter service that is running on the machine of this
+ // agent
+ namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // 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("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+
+
+ // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // *********************************************************************************
+
+
+
+
setpoint = defaultSetpoint; // only first time setpoint is equal to default setpoint
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
- ros::Subscriber safeYAMloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/safeYAMLloaded", 1, safeYAMLloadedCallback);
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready");
- std::string package_path;
- package_path = ros::package::getPath("d_fall_pps") + "/";
- ROS_INFO_STREAM(package_path);
- std::string record_file = package_path + "LoggingSafeController.bag";
- bag.open(record_file, rosbag::bagmode::Write);
+ // std::string package_path;
+ // package_path = ros::package::getPath("d_fall_pps") + "/";
+ // ROS_INFO_STREAM(package_path);
+ // std::string record_file = package_path + "LoggingSafeController.bag";
+ // bag.open(record_file, rosbag::bagmode::Write);
ros::spin();
- bag.close();
+ // bag.close();