diff --git a/dfall_ws/src/dfall_pkg/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
index 094f23d60237fe60102fa9c27e2b4e63aafdcf3c..3191d29bc6013ade732622540dd090f332b95540 100755
--- a/dfall_ws/src/dfall_pkg/CMakeLists.txt
+++ b/dfall_ws/src/dfall_pkg/CMakeLists.txt
@@ -328,6 +328,8 @@ add_executable(PickerControllerService src/nodes/PickerControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(TemplateControllerService src/nodes/TemplateControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(TestMotorsControllerService src/nodes/TestMotorsControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
@@ -403,6 +405,7 @@ add_dependencies(RemoteControllerService dfall_pkg_generate_messages_cpp ${cat
add_dependencies(TuningControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PickerControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TemplateControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(TestMotorsControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -455,6 +458,7 @@ target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
target_link_libraries(PickerControllerService ${catkin_LIBRARIES})
target_link_libraries(TemplateControllerService ${catkin_LIBRARIES})
+target_link_libraries(TestMotorsControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
index e848317a9561d340ffe22e4156c7c15d4f270e0b..faec75cbad2346e7b65a49e0135356bdec56791e 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
@@ -215,6 +215,8 @@
<addaction name="action_showHideController_picker"/>
<addaction name="action_showHideController_tuning"/>
<addaction name="action_showHideController_template"/>
+ <addaction name="separator"/>
+ <addaction name="action_testMotors"/>
</widget>
<addaction name="menuFile"/>
<addaction name="menuLoad_YAML"/>
@@ -313,6 +315,11 @@
<string>Template</string>
</property>
</action>
+ <action name="action_testMotors">
+ <property name="text">
+ <string>TestMotors</string>
+ </property>
+ </action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<customwidgets>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
index a6d8f5584ef4b07122c32857256b9a206e8b4b47..723bdf8da175f7ea13ff3db977ae2cbc79d87b62 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
@@ -116,6 +116,8 @@
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
+#define TESTMOTORS_CONTROLLER 21
+
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_DEFAULT_CONTROLLER 1
@@ -132,6 +134,8 @@
#define CMD_CRAZYFLY_LAND 12
#define CMD_CRAZYFLY_MOTORS_OFF 13
+#define CMD_USE_TESTMOTORS_CONTROLLER 21
+
// Flying states
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF 2
@@ -208,4 +212,4 @@
// ----------------------------------------------------------------------------------
// For standard buttons in the GUI
-#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
\ No newline at end of file
+#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
index 3ffd3ff9f83268b23da78a423237137f1a81a7c9..ff4cb22e2f14a4a199815371561dbca96b18b902 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
@@ -94,6 +94,8 @@ public:
void showHideController_tuning_changed();
void showHideController_template_changed();
+ void testMotors_triggered();
+
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
index 916869bd8ed40c3807059ec66cfe184db17b2f5f..17da92a8fe72c3fe87f03740c1c5dcb13113a4d4 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
@@ -133,6 +133,8 @@ private slots:
void on_action_showHideController_tuning_changed();
void on_action_showHideController_template_changed();
+ void on_action_testMotors_triggered();
+
};
#endif // MAINWINDOW_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
index d5389a8bf817fb1bbaf74c9c4404151a614650b2..43cc94a3e49af91ce382375bb753b79d4ecfd9aa 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
@@ -116,6 +116,20 @@ void EnableControllerLoadYamlBar::showHideController_template_changed()
+// TEST MOTORS
+
+void EnableControllerLoadYamlBar::testMotors_triggered()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_TESTMOTORS_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Test Motors Controller");
+#endif
+}
+
+
// ENABLE CONTROLLER BUTTONS ON-CLICK CALLBACK
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
index 0a523fe34ee7aa2199456e18ef1cea04d7fa64e9..8129239a5991635ea4d0c574480a214bf879bf39 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
@@ -253,6 +253,12 @@ void MainWindow::on_action_showHideController_template_changed()
}
+void MainWindow::on_action_testMotors_triggered()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->testMotors_triggered();
+}
+
// ----------------------------------------------------------------------------------
// III DDDD &&& TTTTT Y Y PPPP EEEEE
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
index b8615d178ff566ddb396a81149aef86b6b34a7a0..b17fab5ae548310ea39023d50f2b7cfce97b8ca7 100755
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
@@ -410,16 +410,27 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
QString filename(":/images/drone_fixed_");
- if(std::regex_search(s, m, e))
- {
- std::string found_string = m[1].str();
- filename.append(QString::fromStdString(found_string));
- filename.append(".svg");
- }
- else
- {
- filename.append("unk.svg");
- }
+ if(std::regex_search(s, m, e))
+ {
+ // Get the string that was found
+ // > it should be a zero-padded 2-digit number
+ std::string found_string = m[1].str();
+ // Convert to an integer
+ int found_string_as_int = QString::fromStdString(found_string).toInt();
+ if ((1 <= found_string_as_int) && (found_string_as_int <= 9))
+ {
+ filename.append(QString::fromStdString(found_string));
+ filename.append(".svg");
+ }
+ else
+ {
+ filename.append("unk.svg");
+ }
+ }
+ else
+ {
+ filename.append("unk.svg");
+ }
crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]), filename);
tmp_p_crazyfly->setScaleCFs(ui->scaleSpinBox->value());
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
index 0843e1a25220581d77dc5ad49472c6ff2b8b257a..707472a545355449ca5f172c32e8236c1bd06f40 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
@@ -129,6 +129,8 @@
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
+#define TESTMOTORS_CONTROLLER 21
+
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_DEFAULT_CONTROLLER 1
@@ -145,6 +147,8 @@
#define CMD_CRAZYFLY_LAND 12
#define CMD_CRAZYFLY_MOTORS_OFF 13
+#define CMD_USE_TESTMOTORS_CONTROLLER 21
+
// Flying states
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF 2
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
index a9beaf7ed658f18b79df4af6b7a30c9f250fed49..2a79f4511574a6dd86d3f266f062b67920029309 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
@@ -39,6 +39,14 @@
#define DEFAULT_CONTROLLER_REQUEST_LANDING 2
+
+// TO NOTIFY THAT MANOEUVRES ARE COMPLETED
+
+#define DEFAULT_CONTROLLER_TAKEOFF_COMPLETE 1
+#define DEFAULT_CONTROLLER_LANDING_COMPLETE 2
+
+
+
// TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
#define DEFAULT_CONTROLLER_STATE_NORMAL 1
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index b415d9875e153e1327001631918c73e44edda140..fb83bc8ea77490f845fc0f5da5abe7ba593461fa 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -70,6 +70,7 @@
#include "dfall_pkg/IntIntService.h"
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
+#include "dfall_pkg/CMQuery.h"
// Include the shared definitions
#include "nodes/Constants.h"
@@ -369,12 +370,27 @@ ros::Publisher m_debugPublisher;
// changes to the setpoint
ros::Publisher m_setpointChangedPublisher;
+// ROS Publisher to inform the flying agent client
+// when a requested manoeuvre is complete
+ros::Publisher m_manoeuvreCompletePublisher;
// ROS Publisher for sending motors-off command
// to the flying agent client
ros::Publisher m_motorsOffToFlyingAgentClientPublisher;
+// Describes the area of the crazyflie and other parameters
+//CrazyflieContext m_context;
+// Flag for whether a context is available or not
+bool m_isAvailableContext = false;
+
+// Variable for each coordinate
+float m_symmetric_area_bounds_x = 0.5;
+float m_symmetric_area_bounds_y = 0.5;
+float m_symmetric_area_bounds_z = 0.5;
+
+// Service Client from which the "CrazyflieContext" is loaded
+ros::ServiceClient m_centralManager;
@@ -458,12 +474,21 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
// PUBLISH THE CURRENT SETPOINT AND STATE
void publishCurrentSetpointAndState();
+// CLIP TO MIN AND MAX
+float clipToBounds(float val, float val_min, float val_max);
+
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
void publish_motors_off_to_flying_agent_client();
+// FUNCTIONS FOR THE CONTEXT OF THIS AGENT
+// > Callback that the context database changed
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
+// > For loading the context of this agent
+void loadCrazyflieContext();
+
// LOADING OF YAML PARAMETERS
void timerCallback_initial_load_yaml(const ros::TimerEvent&);
void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index 2491524e6a8d0fe5b1b8c9dc337da1da581e74b8..2946ad4914cb7fa1bc26f16ee6ac18375eed5ce1 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -204,6 +204,8 @@ ros::ServiceClient m_pickerController;
// The Template controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_templateController;
+// The Test Mtoros controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_testMotorsController;
@@ -294,6 +296,8 @@ void requestChangeFlyingStateToLand();
void takeOffTimerCallback(const ros::TimerEvent&);
// > Callback that the landing phase is complete
void landTimerCallback(const ros::TimerEvent&);
+// > Callback that the Default Controller complete a manoeuvre
+void defaultControllerManoeuvreCompleteCallback(const IntWithHeader & msg);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/TestMotorsControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TestMotorsControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..bfe8717bd41138ae6754cba3bd40951ade020260
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TestMotorsControllerService.h
@@ -0,0 +1,237 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// A controller to test the motors
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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.
+
+// NOTE: many constants are already defined in the
+// "Constant.h" header file
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
+// > the mass of the crazyflie, in [grams]
+float yaml_cf_mass_in_grams = 25.0;
+
+// > the frequency at which the controller is running
+float yaml_control_frequency = 200.0;
+
+// > the coefficients of the 16-bit command to thrust conversion
+//std::vector<float> yaml_motorPoly(3);
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+
+// The min and max for saturating 16 bit thrust commands
+float yaml_command_sixteenbit_min = 1000;
+float yaml_command_sixteenbit_max = 60000;
+
+// > the default setpoint, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool yaml_shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool yaml_shouldDisplayDebugInfo = false;
+
+// The LQR Controller parameters for "LQR_RATE_MODE"
+std::vector<float> yaml_gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixRollRate = { 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixPitchRate = { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
+std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
+
+
+
+// The weight of the Crazyflie in Newtons, i.e., mg
+float m_cf_weight_in_newtons = 0.0;
+
+// The location error of the Crazyflie at the "previous" time step
+float m_previous_stateErrorInertial[9];
+
+// The setpoint to be tracked, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
+
+
+
+
+// ROS Publisher for debugging variables
+ros::Publisher m_debugPublisher;
+
+// ROS Publisher for inform the network about
+// changes to the setpoin
+ros::Publisher m_setpointChangedPublisher;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to
+// complile, but adding the function prototypes here means the the functions
+// can be written below in any order. If the function prototypes are not
+// included then the function need to written below in an order that ensure
+// each function is implemented before it is called from another function,
+// hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// 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);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
+
+// FOR LOADING THE YAML PARAMETERS
+void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg);
+void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/launch/agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
index 52a40b9441559c9293b0cf457e0ce058906a2990..50b230067eceb4430e1108399d2eeaf0204011c7 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -119,6 +119,15 @@
>
</node>
+ <!-- TEST MOTORS CONTROLLER -->
+ <node
+ pkg = "dfall_pkg"
+ name = "TestMotorsControllerService"
+ output = "screen"
+ type = "TestMotorsControllerService"
+ >
+ </node>
+
<!-- PARAMETER SERVICE -->
<node
pkg = "dfall_pkg"
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
index 64d77b13533f071c327fe852406f07169db9ad59..bb232502ca80158afeb3f4e4f93ce53e4a26a744 100644
--- a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -42,7 +42,7 @@ takoff_goto_setpoint_max_time: 4.0
takoff_integrator_on_box_horizontal: 0.25
takoff_integrator_on_box_vertical: 0.15
# The time for: take off integrator-on
-takoff_integrator_on_time: 2.0
+takoff_integrator_on_time: 4.0
# Height change for the landing move-down
@@ -99,16 +99,16 @@ gainMatrixThrust_2StateVector : [ 0.98, 0.25]
gainMatrixRollRate_3StateVector : [-6.20,-3.00, 5.20]
gainMatrixPitchRate_3StateVector : [ 6.20, 3.00, 5.20]
# The LQR Controller parameters for mode 2 (Angle-nested)
-gainMatrixRollAngle_2StateVector : [-0.20,-0.20]
-gainMatrixPitchAngle_2StateVector : [ 0.20, 0.20]
+gainMatrixRollAngle_2StateVector : [-0.80,-0.50]
+gainMatrixPitchAngle_2StateVector : [ 0.80, 0.50]
gainRollRate_fromAngle : 4.00
gainPitchRate_fromAngle : 4.00
# The LQR Controller parameters for yaw
gainYawRate_fromAngle : 2.30
# Integrator gains
-integratorGain_forThrust : 0.00
-integratorGain_forTauXY : 0.00
-integratorGain_forTauYaw : 0.00
+integratorGain_forThrust : 0.10
+integratorGain_forTauXY : 0.06
+integratorGain_forTauYaw : 0.05
diff --git a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
index 76f0d0b0724ea9425792bb53cf73671bdc932385..29ec896efe0bd6f6cc94b0bdf52ba950467aefa8 100755
--- a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
@@ -6,6 +6,8 @@ tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
templateController: "TemplateControllerService/TemplateController"
+testMotorsController: "TestMotorsControllerService/TestMotorsController"
+
# The name of the service advertised by the Default
# Controller for requests manoeuvres to be performed
defaultController_requestManoeuvre: "DefaultControllerService/RequestManoeuvre"
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index d19321f1cfb08ad5876d75dcb1393779a94f9b9b..f05ce8008c6307398f7fd1948887ab1c6558c465 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -538,12 +538,16 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
if (m_time_in_seconds > yaml_takoff_integrator_on_time)
{
// Inform the user
- ROS_INFO("[DEFAULT CONTROLLER] Switch to state: normal");
+ ROS_INFO("[DEFAULT CONTROLLER] Publish message that take-off is complete, and switch to state: normal");
// Reset the time variable
m_time_in_seconds = 0.0;
// Update the state accordingly
m_current_state = DEFAULT_CONTROLLER_STATE_NORMAL;
m_current_state_changed = true;
+ // Publish a message that the take-off is complete
+ IntWithHeader msg;
+ msg.data = DEFAULT_CONTROLLER_TAKEOFF_COMPLETE;
+ m_manoeuvreCompletePublisher.publish(msg);
}
}
@@ -664,12 +668,16 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
if ( m_time_in_seconds > (0.7*yaml_landing_spin_motors_time) )
{
// Inform the user
- ROS_INFO("[DEFAULT CONTROLLER] Switch to state: standby");
+ ROS_INFO("[DEFAULT CONTROLLER] Publish message that landing is complete, and switch to state: standby");
// Reset the time variable
m_time_in_seconds = 0.0;
// Update the state accordingly
m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
m_current_state_changed = true;
+ // Publish a message that the take-off is complete
+ IntWithHeader msg;
+ msg.data = DEFAULT_CONTROLLER_LANDING_COMPLETE;
+ m_manoeuvreCompletePublisher.publish(msg);
}
}
}
@@ -704,13 +712,6 @@ void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)
float max_for_z = yaml_max_setpoint_change_per_second_vertical / yaml_control_frequency;
// > Compute the current difference
float diff_for_z = target_setpoint[2] - current_setpoint[2];
- // > Clip the difference to the maximum
- if (diff_for_z > max_for_z)
- diff_for_z = max_for_z;
- else if (diff_for_z < -max_for_z)
- diff_for_z = -max_for_z;
- // > Update the current setpoint
- current_setpoint[2] += diff_for_z;
// SMOOTH THE X-Y-COORIDINATES
// > Compute the max allowed change
@@ -719,14 +720,19 @@ void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)
float diff_for_x = target_setpoint[0] - current_setpoint[0];
float diff_for_y = target_setpoint[1] - current_setpoint[1];
float diff_for_xy = sqrt( diff_for_x*diff_for_x + diff_for_y*diff_for_y );
- // > Clip the difference to the maximum
- if (diff_for_xy > max_for_xy)
+
+ // > Compute if outside the allowed ellipse
+ float ellipse_value = (diff_for_xy*diff_for_xy)/(max_for_xy*max_for_xy) + (diff_for_z*diff_for_z)/(max_for_z*max_for_z);
+
+ // > Clip the difference with outside the allowed ellispe
+ if (ellipse_value > 1.0f)
{
- // > Convert the difference to a proportion
- float proportion_xy = max_for_xy / diff_for_xy;
+ // > Compute the proportion
+ float proportion_xyz = 1.0f / sqrt(ellipse_value);
// > Update the current setpoint
- current_setpoint[0] += proportion_xy * diff_for_x;
- current_setpoint[1] += proportion_xy * diff_for_y;
+ current_setpoint[0] += proportion_xyz * diff_for_x;
+ current_setpoint[1] += proportion_xyz * diff_for_y;
+ current_setpoint[2] += proportion_xyz * diff_for_z;
}
else
{
@@ -735,7 +741,9 @@ void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)
// reach
current_setpoint[0] = target_setpoint[0];
current_setpoint[1] = target_setpoint[1];
- }
+ current_setpoint[2] = target_setpoint[2];
+ }
+
}
@@ -910,19 +918,19 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI
stateInertialError[6] = stateInertial[6];
stateInertialError[7] = stateInertial[7];
- // Clip the x-coordination to within the specified bounds
+ // Clip the x-error to within the specified bounds
if (stateInertialError[0] > yaml_max_setpoint_error_xy)
stateInertialError[0] = yaml_max_setpoint_error_xy;
else if (stateInertialError[0] < -yaml_max_setpoint_error_xy)
stateInertialError[0] = -yaml_max_setpoint_error_xy;
- // Clip the y-coordination to within the specified bounds
+ // Clip the y-error to within the specified bounds
if (stateInertialError[1] > yaml_max_setpoint_error_xy)
stateInertialError[1] = yaml_max_setpoint_error_xy;
else if (stateInertialError[1] < -yaml_max_setpoint_error_xy)
stateInertialError[1] = -yaml_max_setpoint_error_xy;
- // Clip the z-coordination to within the specified bounds
+ // Clip the z-error to within the specified bounds
if (stateInertialError[2] > yaml_max_setpoint_error_z)
stateInertialError[2] = yaml_max_setpoint_error_z;
else if (stateInertialError[2] < -yaml_max_setpoint_error_z)
@@ -1014,9 +1022,17 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
- yaml_gainMatrixPitchAngle_2StateVector[1] * stateErrorBody[3];
// Clip the request to within the specified limits
if (pitchAngle_desired > yaml_max_roll_pitch_request_radians)
+ {
pitchAngle_desired = yaml_max_roll_pitch_request_radians;
+ //ROS_ERROR_STREAM("[DEFAULT CONTROLLER] pitchAngle_desired = " << pitchAngle_desired);
+ }
else if (pitchAngle_desired < -yaml_max_roll_pitch_request_radians)
- pitchAngle_desired = -yaml_max_roll_pitch_request_radians;
+ {
+ pitchAngle_desired = -yaml_max_roll_pitch_request_radians;
+ //ROS_ERROR_STREAM("[DEFAULT CONTROLLER] pitchAngle_desired = " << pitchAngle_desired);
+ }
+
+
// > Compute the pitch rate
pitchRate_forResponse =
- yaml_gainPitchRate_fromAngle * (stateErrorBody[7] - pitchAngle_desired);
@@ -1294,11 +1310,22 @@ void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
// CHANGE SETPOINT FUNCTION
void setNewSetpoint(float x, float y, float z, float yaw)
{
- // Put the new setpoint into the class variable
- m_setpoint[0] = x;
- m_setpoint[1] = y;
- m_setpoint[2] = z;
- m_setpoint[3] = yaw;
+ if (!m_isAvailableContext)
+ {
+ ROS_ERROR("[DEFAULT CONTROLLER] New setpoint requested, however there is no context available, setting default setpoint instead.");
+ m_setpoint[0] = yaml_default_setpoint[0];
+ m_setpoint[1] = yaml_default_setpoint[1];
+ m_setpoint[2] = yaml_default_setpoint[2];
+ m_setpoint[3] = yaml_default_setpoint[3];
+ }
+ else
+ {
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = clipToBounds( x , -m_symmetric_area_bounds_x , m_symmetric_area_bounds_x );
+ m_setpoint[1] = clipToBounds( y , -m_symmetric_area_bounds_y , m_symmetric_area_bounds_y );;
+ m_setpoint[2] = clipToBounds( z , -m_symmetric_area_bounds_z , m_symmetric_area_bounds_z );;
+ m_setpoint[3] = yaw;
+ }
// Publish the change so that the network is updated
// (mainly the "flying agent GUI" is interested in
@@ -1307,10 +1334,10 @@ void setNewSetpoint(float x, float y, float z, float yaw)
// Instantiate a local variable of type "SetpointWithHeader"
SetpointWithHeader msg;
// Fill in the setpoint
- msg.x = x;
- msg.y = y;
- msg.z = z;
- msg.yaw = yaw;
+ msg.x = m_setpoint[0];
+ msg.y = m_setpoint[1];
+ msg.z = m_setpoint[2];
+ msg.yaw = m_setpoint[3];
// Publish the message
m_setpointChangedPublisher.publish(msg);
}
@@ -1347,6 +1374,16 @@ void publishCurrentSetpointAndState()
m_setpointChangedPublisher.publish(msg);
}
+float clipToBounds(float val, float val_min, float val_max)
+{
+ float return_val = val;
+ if (return_val < val_min)
+ return_val = val_min;
+ if (return_val > val_max)
+ return_val = val_max;
+
+ return return_val;
+}
@@ -1448,6 +1485,79 @@ void publish_motors_off_to_flying_agent_client()
+// ----------------------------------------------------------------------------------
+// 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
+//
+// CCCC OOO N N TTTTT EEEEE X X TTTTT
+// C O O NN N T E X X T
+// C O O N N N T EEE X T
+// C O O N NN T E X X T
+// CCCC OOO N N T EEEEE X X T
+// ----------------------------------------------------------------------------------
+
+
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
+{
+ ROS_INFO("[DEFAULT CONTROLLER] Received message that the Context Database Changed");
+ loadCrazyflieContext();
+}
+
+
+
+void loadCrazyflieContext()
+{
+ CMQuery contextCall;
+ contextCall.request.studentID = m_agentID;
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] AgentID:" << m_agentID);
+
+ CrazyflieContext new_context;
+
+ m_centralManager.waitForExistence(ros::Duration(-1));
+
+ if(m_centralManager.call(contextCall)) {
+ new_context = contextCall.response.crazyflieContext;
+ //ROS_INFO_STREAM("[DEFAULT CONTROLLER] CrazyflieContext:\n" << new_context);
+
+ //if((m_context.crazyflieName != "") && (new_context.crazyflieName != m_context.crazyflieName)) //linked crazyflie name changed and it was not empty before
+ //{
+
+ // Motors off is done in python script now everytime we disconnect
+
+ // send motors OFF and disconnect before setting m_context = new_context
+ // std_msgs::Int32 msg;
+ // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ // commandPublisher.publish(msg);
+
+ // ROS_INFO("[DEFAULT CONTROLLER] CF is now different for this student. Disconnect and turn it off");
+
+ // IntWithHeader msg;
+ // msg.shouldCheckForAgentID = false;
+ // msg.data = CMD_DISCONNECT;
+ // m_crazyRadioCommandPublisher.publish(msg);
+ //}
+
+ m_symmetric_area_bounds_x = 0.5 * (new_context.localArea.xmax - new_context.localArea.xmin);
+ m_symmetric_area_bounds_y = 0.5 * (new_context.localArea.ymax - new_context.localArea.ymin);
+ m_symmetric_area_bounds_z = 0.5 * (new_context.localArea.zmax - new_context.localArea.zmin);
+ m_isAvailableContext = true;
+
+ }
+ else
+ {
+ m_isAvailableContext = false;
+ ROS_ERROR("[DEFAULT CONTROLLER] Failed to load context. Waiting for next Save in DB by teacher");
+ }
+}
+
+
+
+
+
+
// ----------------------------------------------------------------------------------
// L OOO A DDDD
@@ -1908,6 +2018,15 @@ int main(int argc, char* argv[])
// will take to perform (in milliseconds)
ros::ServiceServer requestManoeuvreService = nodeHandle.advertiseService("RequestManoeuvre", requestManoeuvreCallback);
+ // Instantiate the class variable "m_manoeuvreCompletePublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "ManoeuvreComplete" and is a message with the structure defined
+ // in the file "IntWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // flying state once the manoeuvre is complete.
+ m_manoeuvreCompletePublisher = nodeHandle.advertise<IntWithHeader>("ManoeuvreComplete", 1);
+
+
// Instantiate the class variable "m_motorsOffToFlyingAgentClientPublisher"
// to be a "ros::Publisher". This variable advertises under the
// name space:
@@ -1923,6 +2042,22 @@ int main(int argc, char* argv[])
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+ // LOADING OF THIS AGENT'S CONTEXT
+ // Service cleint for loading the allocated flying
+ // zone and other context details
+ //ros::service::waitForService("/CentralManagerService/CentralManager");
+ m_centralManager = nodeHandle_dfall_root.serviceClient<CMQuery>("CentralManagerService/Query", false);
+ // Call the class function that uses this service
+ // client to load the context
+ loadCrazyflieContext();
+ // Subscriber for when the Flying Zone Database changed
+ ros::Subscriber databaseChangedSubscriber = nodeHandle_dfall_root.subscribe("CentralManagerService/DBChanged", 1, crazyflieContextDatabaseChangedCallback);
+
+
+
// Print out some information to the user.
ROS_INFO("[DEFAULT CONTROLLER] Service ready :-)");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 14c80a6a76b2ecf030101b6f77653eb06d3aadee..210946f021a8df09d9eac05a21d7bcef591b4edc 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -116,6 +116,43 @@ void viconCallback(const ViconData& viconData)
)
{
+ // If motors-off and testing motors
+ if (
+ m_flying_state == STATE_MOTORS_OFF
+ &&
+ m_controller_nominally_selected == TESTMOTORS_CONTROLLER
+ &&
+ m_testMotorsController.exists()
+ )
+ {
+ // Initliase the "Contrller" service call variable
+ Controller testMotorsCall;
+
+ // Initialise a local boolean success variable
+ bool isSuccessful_testMotorsCall = false;
+
+ // Call the controller service client
+ isSuccessful_testMotorsCall = m_testMotorsController.call(testMotorsCall);
+
+ // Ensure success and enforce safety
+ if(isSuccessful_testMotorsCall)
+ {
+ m_commandForSendingToCrazyfliePublisher.publish(testMotorsCall.response.controlOutput);
+ }
+ else
+ {
+ // Let the user know that the controller call failed
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call test motors controller, valid: " << m_testMotorsController.isValid() << ", exists: " << m_testMotorsController.exists());
+ // Change back to the default controller
+ setControllerNominallySelected(DEFAULT_CONTROLLER);
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ }
+ return;
+ }
+
+
+
// Only continue if:
// (1) the agent is NOT occulded
if(!poseDataForThisAgent.occluded)
@@ -354,13 +391,15 @@ void sendZeroOutputCommandForMotors()
bool safetyCheck_on_positionAndTilt(CrazyflieData data)
{
// Check on the X position
- if((data.x < m_context.localArea.xmin) or (data.x > m_context.localArea.xmax))
+ float symmetric_bound_x = 0.5 * (m_context.localArea.xmax-m_context.localArea.xmin);
+ if((data.x < -symmetric_bound_x) or (data.x > symmetric_bound_x))
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
return false;
}
// Check on the Y position
- if((data.y < m_context.localArea.ymin) or (data.y > m_context.localArea.ymax))
+ float symmetric_bound_y = 0.5 * (m_context.localArea.ymax-m_context.localArea.ymin);
+ if((data.y < -symmetric_bound_y) or (data.y > symmetric_bound_y))
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] y safety failed");
return false;
@@ -626,8 +665,8 @@ void requestChangeFlyingStateToLand()
void timerCallback_takeoff_complete(const ros::TimerEvent&)
{
- // Only change to flying if still in the take-off state
- if (m_flying_state == STATE_TAKE_OFF)
+ // Only change to flying if still in the take-off state
+ if (m_flying_state == STATE_TAKE_OFF)
{
// Inform the user
ROS_INFO("[FLYING AGENT CLIENT] Take-off complete, changed state to STATE_FLYING");
@@ -649,8 +688,8 @@ void timerCallback_takeoff_complete(const ros::TimerEvent&)
void timerCallback_land_complete(const ros::TimerEvent&)
{
- // Only change to flying if still in the take-off state
- if (m_flying_state == STATE_LAND)
+ // Only change to flying if still in the take-off state
+ if (m_flying_state == STATE_LAND)
{
// Inform the user
ROS_INFO("[FLYING AGENT CLIENT] Land complete, changed state to STATE_MOTORS_OFF");
@@ -673,6 +712,67 @@ void timerCallback_land_complete(const ros::TimerEvent&)
+void defaultControllerManoeuvreCompleteCallback(const IntWithHeader & msg)
+{
+ // Switch between the cases
+ switch (msg.data)
+ {
+ case DEFAULT_CONTROLLER_TAKEOFF_COMPLETE:
+ {
+ // Only change to flying if still in the take-off state
+ if (m_flying_state == STATE_TAKE_OFF)
+ {
+ // Stop the timer
+ m_timer_takeoff_complete.stop();
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Take-off complete, changed state to STATE_FLYING");
+ // Update the class variable
+ m_flying_state = STATE_FLYING;
+ // Publish a message with the new flying state
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
+ // Change back to the nominal controller
+ setInstantController( m_controller_nominally_selected );
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Received a take-off complete message, BUT the agent is no longer in STATE_TAKE_OFF.");
+ }
+ break;
+ }
+
+
+ case DEFAULT_CONTROLLER_LANDING_COMPLETE:
+ {
+ // Only change to flying if still in the take-off state
+ if (m_flying_state == STATE_LAND)
+ {
+ // Stop the timer
+ m_timer_land_complete.stop();
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Land complete, changed state to STATE_MOTORS_OFF");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ // Publish a message with the new flying state
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
+ // Change back to the nominal controller
+ setInstantController( m_controller_nominally_selected );
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Received a landing complete message, BUT the agent is no longer in STATE_LAND.");
+ }
+ break;
+ }
+ }
+}
+
+
@@ -726,6 +826,9 @@ void setInstantController(int controller)
case TEMPLATE_CONTROLLER:
m_instant_controller_service_client = &m_templateController;
break;
+ case TESTMOTORS_CONTROLLER:
+ m_instant_controller_service_client = &m_defaultController;
+ break;
default:
break;
}
@@ -863,6 +966,7 @@ void flyingStateRequestCallback(const IntWithHeader & msg) {
setControllerNominallySelected(TEMPLATE_CONTROLLER);
break;
+
case CMD_CRAZYFLY_TAKE_OFF:
ROS_INFO("[FLYING AGENT CLIENT] TAKE_OFF Command received");
requestChangeFlyingStateTo(STATE_TAKE_OFF);
@@ -877,6 +981,20 @@ void flyingStateRequestCallback(const IntWithHeader & msg) {
requestChangeFlyingStateTo(STATE_MOTORS_OFF);
break;
+
+ case CMD_USE_TESTMOTORS_CONTROLLER:
+ if (m_flying_state == STATE_MOTORS_OFF)
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] USE_TEST_MOTORS_CONTROLLER Command received");
+ setControllerNominallySelected(TESTMOTORS_CONTROLLER);
+ }
+ else
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] USE_TEST_MOTORS_CONTROLLER Command received, but state is not currently STATE_MOTORS_OFF");
+ }
+ break;
+
+
default:
ROS_ERROR_STREAM("[FLYING AGENT CLIENT] unexpected command number: " << cmd);
break;
@@ -1141,6 +1259,8 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
createControllerServiceClientFromParameterName( "pickerController" , m_pickerController );
createControllerServiceClientFromParameterName( "templateController" , m_templateController );
+ createControllerServiceClientFromParameterName( "testMotorsController" , m_testMotorsController );
+
// INITIALISE THE SERVICE FOR REQUESTING THE DEFAULT
// CONTROLLER TO PERFORM MANOEUVRES
createIntIntServiceClientFromParameterName( "defaultController_requestManoeuvre" , m_defaultController_requestManoeuvre );
@@ -1507,6 +1627,15 @@ int main(int argc, char* argv[])
ros::Subscriber CFBatterySubscriber = nodeHandle_to_battery_monitor.subscribe("ChangedStateTo", 1, batteryMonitorStateChangedCallback);
+ // SUBSCRIBER FOR BATTERY STATE CHANGES
+ // The battery state change message from the Battery
+ // Monitor node
+ std::string namespace_to_default_contoller = m_namespace + "/DefaultControllerService";
+ ros::NodeHandle nodeHandle_to_default_controller(namespace_to_default_contoller);
+ ros::Subscriber ManoeuvreCompleteSubscriber = nodeHandle_to_default_controller.subscribe("ManoeuvreComplete", 1, defaultControllerManoeuvreCompleteCallback);
+
+
+
// SERVICE SERVER FOR OTHERS TO GET THE CURRENT FLYING STATE
// Advertise the service that return the "m_flying_state"
// variable when called upon
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TestMotorsControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TestMotorsControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9dafb62fcc64fa75f58917cc8ef22c9c25556519
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TestMotorsControllerService.cpp
@@ -0,0 +1,847 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// A controller to test the motors
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/TestMotorsControllerService.h"
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// OOO U U TTTTT EEEEE RRRR
+// O O U U T E R R
+// O O U U T EEE RRRR
+// O O U U T E R R
+// OOO UUU T EEEEE R R
+//
+// 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
+// ----------------------------------------------------------------------------------
+
+// This function is the callback that is linked to the "TemplateController"
+// service that is advertised in the main function. This must have arguments
+// that match the "input-output" behaviour defined in the "Controller.srv"
+// file (located in the "srv" folder)
+//
+// The arument "request" is a structure provided to this service with the
+// following two properties:
+//
+// >> request.ownCrazyflie
+// This property is itself a structure of type "CrazyflieData", which is
+// defined in the file "CrazyflieData.msg", and has the following properties
+// string crazyflieName
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// The values in these properties are directly the measurement taken by the
+// motion capture system of the Crazyflie that is to be controlled by this
+// service.
+//
+// >> request.otherCrazyflies
+// This property is an array of "CrazyflieData" structures, what allows access
+// to the motion capture measurements of other Crazyflies.
+//
+// The argument "response" is a structure that is expected to be filled in by
+// this service by this function, it has only the following property
+//
+// >> response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is
+// defined in the file "ControlCommand.msg", and has the following properties:
+// float32 roll The command sent to the Crazyflie for the body frame x-axis
+// float32 pitch The command sent to the Crazyflie for the body frame y-axis
+// float32 yaw The command sent to the Crazyflie for the body frame z-axis
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
+// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
+// > The allowed values for "onboardControllerType" are in the "Defines"
+// section in the header file, they are:
+// - CF_COMMAND_TYPE_MOTORS
+// - CF_COMMAND_TYPE_RATE
+// - CF_COMMAND_TYPE_ANGLE
+//
+// > For most common option to use is CF_COMMAND_TYPE_RATE option.
+//
+// > For the CF_COMMAND_TYPE_RATE optoin:
+// 1) the ".roll", ".ptich", and ".yaw" properties of
+// "response.ControlCommand" specify the angular rate in
+// [radians/second] that will be requested from the PID controllers
+// running in the Crazyflie 2.0 firmware.
+// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// "response.ControlCommand" are the baseline motor commands
+// requested from the Crazyflie, with the adjustment for body rates
+// being added on top of this in the firmware (i.e., as per the
+// code of the "distribute_power" found in the firmware).
+// 3) the axis convention for the roll, pitch, and yaw body rates
+// returned in "response.ControlCommand" should use right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the following is an ASCII art of this convention.
+//
+// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
+//
+// > This is a top view,
+// > M1 to M4 stand for Motor 1 to Motor 4,
+// > "CW" indicates that the motor rotates Clockwise,
+// > "CCW" indicates that the motor rotates Counter-Clockwise,
+// > By right-hand axis convention, the positive z-direction points out
+// of the screen,
+// > This being a "top view" means that the direction of positive thrust
+// produced by the propellers is also out of the screen.
+//
+// ____ ____
+// / \ / \
+// (CW) | M4 | x | M1 | (CCW)
+// \____/\ ^ /\____/
+// \ \ | / /
+// \ \ | / /
+// \ \______ | ______/ /
+// \ | /
+// | | |
+// y <-------------o |
+// | |
+// / _______________ \
+// / / \ \
+// / / \ \
+// ____/ / \ \____
+// / \/ \/ \
+// (CCW) | M3 | | M2 | (CW)
+// \____/ \____/
+//
+//
+//
+//
+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
+
+ static int ticks = 0;
+
+
+ ticks++;
+
+
+ response.controlOutput.roll = 0.0f;
+ response.controlOutput.pitch = 0.0f;
+ response.controlOutput.yaw = 0.0f;
+
+ response.controlOutput.motorCmd1 = 0;
+ response.controlOutput.motorCmd2 = 0;
+ response.controlOutput.motorCmd3 = 0;
+ response.controlOutput.motorCmd4 = 0;
+
+
+ if (ticks < 300)
+ response.controlOutput.motorCmd1 = 9000;
+ else if (ticks < 600)
+ response.controlOutput.motorCmd2 = 9000;
+ else if (ticks < 900)
+ response.controlOutput.motorCmd3 = 9000;
+ else if (ticks < 1200)
+ response.controlOutput.motorCmd4 = 9000;
+ else
+ {
+ response.controlOutput.motorCmd4 = 9000;
+ ticks = 0;
+ }
+
+
+ // PREPARE AND RETURN THE VARIABLE "response"
+
+ // Choose the controller type use on-board the Crazyflie,
+ // it can be either be Motor, Rate, or Angle based
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+ // Return "true" to indicate that the control computation was
+ // performed successfully
+ return true;
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
+// R R O O T A A T E I NN N T O O
+// RRRR O O T A A T EEE I N N N T O O
+// R R O O T AAAAA T E I N NN T O O
+// R R OOO T A A T EEEEE III N N T OOO
+//
+// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
+// B B O O D D Y Y F R R A A MM MM E
+// BBBB O O D D Y FFF RRRR A A M M M EEE
+// B B O O D D Y F R R AAAAA M M E
+// BBBB OOO DDDD Y F R R A A M M EEEEE
+// ----------------------------------------------------------------------------------
+
+// The arguments for this function are as follows:
+// stateInertial
+// This is an array of length 9 with the estimates the error of of the following values
+// relative to the sepcifed setpoint:
+// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
+// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
+// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
+//
+// stateBody
+// This is an empty array of length 9, this function should fill in all elements of this
+// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
+// position and (x,y) velocities are rotated into the body frame.
+//
+// yaw_measured
+// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
+// the Vicon motion capture system
+//
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
+{
+ 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];
+}
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ----------------------------------------------------------------------------------
+
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-but command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd_16bit < yaml_command_sixteenbit_min)
+ {
+ cmd_16bit = 0;
+ }
+ else if (cmd_16bit > yaml_command_sixteenbit_max)
+ {
+ cmd_16bit = yaml_command_sixteenbit_max;
+ }
+ // Return the result
+ return cmd_16bit;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Check if the request if for the default setpoint
+ if (newSetpoint.buttonID == REQUEST_DEFAULT_SETPOINT_BUTTON_ID)
+ {
+ setNewSetpoint(
+ yaml_default_setpoint[0],
+ yaml_default_setpoint[1],
+ yaml_default_setpoint[2],
+ yaml_default_setpoint[3]
+ );
+ }
+ else
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw
+ );
+ }
+ }
+}
+
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw)
+{
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = x;
+ m_setpoint[1] = y;
+ m_setpoint[2] = z;
+ m_setpoint[3] = yaw;
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
+{
+ // Directly put the current setpoint into the response
+ response.setpointWithHeader.x = m_setpoint[0];
+ response.setpointWithHeader.y = m_setpoint[1];
+ response.setpointWithHeader.z = m_setpoint[2];
+ response.setpointWithHeader.yaw = m_setpoint[3];
+ // Return
+ return true;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC U U SSSS TTTTT OOO M M
+// C U U S T O O MM MM
+// C U U SSS T O O M M M
+// C U U S T O O M M
+// CCCC UUU SSSS T OOO M M
+//
+// CCCC OOO M M M M A N N DDDD
+// C O O MM MM MM MM A A NN N D D
+// C O O M M M M M M A A N N N D D
+// C O O M M M M AAAAA N NN D D
+// CCCC OOO M M M M A A N N DDDD
+// ----------------------------------------------------------------------------------
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , commandReceived.shouldCheckForAgentID , commandReceived.agentIDs );
+
+ if (isRevelant)
+ {
+ // Extract the data from the message
+ int custom_button_index = commandReceived.button_index;
+ float float_data = commandReceived.float_data;
+ //std::string string_data = commandReceived.string_data;
+
+ // Switch between the button pressed
+ switch(custom_button_index)
+ {
+
+ // > FOR CUSTOM BUTTON 1
+ case 1:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
+ // Code here to respond to custom button 1
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 2
+ case 2:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
+ // Code here to respond to custom button 2
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 3
+ case 3:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 3 received in controller, with message.float_data = " << float_data );
+ // Code here to respond to custom button 3
+
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.float_data = " << float_data );
+ break;
+ }
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// CALLBACK NOTIFYING THAT THE YAML PARAMETERS ARE READY TO BE LOADED
+void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[TEMPLATE CONTROLLER] Now fetching the TemplateController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[TEMPLATE CONTROLLER] Now fetching the TemplateController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[TEMPLATE CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchTemplateControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+// LOADING OF THE YAML PARAMTERS
+void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // TemplateController.yaml
+
+ // Add the "TemplateController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "TemplateController");
+
+
+
+ // GET THE PARAMETERS:
+
+ // > The mass of the crazyflie
+ yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
+
+ // > The frequency at which the "computeControlOutput" is being called,
+ // as determined by the frequency at which the motion capture system
+ // provides position and attitude data
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor
+ // command to thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+ // > The min and max for saturating 16 bit thrust commands
+ yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ yaml_command_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
+
+ // The default setpoint, the ordering is (x,y,z,yaw),
+ // with unit [meters,meters,meters,radians]
+ getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+
+ // Boolean indiciating whether the "Debug Message" of this agent
+ // should be published or not
+ yaml_shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should
+ // be displayed or not
+ yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
+
+ // The LQR Controller parameters
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+
+ // > DEBUGGING: Print out one of the parameters that was loaded to
+ // debug if the fetching of parameters worked correctly
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: the fetched TemplateController/mass = " << yaml_cf_mass_in_grams);
+
+
+
+ // PROCESS THE PARAMTERS
+
+ // > Compute the feed-forward force that we need to counteract
+ // gravity (i.e., mg) in units of [Newtons]
+ m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// This function does NOT need to be edited
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "TestMotorsControllerService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle"
+ // as the current node, the "~" indcates that "self" is the
+ // node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this "TestMotorsControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TEST MOTORS CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[TEST MOTORS CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[TEST MOTORS CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[TEST MOTORS CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[TEST MOTORS CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "TemplateController", 1, isReadyTemplateControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("TemplateController", 1, isReadyTemplateControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "TemplateController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyTemplateControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[TEST MOTORS CONTROLLER] The request load yaml file service call failed.");
+ }
+
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the setpoint is changed
+ // by the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TemplateControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Instantiate the class variable "m_setpointChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current setpoint for this controller.
+ m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+
+ // Instantiate the local variable "getCurrentSetpointService" to be
+ // a "ros::ServiceServer" type variable that advertises the service
+ // called "GetCurrentSetpoint". This service has the input-output
+ // behaviour defined in the "GetSetpointService.srv" file (located
+ // in the "srv" folder). This service, when called, is provided with
+ // an integer (that is essentially ignored), and is expected to respond
+ // with the current setpoint of the controller. When a request is made
+ // of this service the "getCurrentSetpointCallback" function is called.
+ ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
+
+
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "TemplateController". This service has
+ // the input-output behaviour defined in the "Controller.srv" file (located in the
+ // "srv" folder). This service, when called, is provided with the most recent
+ // measurement of the Crazyflie and is expected to respond with the control action
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("TestMotorsController", calculateControlOutput);
+
+ // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "GUIButton" topic and calls the class
+ // function "customCommandReceivedCallback" each time a messaged is received on this topic
+ // and the message received is passed as an input argument to the callback function.
+ ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("CustomButtonPressed", 1, customCommandReceivedCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ //std::string namespace_to_coordinator;
+ //constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ //ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TemplateControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
+
+
+
+ // Print out some information to the user.
+ ROS_INFO("[TEST MOTORS CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}
diff --git a/install/dfall_install.sh b/install/dfall_install.sh
index daba24eed8d7c9b2b99580b28c65938f02bafb5a..eac9dc54c2ba43bb30844c69465222a524c68b24 100755
--- a/install/dfall_install.sh
+++ b/install/dfall_install.sh
@@ -5,44 +5,65 @@ die () {
exit 1
}
-[ "$#" -eq 1 ] || die "1 argument required (StudentID), $# provided"
+# Check the input argument is supplied and correct
+[ "$#" -eq 1 ] || die "1 argument required (AgentID), $# provided"
echo $1 | grep -E -q '^[0-9]+$' || die "Numeric argument required, $1 provided"
-#ros repository
+# Ensure the ROS repository is available
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
-#system update and installation
+# Update the list of programs
sudo apt-get update
+# Install all available upgrades
+# > Note: the -y option means Automatic yes to prompts
sudo apt-get -y upgrade
-sudo apt-get -y install ros-kinetic-desktop-full python-pip
+# Install ROS Kinetic
+sudo apt-get -y install ros-kinetic-desktop-full
+# Install the python package management system "python-pip"
+sudo apt-get -y install python-pip
+# Install the python USB package "pyusb"
+# > Note: this is needed to connected to the Crazyradio USB dongle
sudo pip install pyusb
-#rosdep
+# Initialise and update the ROS dependencies
sudo rosdep init
rosdep update
-#untar catkin workspace
-#needs to run after ros installation because of symbolic link to CMakeLists.txt
-mkdir -p ~/dfall_ws/src
-tar -xf package.tar.gz -C ~/dfall_ws/src
+# Make the "work" directory under the users root
+# > Note: the -p option means: no error if existing, make parent directories as needed
+mkdir -p ~/work
+# Change directory to this folder
+cd ~/work
+# Clone the D-FaLL-System git repository
+git clone https://gitlab.ethz.ch/D-FaLL/PandS-System/D-FaLL-System.git
-#environment setup
+# Add the ROS environment setup to the .bashrc
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
+# Run it now so that it is valid for this terminal session
source /opt/ros/kinetic/setup.bash
-sudo sh -c "echo '10.42.0.32 teacher' >> /etc/hosts"
-sudo sh -c "echo $1 >> /etc/StudentID"
+# Add the teacher's IP address to the /etc/hosts file
+sudo sh -c "echo '10.42.0.10 teacher' >> /etc/hosts"
-#copy rules before cd
-sudo cp ./99-crazyflie.rules /etc/udev/rules.d
-sudo cp ./99-crazyradio.rules /etc/udev/rules.d
+# Add a new file with the default agent ID
+sudo sh -c "echo $1 >> /etc/dfall_default_agent_id"
+# Add a new file with the default coordinator ID
+sudo sh -c "echo 1 >> /etc/dfall_default_coord_id"
-#build workspace
-cd ~/dfall_ws
+# Copy rules necessary for using the Crazyradio
+sudo cp ~/work/D-FaLL-System/install/99-crazyflie.rules /etc/udev/rules.d
+sudo cp ~/work/D-FaLL-System/install/99-crazyradio.rules /etc/udev/rules.d
+
+# Build the D-FaLL ROS Package, called dfall_pkg
+# > This is done by calling "catkin_make" from the dfall_ws workspace
+cd ~/work/D-FaLL-System/dfall_ws
catkin_make -j4
-echo "source ~/dfall_ws/devel/setup.bash" >> ~/.bashrc
-source ~/dfall_ws/devel/setup.bash
-echo "source ~/dfall_ws/src/dfall_pkg/launch/Config.sh" >> ~/.bashrc
-source ~/dfall_ws/src/dfall_pkg/launch/Config.sh
+# Add the D-FaLL ROS Package setup to the .bashrc
+echo "source ~/work/D-FaLL-System/dfall_ws/devel/setup.bash" >> ~/.bashrc
+source ~/work/D-FaLL-System/dfall_ws/devel/setup.bash
+
+# Add the "Config.sh" shell script to the .bashrc
+echo "source ~/work/D-FaLL-System/dfall_ws/src/dfall_pkg/launch/Config.sh" >> ~/.bashrc
+source ~/work/D-FaLL-System/dfall_ws/src/dfall_pkg/launch/Config.sh
diff --git a/wiki/installation.md b/wiki/installation.md
index a97bfeebb14a6efef71a26f5982a3bf913380c67..bc1d480591032d88e8538306419eda6a364505dd 100644
--- a/wiki/installation.md
+++ b/wiki/installation.md
@@ -3,16 +3,14 @@
## For Student and Teacher
### Install Script
-Installation with the install script is the easiest. You will need:
-- the install script
-- the ``dfall_pkg`` package compressed in a file called ``package.tar.gz``
-- the rule files for the USB connection to the crazyradio, called ``99-crazyflie.rules`` and ``99-crazyradio.rules``
+Installation with the install script is the easiest. You will need only the install script from this repository, located in the ``install`` folder and named ``dfall_install.sh``
-These files all need to be in the same directory. To run the installation, move to the containing directory (pps\ install) and call it with
+
+To run the installation, using terminal change directory to the folder containing ``dfall_install.sh`` and call it with
```
-./pps_install.sh <student id>
+./dfall_install.sh <agent id>
```
-The student id needs to be a unique number that is used as identication for the student laptops connected to the teacher. Make sure not that the script file is marked executable and to not run the script as root, as it will ask for the password and only execute some commands with root privilege.
+The ``<agent id>`` needs to be a unique number that is used as identication for the agent's laptop. Make sure not that the script file is marked executable and do NOT run the script as root, i.e., do NOT run the script using ``sudo``. The installation script will ask for the password and only execute commands with root privilege when required.
### Manual Installation
The installation process consists of the following steps:
@@ -20,15 +18,36 @@ The installation process consists of the following steps:
- Installation of ROS: <br />
The detailed instructions for the installation of ROS can be found [here](http://wiki.ros.org/kinetic/Installation/Ubuntu).
-- Workspace: <br />
-Create a new catkin workspace and copy the ``dfall_pkg`` package into the ``src`` folder of the workspace. Then build the package with ``catkin_make`` called from the workspace root.
-- Environment Setup: <br />
-Add a new line in the ``/etc/hosts`` file that links the teacher's IP with the domain name ``teacher`` and create a file called ``/etc/StudentID`` that contains the student id. Only write digits without any other symbols or whitespace characters.
+- Clone this repository: <br />
+Clone this repository into the desired location on your computer, we use the location ``~/work``:
+
+```
+mkdir -p ~/work
+```
+
+```
+cd ~/work
+```
+
+```
+git clone https://gitlab.ethz.ch/D-FaLL/PandS-System/D-FaLL-System.git
+```
+
+
+- USB Crazyradio and Python USB package: <br />
+To set up the crazyradio USB dongle just copy the rule files ``99-crazyflie.rules`` and ``99-crazyradio.rules`` from directory ``install`` to the directory ``/etc/udev/rules.d``:
+
+```
+sudo cp ~/work/D-FaLL-System/install/99-crazyflie.rules /etc/udev/rules.d
+```
+
+```
+sudo cp ~/work/D-FaLL-System/install/99-crazyradio.rules /etc/udev/rules.d
+```
+
-- USB Crazyradio: <br />
-To set up the crazyradio USB dongle just copy the rule files ``99-crazyflie.rules`` and ``99-crazyradio.rules`` from directory ``pps\ install/`` to the directory ``/etc/udev/rules.d``.
-You also have to install the library pyusb:
+You also have to install the library ``pyusb``:
```
sudo apt-get update
@@ -41,6 +60,35 @@ sudo apt-get install python-pip
```
sudo pip install pyusb
```
+The second command installs the python package management system ``python-pip``. The third command installs the python USB package ``pyusb``
+
+
+- Environment Setup: <br />
+Add a new line in the ``/etc/hosts`` file that links the teacher's IP with the domain name ``teacher``, create a file called ``/etc/dfall_default_agent_id`` that contains the agent id, and create a file called ``/etc/dfall_default_coord_id`` that contains the coordinator id.
+```
+sudo sh -c "echo '10.42.0.10 teacher' >> /etc/hosts"
+```
+
+```
+sudo sh -c "echo $1 >> /etc/dfall_default_agent_id"
+```
+
+```
+sudo sh -c "echo 1 >> /etc/dfall_default_coord_id"
+```
+
+
+- Build the D-FaLL ROS Package: <br />
+To do this you need to first change direction to the ``dfall_ws`` directory, where ``ws`` stands for workspace:
+```
+cd ~/work/D-FaLL-System/dfall_ws
+```
+
+And then you build the D-FaLL ROS Package using the ``catkin_make`` command:
+```
+catkin_make -j4
+```
+
- Source scripts in ``.bashrc``: <br />
Add following lines to the bottom of the file ``~/.bashrc`` (replace ``<catkin workspace>`` with correct directory)
@@ -50,9 +98,10 @@ source <catkin workspace>/devel/setup.bash
source <catkin workspace>/src/dfall_pkg/launch/Config.sh
```
-The workspace setup script will only appear after the first compilation of the catkin workspace.
+If you follow the steps above, then the ``<catkin workspace>`` should be ``~/work/D-FaLL-System/dfall_ws``. Note that the workspace setup script will only appear after the first compilation of the catkin workspace.
+
+If you are not sure at any point you can check out the ``dfall_install.sh`` script.
-If you are not sure at any point you can check out the install script.
### Vicon Datastream SDK Installation
diff --git a/wiki/latex/dfall_system_getting_started_cheat_sheet.tex b/wiki/latex/dfall_system_getting_started_cheat_sheet.tex
index d07866da48745d6b1db6c646caa7010f9c98d63a..b2d1b79e64cd8932a59daca2c95ca82229e93880 100644
--- a/wiki/latex/dfall_system_getting_started_cheat_sheet.tex
+++ b/wiki/latex/dfall_system_getting_started_cheat_sheet.tex
@@ -35,17 +35,22 @@
% ----------------------------------------------------- %
-% THINGS THAT MAY CHANGES AS THE SYSTEM IS DEVELOPER
-\newcommand{\roslaunchcommand}{roslaunch d\_fall\_pps Agent.launch}
+% THINGS THAT MAY CHANGES AS THE SYSTEM IS DEVELOPED
+\newcommand{\gitwebaddress}{gitlab.ethz.ch/D-FaLL/PandS-System/D-FaLL-System/}
+\newcommand{\repositoryname}{D-FaLL-System}
+\newcommand{\roslaunchcommand}{roslaunch dfall\_pkg agent.launch}
+\newcommand{\rospackagename}{dfall\_pkg}
+\newcommand{\launchfilename}{agent.launch}
\newcommand{\repositoryrootpath}{\textasciitilde/work/D-FaLL-System/}
-\newcommand{\catkinmakefullpath}{\textasciitilde/work/D-FaLL-System/pps\_ws}
+\newcommand{\catkinmakefullpath}{\textasciitilde/work/D-FaLL-System/dfall\_ws}
+\newcommand{\workspacefoldername}{dfall\_ws}
% ----------------------------------------------------- %
% BEGIN THE DOCUMENT
\begin{document}
\begin{center}
- \huge{\textsc{D-FaLL System Cheat Sheet}}
+ \huge{\textsc{D-FaLL-System Cheat Sheet}}
\end{center}
\noindent
@@ -53,12 +58,12 @@ A few useful hints, commands, web addresses, and file paths:
\begin{enumerate}[topsep=-1pt , itemsep=1pt , label = \textbf{(\arabic{*})} ]
\item The code can be found at the website \texttt{gitlab.ethz.ch}
- \item After logging into (or registering on) this website you can find the repository by searching for its name: \texttt{D-FaLL-System}
+ \item After logging into (or registering on) this website you can find the repository by searching for its name: \texttt{\repositoryname}
\item Alternately, the full web address for the repository is:
\begin{center}
- \large{\texttt{gitlab.ethz.ch/D-FaLL/PandS-System/D-FaLL-System/}}
+ \large{\texttt{\gitwebaddress}}
\end{center}
@@ -75,7 +80,7 @@ A few useful hints, commands, web addresses, and file paths:
\begin{center}
\begin{tabular}{ll}
\large{\texttt{git checkout .}}
- & Removes all changes
+ & Removes all changes (note the dot)
\\
\large{\texttt{git checkout master}}
& Switches to the ``master" branch
@@ -92,7 +97,7 @@ A few useful hints, commands, web addresses, and file paths:
\large{\texttt{\roslaunchcommand}}
\end{center}
- This will launch the Graphical User Interface (GUI) that is explained on the wiki and allows you to test out the flight performance of your control algorithm on the quad-rotor. This command works because \texttt{d\_fall\_pps} is defined as the absolute path to where the \texttt{Agent.launch} file is located.
+ This will launch the Graphical User Interface (GUI) that is explained on the wiki and allows you to test out the flight performance of your control algorithm on the quad-rotor. This command works because \texttt{\rospackagename} is defined as the absolute path to where the \texttt{\launchfilename} file is located.
\end{enumerate}
\clearpage
@@ -104,9 +109,7 @@ A few useful hints, commands, web addresses, and file paths:
\begin{enumerate}[topsep=-1pt , itemsep=1pt , label = \textbf{(\arabic{*})} ]
\item Close the GUI window,
\item Kill the GUI process, (this is achieved by pressing the \texttt{Ctrl+c} in the terminal window from which \texttt{\roslaunchcommand} was run),
- \item Re-compile all of the code. To do this, in a Terminal window first change to the \texttt{pps\_ws} folder of the repository (where \texttt{ws} stands for workspace), and then use the \texttt{catkin\_make} command, i.e.:
-
- \texttt{catkin\_make}
+ \item Re-compile all of the code. To do this, in a Terminal window first change to the \texttt{\workspacefoldername} folder of the repository (where \texttt{ws} stands for workspace), and then use the \texttt{catkin\_make} command, i.e.:
\begin{center}
\begin{tabular}{ll}
@@ -118,7 +121,7 @@ A few useful hints, commands, web addresses, and file paths:
\end{tabular}
\end{center}
- This will compile all the code, or throw an error if your changes have introduced errors that prevent the compilation from completing successfully.
+ This will compile all the code, or throw an error if the compilation is unsuccessfully.
\item Relaunch the GUI, (by running \texttt{\roslaunchcommand} from a Terminal window).
\end{enumerate}
@@ -126,11 +129,11 @@ A few useful hints, commands, web addresses, and file paths:
\vspace{0.3cm}
\noindent
- Steps~\textbf{(1)} and \textbf{(2)} are required because the GUI and your controller is loaded into RAM when the system is launched, and so your newly compile changes in Step~\textbf{(3)} only take effect when the system is launched again in Step~\textbf{(4)}.
+ Steps~\textbf{(1)} and \textbf{(2)} are required because the GUI and your controller is loaded into RAM when the system is launched, and so your newly compiled changes in Step~\textbf{(3)} only take effect when the system is launched again in Step~\textbf{(4)}.
\vspace{0.3cm}
\noindent
- Note also that the GUI will launch in Step~\textbf{(4)} even if there were compiler errors in Step~\textbf{(3)}, but it is launching the previously compiled version and hence any changes you made will not be running.
+ Note also that the GUI will launch in Step~\textbf{(4)} even if there were compiler errors in Step~\textbf{(3)}, but it is launching the previously compiled version and hence any changes you made will \textbf{not} be running.
\end{document}
\ No newline at end of file
diff --git a/wiki/latex/dfall_system_student_workflow_cheat_sheet.tex b/wiki/latex/dfall_system_student_workflow_cheat_sheet.tex
new file mode 100644
index 0000000000000000000000000000000000000000..276f128150febb5ef7953fa8373cca054f00504f
--- /dev/null
+++ b/wiki/latex/dfall_system_student_workflow_cheat_sheet.tex
@@ -0,0 +1,147 @@
+\documentclass[]{report}
+
+
+% ----------------------------------------------------- %
+% PACKAGES REQUIRED
+\usepackage{enumitem}
+%\usepackage{amsmath}
+%\usepackage{amssymb}
+
+
+
+% ----------------------------------------------------- %
+% SET THE GEOMETRY OF THE PAGE
+\usepackage{geometry}
+\setlength{\paperwidth}{148mm}
+\setlength{\paperheight}{210mm}
+\setlength{\hoffset}{-20mm} % -1in subtract latex default margin (1 inch) from hoffset
+\setlength{\textwidth}{120mm} % Width of the Body Box = 210mm - 50mm = 160mm
+
+\setlength{\textheight}{180mm} % Height of the Body Box = 297mm - 50mm = 247mm
+\setlength{\voffset}{-15mm} % -1in subtract latex default margin (1 inch) from voffset
+
+
+% ----------------------------------------------------- %
+% SET THE PAGE STYLE
+\usepackage{fancyhdr}
+\fancyhf{}
+\rhead{\textsc{page} \thepage}
+\pagestyle{fancy}
+
+
+% ----------------------------------------------------- %
+% REMOVE THE PARAGRAPH INDENT
+
+
+
+% ----------------------------------------------------- %
+% THINGS THAT MAY CHANGES AS THE SYSTEM IS DEVELOPED
+\newcommand{\cppfile}{StudentControllerService.cpp}
+\newcommand{\headerfile}{StudentControllerService.h}
+\newcommand{\yamlfile}{StudentController.yaml}
+\newcommand{\computecontroloutputfunction}{calculateControlOutput}
+\newcommand{\convertintobodyframefunction}{convertIntoBodyFrame}
+\newcommand{\convertnewtonsintocommandfunction}{computeMotorPolyBackward}
+\newcommand{\loadyamlfunction}{fetchStudentControllerYamlParameters}
+
+
+% ----------------------------------------------------- %
+% BEGIN THE DOCUMENT
+\begin{document}
+
+ \begin{center}
+ \huge{\textsc{Student Workflow Hints}}
+ \end{center}
+
+ \begin{center}
+ \textbf{Emergency stop button: the space-bar can be pressed at anytime to trigger the motors-off command}
+ \end{center}
+
+
+ \begin{enumerate}[topsep=-1pt , itemsep=1pt , label = \textbf{(\arabic{*})} ]
+ \item Your controller is implemented using the following 3 files:
+
+ \begin{center}
+ \begin{tabular}{l}
+ \large{\texttt{\cppfile}}
+ \\
+ \large{\texttt{\headerfile}}
+ \\
+ \large{\texttt{\yamlfile}}
+ \end{tabular}
+ \end{center}
+
+ The \texttt{.cpp} file is where you implement your controller. The \texttt{.h} file is where you can define new variables and function (these can also be added at the top of the \texttt{.cpp} file). The \texttt{.yaml} file is where you add parameters that can be changed during flight.
+
+ \item In the \texttt{\cppfile}, locate the function named:
+ \begin{center}
+ \large{\texttt{\computecontroloutputfunction}}
+ \end{center}
+ This is the function where you implement your controller. When your controller is running, this function is called at the frequency of the motion capture system, normally set to 200Hz. Every time your function is called, it is provided with the most recent position and attitude measurement, and is expected to return the control action to be sent to your Crazyflie.
+
+ \item The position and yaw error are already computed in the first lines of the \texttt{\computecontroloutputfunction} function. You should check whether the error is computed as ``setpoint minus measurement" or the opposite.
+
+ \item A function for rotating the inertial frame position errors into body frame errors is already defined and named:
+ \begin{center}
+ \large{\texttt{\convertintobodyframefunction}}
+ \end{center}
+ however the conversion is not implemented. You should edit this function to implement the conversion from inertial frame into body frame.
+
+ \item A function for converting a thrust in Newtons to a 16-bit command is already defined and named:
+ \begin{center}
+ \large{\texttt{\convertnewtonsintocommandfunction}}
+ \end{center}
+ The conversion is correctly implemented, however, if the result is greater than ${(2^{16}\!-\!1)}$, then this is wrapped back around when the command is received by the Crazyflie. You should edit this function to limit commands to a maximum of \texttt{60000}, and set commands below \texttt{2000} to be \texttt{0}.
+
+ \end{enumerate}
+
+ \clearpage
+
+ \begin{center}
+ \textbf{The following steps expliain how to add a \texttt{yaml} parameter}
+ \end{center}
+
+ \begin{enumerate}[topsep=-1pt , itemsep=1pt , label = \textbf{(\arabic{*})} ]
+ \item In the file \texttt{\yamlfile} add the following line of code:
+ \begin{center}
+ \large{\texttt{example\_parameter : 10}}
+ \end{center}
+ Where the part before the \texttt{:} is the string that names the parameter, and the part after the \texttt{:} is the value of the parameter.
+
+ \item In the file \texttt{\headerfile} add the following line of code:
+ \begin{center}
+ \large{\texttt{float yaml\_example\_parameter = 1.0;}}
+ \end{center}
+ This defines a class variable named \texttt{yaml\_example\_parameter} which is where the value from the \texttt{.yaml} file will be stored when it is loaded.
+ \textbf{Note:} the variable can be given any name, we use the convention of a \texttt{yaml\_} prefix for variables that store values loaded from a yaml file.
+
+ \textbf{Note:} it is important to the initialise the variable with an appropriate value, in this case \texttt{1.0}. This is because the variable \texttt{yaml\_example\_parameter} may be used during execution of your code before it is loaded for the first time, and so the default initialisation value of zero will be problematic if you divide by this variable anywhere.
+
+ \item The loading of values from the \texttt{.yaml} file is performed in the \texttt{.cpp} file. Hence locate in the file \texttt{\cppfile} the function named:
+ \begin{center}
+ \large{\texttt{\loadyamlfunction}}
+ \end{center}
+ Add the following line of code to that function:
+ \begin{flushleft}
+ \large{\texttt{yaml\_example\_parameter =}}
+ \\
+ \hspace{0.8cm}\large{\texttt{getParameterFloat(nodeHandle\_for\_paramaters,}}
+ \\
+ \hspace{0.8cm}\large{\texttt{"example\_parameter");}}
+ \end{flushleft}
+ The function \texttt{getParameterFloat} that is called by this line searches the appropriate \texttt{.yaml} file for the string \texttt{"example\_parameter"} and returns the respective value found in the \texttt{.yaml} file. Other functions available for loading different types of parameters are:
+ \begin{center}
+ \begin{tabular}{l}
+ \large{\texttt{getParameterInt}}
+ \\
+ \large{\texttt{getParameterBool}}
+ \\
+ \large{\texttt{getParameterFloatVector}}
+ \end{tabular}
+ \end{center}
+ The syntax for the first two functions is identical to \texttt{getParameterFloat}, while the syntax for the third function differs slightly and an example can be found in the \texttt{.cpp} file.
+
+ \end{enumerate}
+
+
+\end{document}
\ No newline at end of file
diff --git a/wiki/workflow_for_students.md b/wiki/workflow_for_students.md
index b1cf0b1a0d61752d866c1c116b6c63259109dfba..f0156d4495916eaba935df8bd7608cb8331ac923 100644
--- a/wiki/workflow_for_students.md
+++ b/wiki/workflow_for_students.md
@@ -15,64 +15,51 @@ gyrosensor needs to initialize.
2. Checkout master branch of the repository and pull:<br />
``git checkout master``<br />
``git pull origin master``<br />
- *Note: to do this step, you will be asked a username and a password. Use
- the same credentials you use for your ETH account. Also, make sure you
- have an active account in gitlab: [https://gitlab.ethz.ch/](https://gitlab.ethz.ch/)*<br />
3. Compile the source code running `catkin_make`
---
### Start the student's GUI
- * Once all the prerequisites have been fulfilled, we can start the student's
- GUI by going to a terminal and typing:
- `roslaunch dfall_pkg Student.launch`
+ * Once all the prerequisites have been fulfilled, you can start the student's GUI by going to a terminal and typing:
+ `roslaunch dfall_pkg agent.launch`
- *Note: for this to work, the teacher's computer has to be connected to the
- network and the teacher's GUI has to be started before. Please wait until
- your teacher has already set up everything.*
+ *Note: for this to work, the teacher's computer has to be connected to the network and the `teacher.launch` GUI has to be started before. Please wait until your teacher has already set up everything.*
- * Once started, you will see something like this: <br>
+ * Once started, you will see something like this:<br>
<img src="./pics/student_gui.png" style="width: 800px;"/> <br><br>
- * Connect to/Disconnect from Crazyflie: physically connects/disconnects our computer to
- the assigned crazyflie using the Crazyradio dongle.<br><br>
+ * Connect to/Disconnect from Crazyflie: this connects/disconnects your computer to the assigned Crazyflie using the Crazyradio USB dongle.<br>
* Crazyradio status: can take the values "Connected!", "Disconnected" or
- "Connecting..." <br><br>
- * Below the disconnect button we see two lines of text. The first one
- contains information about our StudentID number, and the Crazyflie we are
- linked to. This is the Crazyflie we must connect to. <br><br>
- Below that, we can also see the Raw voltage line, which contains the instantaneous voltage of the
- battery of the Crazyflie, in Volts.<br><br>
- * In the right part, there are 3 buttons to control the flying state of our
- Crazyflie, and a text label containing the current flying state. **It is
- important to know that we can only take off when we are in the state "Motors
- OFF", and we can only land if we are NOT in the state "Motors OFF"**<br><br>
- * In the middle-bottom part of the GUI there are two tabs: Safe and Student
- controller. <br><br>
- In the left part of these tabs, there is information about the
- current position of the Crazyflie, and also the difference between the current
- position and the current setpoint, useful for keeping track of the error of
- our controller.<br><br>
- In the right part of the tabs, there is information about the current
- setpoint, and boxes to edit the new setpoint. When we press the button "Set
- setpoint", we change the current setpoint with the information filled.<br><br>
- * The button called "Enable <controller> Controller" enables the selected
- controller. The current enabled controller is the one which is highlighted in
- green in the tab name.<br><br>
- * The button "Load <filename> YAML file" loads and refreshes the parameters
- that are in the corresponding YAML file.<br><br>
-
-
- * You can now play with the landing, take off and change of setpoint using the
- safe controller to get familiar with the system.
-
- *Note: there are different parameters in the file called
- `SafeController.yaml`, in the folder param (use `roscd dfall_pkg/param` in a
- terminal to go there).* **These are the safe controller parameters and should NOT be
- changed.** *Take a look at the file and get familiar with the format used,
- since may have to write your own for the student controller.*
+ "Connecting...", and the icon changes to display the current status.<br>
+ * The title at the top contains information about your AgentID number, and the Crazyflie that is allocated to you. This is the only Crazyflie you can connect to.<br><br>
+
+ * The voltage displays the instantaneous voltage of the battery of the Crazyflie, in Volts.<br>
+ * The battery icon displays this relative to the voltages for full and empty.<br>
+ * Because flying requires a high current draw from the battery, the full and empty voltages levels automatically adjust depending on whether your Crazyflie is flying or has the motors turned off.<br>
+ * If the battery voltage falls below the empty level, then a low battery warning is raise and your Crazyflie will be forced to land.<br>
+ * You must change the battery if the low battery warning occurs because continuing to drain the battery further will cause permanent damage to the battery.<br><br>
+
+ * There are 3 buttons to control the flying state of your Crazyflie: **"Take-off", "Land", and "Motors-OFF"**<br>
+ * The icon displays the current flying state. **It is important to know that you can only "Take-off" when the state is "Motors
+ OFF", and you can only "Land" if the state is NOT "Motors OFF".**<br>
+ * **IMPORTANT: YOU CAN PRESS SPACE-BAR AT ANYTIME AS A SHORT-CUT TO THE "MOTORS-OFF" BUTTON.**<br><br>
+
+ * In the lower part of the GUI there are two tabs: **"Default" and "Student" controller**<br>
+ * These tabs allow you to interact with the respective controller.<br>
+ * In these tabs, the first column of numbers provides information about the current position of the Crazyflie, the second column of numbers is computed as the difference between the current position and the current setpoint, and the third column of number is the current setpoint. This information is useful for keeping track of the error of your controller.<br>
+ * The next column of boxes can be edited and allows you to type in a new setpoint. When you press the button "Set
+ setpoint" (or the enter key on the keyboard), we change the current setpoint with the information filled.<br><br>
+
+ * The button called "Enable <controller> Controller" enables the selected controller. The current enabled controller is the one which is highlighted in green in the tab name.<br>
+ * The button "Load <filename> YAML file" loads and refreshes the parameters that are in the corresponding YAML file.<br><br>
+
+
+ * You can now play with the landing, take off and change of setpoint using the default controller to get familiar with the system.<br>
+
+ *Note: there are different parameters in the file called `DefaultController.yaml`, in the folder param (use `roscd dfall_pkg/param` in a terminal to go there).* **These are the paramteres of the default controller parameters and should NOT be changed.** *Take a look at the file and get familiar with the format used, since may have to write your own for the student controller.*
+
### Creating your own controller!
In this part, we will learn how to design our own controller and actually
@@ -130,19 +117,6 @@ important files that should be taken into account.
-<!-- ##### -- Useful files: -->
-<!-- in `dfall_ws/src/dfall_pkg/scripts` -->
-<!-- -\-> call scripts in terminal by going to the above path and then typing -->
-<!-- ./SCRIPTNAME, e.g.: `./enable_crazyflie` -->
-
-<!-- * *disable_crazyflie* -->
-<!-- * *enable_crazyflie* -->
-<!-- * *load_custom_controller* -->
-<!-- * *load_safe_controller* -->
-<!-- * *safe_controller_setpoint* <br> -->
-<!-- This one needs 4 parameters for x,y,z and yaw. The setpoint of the crazyflie is then set to those values. -->
-
-
<!-- ##### -- Files to look at: -->
<!-- in `dfall_ws/src/dfall_pkg/param` -->
<!-- * _SafeController.yaml_ <br> -->
@@ -233,32 +207,4 @@ to it*
list of all the topics available when you start typing in the field "Topic". <br><br>
5. Start the Student node following the steps mentioned before (`roslaunch dfall_pkg Student.launch`) and enable the Student Controller.<br><br>
6. Once we are using the Student Controller, we will be seeing how the data
- selected gets plotted in the rqt plots.
-
-<!-- --- -->
-
-
-<!-- ## Workflow: -->
-<!-- **Setup** -->
-<!-- 1. Teacher must run his part, that publishes ViconData for students and hosts the roscore. -->
-<!-- 2. Each student/group has a CrazyFlie and a laptop. -->
-<!-- 3. Use `roscd dfall_pkg/launch` in a terminal as well as `roscd dfall_pkg/scripts` in another terminal -->
-
-<!-- <br> -->
-<!-- **Working** -->
-<!-- 1. Adjust your custom controller -->
-<!-- 2. Use `catkin_make` in the dfall_ws directory to compile your controller implementation -->
-<!-- 3. Start your crazyflie -->
-<!-- 4. Launch the correct file in the launch directory as described above. ClientConfig.yaml has to be correct. -->
-<!-- 5. Use the scripts to change from the safe to your custom controller. -->
-<!-- 6. When your done, you can turn of your crazyflie by using the script `disable_crazyflie`. -->
-<!-- 7. Repeat -->
-
-
-<!-- --- -->
-<!-- **Troubleshooting** -->
-<!-- - _SafeController is not working_ <br> -->
-<!-- Was the antenna of the crazyflie facing in the *opposite* direction of the defined Vicon x-axis? -\-> Define it again! <br> -->
-<!-- The crazyflie has to lie on the table when you turn it on because the gyro sensor is initialized upon start-up. <br> -->
-<!-- Is the crazyflie still properly showing in the ViconTracker software? -\-> Define it again and check that the markers don't move! -->
-<!-- - If you have added a new controller. Don't forget to adjust the CMakeList.txt file and use catkin_make again. -->
+ selected gets plotted in the rqt plots.
\ No newline at end of file